CMyntEyeCamera.cpp

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */

#include "hwdrivers-precomp.h"  // Precompiled headers

#include <mrpt/3rdparty/do_opencv_includes.h>
#include <mrpt/hwdrivers/CMyntEyeCamera.h>
#include <iostream>
#include <thread>

#if MRPT_HAS_MYNTEYE_D
#include <mynteyed/camera.h>
#include <mynteyed/utils.h>
#endif

using namespace mrpt;
using namespace mrpt::hwdrivers;

struct CMyntEyeCamera::Impl
{
#if MRPT_HAS_MYNTEYE_D
    std::shared_ptr<mynteyed::Camera> cam =
        std::make_shared<mynteyed::Camera>();
    mynteyed::DeviceInfo dev_info;
#endif
};

#if MRPT_HAS_MYNTEYE_D
static mrpt::img::TCamera convertIntrinsics(const mynteyed::CameraIntrinsics& i)
{
    mrpt::img::TCamera p;
    p.ncols = i.width;
    p.nrows = i.height;

    p.cx(i.cx);
    p.cy(i.cy);
    p.fx(i.fx);
    p.fy(i.fy);
    // The distortion coefficients: k1,k2,p1,p2,k3
    for (int k = 0; k < 5; k++) p.dist[k] = i.coeffs[k];

    return p;
}
#endif

CMyntEyeCamera::CMyntEyeCamera(const TMyntEyeCameraParameters& p)
    : m_capture(mrpt::make_impl<CMyntEyeCamera::Impl>())
{
    MRPT_START
#if MRPT_HAS_MYNTEYE_D
    if (!mynteyed::util::select(*m_capture->cam, &m_capture->dev_info))
    {
        THROW_EXCEPTION("No MYNTEYE-D cameras was found!");
    }
    mynteyed::util::print_stream_infos(
        *m_capture->cam, m_capture->dev_info.index);

    mynteyed::OpenParams params(m_capture->dev_info.index);
    params.color_mode = mynteyed::ColorMode::COLOR_RECTIFIED;
    params.stream_mode = mynteyed::StreamMode::STREAM_1280x720;
    params.ir_intensity = p.ir_intensity;

    mynteyed::StreamMode stream_mode = params.stream_mode;
    m_capture->cam->Open(params);
    std::cout << std::endl;
    if (!m_capture->cam->IsOpened())
    {
        THROW_EXCEPTION("Error: Open camera failed");
    }
    std::cout << "[CMyntEyeCamera] Open device successful.\n";

    mynteyed::StreamIntrinsics si =
        m_capture->cam->GetStreamIntrinsics(stream_mode);

    m_intrinsics_left = convertIntrinsics(si.left);
    m_intrinsics_right = convertIntrinsics(si.right);

    std::cout << "LEFT camera intrinsics:\n"
              << m_intrinsics_left.dumpAsText() << "\n";
    std::cout << "RIGHT camera intrinsics:\n"
              << m_intrinsics_right.dumpAsText() << "\n";

    std::cout << "Waiting for streams...\n";
    m_capture->cam->WaitForStreams();
    std::cout << "Streams started OK.\n";

    m_bInitialized = true;

#else
    THROW_EXCEPTION("MRPT was built without MYNTEYE-D SDK");
#endif
    MRPT_END
}

CMyntEyeCamera::~CMyntEyeCamera() {}

bool CMyntEyeCamera::getObservation(mrpt::obs::CObservation3DRangeScan& out)
{
#if MRPT_HAS_MYNTEYE_D && MRPT_HAS_OPENCV
    ASSERT_(m_bInitialized);

    mynteyed::StreamData image_color, image_depth;

    for (int nRetries = 0; nRetries < 100; nRetries++)
    {
        if (!image_color.img)
            image_color = m_capture->cam->GetStreamData(
                mynteyed::ImageType::IMAGE_LEFT_COLOR);

        if (!image_depth.img)
            image_depth =
                m_capture->cam->GetStreamData(mynteyed::ImageType::IMAGE_DEPTH);

        if (image_color.img && image_depth.img)
            break;
        else
        {
            std::this_thread::sleep_for(std::chrono::milliseconds(1));
        }
    }

    // Empty output obs:
    out = mrpt::obs::CObservation3DRangeScan();

    if (!image_color.img && !image_depth.img) return false;

    // all is good:
    out.timestamp = mrpt::Clock::now();

    // RGB= 3xu8
    if (image_color.img)
    {
        auto i = image_color.img->To(mynteyed::ImageFormat::COLOR_BGR);

        ASSERT_EQUAL_(
            i->data_size(), static_cast<size_t>(i->width() * i->height() * 3));

        cv::Mat m(i->height(), i->width(), CV_8UC3, i->data());

        out.hasIntensityImage = true;
        out.cameraParamsIntensity = m_intrinsics_left;
        out.intensityImage = mrpt::img::CImage(m, mrpt::img::DEEP_COPY);
    }

    // Depth= u16
    if (image_depth.img)
    {
        auto i = image_depth.img->To(mynteyed::ImageFormat::DEPTH_RAW);

        const auto h = i->height(), w = i->width();

        out.hasRangeImage = true;
        out.cameraParams = m_intrinsics_left;

        out.range_is_depth = true;
        // This method will try to exploit memory pooling if possible:
        out.rangeImage_setSize(h, w);
        out.rangeUnits = 1e-3f;  // we use mm as units

        ASSERT_EQUAL_(
            i->data_size(), static_cast<size_t>(i->width() * i->height() * 2));
        cv::Mat m(i->height(), i->width(), CV_16UC1, i->data());

        for (int r = 0; r < h; r++)
        {
            for (int c = 0; c < w; c++)
            {
                const uint16_t v = m.at<uint16_t>(r, c);
                out.rangeImage.coeffRef(r, c) = v;
            }
        }
    }

    return true;
#else
    THROW_EXCEPTION("MRPT was built without MYNTEYE-D SDK or OpenCV");
#endif
}

void TMyntEyeCameraParameters::loadFromConfigFile(
    const mrpt::config::CConfigFileBase& c, const std::string& s)
{
    MRPT_LOAD_CONFIG_VAR_CS(ir_intensity, int);
}