Debian Codegraph @

libs/hwdrivers/src/CMyntEyeCamera.cpp

raw
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 }