/* +------------------------------------------------------------------------+ | Mobile Robot Programming Toolkit (MRPT) | | https://www.mrpt.org/ | | | | Copyright (c) 2005-2026, 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 #include #include #include #include #if MRPT_HAS_MYNTEYE_D #include #include #endif using namespace mrpt; using namespace mrpt::hwdrivers; struct CMyntEyeCamera::Impl { #if MRPT_HAS_MYNTEYE_D std::shared_ptr cam = std::make_shared(); 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()) { 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(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(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(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); }