| 1 |
/* +------------------------------------------------------------------------+ |
| 2 |
| Mobile Robot Programming Toolkit (MRPT) | |
| 3 |
| https://www.mrpt.org/ | |
| 4 |
| | |
| 5 |
| Copyright (c) 2005-2026, Individual contributors, see AUTHORS file | |
| 6 |
| See: https://www.mrpt.org/Authors - All rights reserved. | |
| 7 |
| Released under BSD License. See: https://www.mrpt.org/License | |
| 8 |
+------------------------------------------------------------------------+ */ |
| 9 |
|
| 10 |
#include "hwdrivers-precomp.h" // Precompiled headers |
| 11 |
// |
| 12 |
#include <mrpt/3rdparty/do_opencv_includes.h> |
| 13 |
#include <mrpt/hwdrivers/CMyntEyeCamera.h> |
| 14 |
|
| 15 |
#include <chrono> |
| 16 |
#include <iostream> |
| 17 |
#include <thread> |
| 18 |
|
| 19 |
#if MRPT_HAS_MYNTEYE_D |
| 20 |
#include <mynteyed/camera.h> |
| 21 |
#include <mynteyed/utils.h> |
| 22 |
#endif |
| 23 |
|
| 24 |
using namespace mrpt; |
| 25 |
using namespace mrpt::hwdrivers; |
| 26 |
|
| 27 |
struct CMyntEyeCamera::Impl |
| 28 |
{ |
| 29 |
#if MRPT_HAS_MYNTEYE_D |
| 30 |
std::shared_ptr<mynteyed::Camera> cam = std::make_shared<mynteyed::Camera>(); |
| 31 |
mynteyed::DeviceInfo dev_info; |
| 32 |
#endif |
| 33 |
}; |
| 34 |
|
| 35 |
#if MRPT_HAS_MYNTEYE_D |
| 36 |
static mrpt::img::TCamera convertIntrinsics(const mynteyed::CameraIntrinsics& i) |
| 37 |
{ |
| 38 |
mrpt::img::TCamera p; |
| 39 |
p.ncols = i.width; |
| 40 |
p.nrows = i.height; |
| 41 |
|
| 42 |
p.cx(i.cx); |
| 43 |
p.cy(i.cy); |
| 44 |
p.fx(i.fx); |
| 45 |
p.fy(i.fy); |
| 46 |
// The distortion coefficients: k1,k2,p1,p2,k3 |
| 47 |
for (int k = 0; k < 5; k++) p.dist[k] = i.coeffs[k]; |
| 48 |
|
| 49 |
return p; |
| 50 |
} |
| 51 |
#endif |
| 52 |
|
| 53 |
CMyntEyeCamera::CMyntEyeCamera(const TMyntEyeCameraParameters& p) : |
| 54 |
m_capture(mrpt::make_impl<CMyntEyeCamera::Impl>()) |
| 55 |
{ |
| 56 |
MRPT_START |
| 57 |
#if MRPT_HAS_MYNTEYE_D |
| 58 |
if (!mynteyed::util::select(*m_capture->cam, &m_capture->dev_info)) |
| 59 |
{ |
| 60 |
THROW_EXCEPTION("No MYNTEYE-D cameras was found!"); |
| 61 |
} |
| 62 |
mynteyed::util::print_stream_infos(*m_capture->cam, m_capture->dev_info.index); |
| 63 |
|
| 64 |
mynteyed::OpenParams params(m_capture->dev_info.index); |
| 65 |
params.color_mode = mynteyed::ColorMode::COLOR_RECTIFIED; |
| 66 |
params.stream_mode = mynteyed::StreamMode::STREAM_1280x720; |
| 67 |
params.ir_intensity = p.ir_intensity; |
| 68 |
|
| 69 |
mynteyed::StreamMode stream_mode = params.stream_mode; |
| 70 |
m_capture->cam->Open(params); |
| 71 |
std::cout << std::endl; |
| 72 |
if (!m_capture->cam->IsOpened()) |
| 73 |
{ |
| 74 |
THROW_EXCEPTION("Error: Open camera failed"); |
| 75 |
} |
| 76 |
std::cout << "[CMyntEyeCamera] Open device successful.\n"; |
| 77 |
|
| 78 |
mynteyed::StreamIntrinsics si = m_capture->cam->GetStreamIntrinsics(stream_mode); |
| 79 |
|
| 80 |
m_intrinsics_left = convertIntrinsics(si.left); |
| 81 |
m_intrinsics_right = convertIntrinsics(si.right); |
| 82 |
|
| 83 |
std::cout << "LEFT camera intrinsics:\n" << m_intrinsics_left.dumpAsText() << "\n"; |
| 84 |
std::cout << "RIGHT camera intrinsics:\n" << m_intrinsics_right.dumpAsText() << "\n"; |
| 85 |
|
| 86 |
std::cout << "Waiting for streams...\n"; |
| 87 |
m_capture->cam->WaitForStreams(); |
| 88 |
std::cout << "Streams started OK.\n"; |
| 89 |
|
| 90 |
m_bInitialized = true; |
| 91 |
|
| 92 |
#else |
| 93 |
THROW_EXCEPTION("MRPT was built without MYNTEYE-D SDK"); |
| 94 |
#endif |
| 95 |
MRPT_END |
| 96 |
} |
| 97 |
|
| 98 |
CMyntEyeCamera::~CMyntEyeCamera() {} |
| 99 |
|
| 100 |
bool CMyntEyeCamera::getObservation(mrpt::obs::CObservation3DRangeScan& out) |
| 101 |
{ |
| 102 |
#if MRPT_HAS_MYNTEYE_D && MRPT_HAS_OPENCV |
| 103 |
ASSERT_(m_bInitialized); |
| 104 |
|
| 105 |
mynteyed::StreamData image_color, image_depth; |
| 106 |
|
| 107 |
for (int nRetries = 0; nRetries < 100; nRetries++) |
| 108 |
{ |
| 109 |
if (!image_color.img) |
| 110 |
image_color = m_capture->cam->GetStreamData(mynteyed::ImageType::IMAGE_LEFT_COLOR); |
| 111 |
|
| 112 |
if (!image_depth.img) |
| 113 |
image_depth = m_capture->cam->GetStreamData(mynteyed::ImageType::IMAGE_DEPTH); |
| 114 |
|
| 115 |
if (image_color.img && image_depth.img) |
| 116 |
break; |
| 117 |
else |
| 118 |
{ |
| 119 |
std::this_thread::sleep_for(std::chrono::milliseconds(1)); |
| 120 |
} |
| 121 |
} |
| 122 |
|
| 123 |
// Empty output obs: |
| 124 |
out = mrpt::obs::CObservation3DRangeScan(); |
| 125 |
|
| 126 |
if (!image_color.img && !image_depth.img) return false; |
| 127 |
|
| 128 |
// all is good: |
| 129 |
out.timestamp = mrpt::Clock::now(); |
| 130 |
|
| 131 |
// RGB= 3xu8 |
| 132 |
if (image_color.img) |
| 133 |
{ |
| 134 |
auto i = image_color.img->To(mynteyed::ImageFormat::COLOR_BGR); |
| 135 |
|
| 136 |
ASSERT_EQUAL_(i->data_size(), static_cast<size_t>(i->width() * i->height() * 3)); |
| 137 |
|
| 138 |
cv::Mat m(i->height(), i->width(), CV_8UC3, i->data()); |
| 139 |
|
| 140 |
out.hasIntensityImage = true; |
| 141 |
out.cameraParamsIntensity = m_intrinsics_left; |
| 142 |
out.intensityImage = mrpt::img::CImage(m, mrpt::img::DEEP_COPY); |
| 143 |
} |
| 144 |
|
| 145 |
// Depth= u16 |
| 146 |
if (image_depth.img) |
| 147 |
{ |
| 148 |
auto i = image_depth.img->To(mynteyed::ImageFormat::DEPTH_RAW); |
| 149 |
|
| 150 |
const auto h = i->height(), w = i->width(); |
| 151 |
|
| 152 |
out.hasRangeImage = true; |
| 153 |
out.cameraParams = m_intrinsics_left; |
| 154 |
|
| 155 |
out.range_is_depth = true; |
| 156 |
// This method will try to exploit memory pooling if possible: |
| 157 |
out.rangeImage_setSize(h, w); |
| 158 |
out.rangeUnits = 1e-3f; // we use mm as units |
| 159 |
|
| 160 |
ASSERT_EQUAL_(i->data_size(), static_cast<size_t>(i->width() * i->height() * 2)); |
| 161 |
cv::Mat m(i->height(), i->width(), CV_16UC1, i->data()); |
| 162 |
|
| 163 |
for (int r = 0; r < h; r++) |
| 164 |
{ |
| 165 |
for (int c = 0; c < w; c++) |
| 166 |
{ |
| 167 |
const uint16_t v = m.at<uint16_t>(r, c); |
| 168 |
out.rangeImage.coeffRef(r, c) = v; |
| 169 |
} |
| 170 |
} |
| 171 |
} |
| 172 |
|
| 173 |
return true; |
| 174 |
#else |
| 175 |
THROW_EXCEPTION("MRPT was built without MYNTEYE-D SDK or OpenCV"); |
| 176 |
#endif |
| 177 |
} |
| 178 |
|
| 179 |
void TMyntEyeCameraParameters::loadFromConfigFile( |
| 180 |
const mrpt::config::CConfigFileBase& c, const std::string& s) |
| 181 |
{ |
| 182 |
MRPT_LOAD_CONFIG_VAR_CS(ir_intensity, int); |
| 183 |
} |