20 #include <mrpt/3rdparty/do_opencv_includes.h>
40 : m_sensorPoseOnRobot(),
42 m_relativePoseIntensityWRTDepth(0, 0, 0, -90.0_deg, 0.0_deg, -90.0_deg)
46 m_sensorLabel =
"OPENNI2";
50 m_cameraParamsRGB.ncols = 0;
51 m_cameraParamsRGB.nrows = 0;
53 m_cameraParamsRGB.cx(-1);
54 m_cameraParamsRGB.cy(-1);
55 m_cameraParamsRGB.fx(-1);
56 m_cameraParamsRGB.fy(-1);
58 m_cameraParamsRGB.dist.fill(0);
61 m_cameraParamsDepth.ncols = 0;
62 m_cameraParamsDepth.nrows = 0;
64 m_cameraParamsDepth.cx(-1);
65 m_cameraParamsDepth.cy(-1);
66 m_cameraParamsDepth.fx(-1);
67 m_cameraParamsDepth.fy(-1);
69 m_cameraParamsDepth.dist.fill(0);
78 close(m_user_device_number);
79 #endif // MRPT_HAS_OPENNI2
92 if (getConnectedDevices() <= 0)
98 if (m_serial_number != 0)
100 openDeviceBySerial(m_serial_number);
101 if (getDeviceIDFromSerialNum(
102 m_serial_number, m_user_device_number) ==
false)
105 "Failed to find sensor_id from serial number(%d).",
110 open(m_user_device_number);
112 if (isOpen(m_user_device_number) ==
false)
115 "Failed to open OpenNI2 device(%d).", m_user_device_number));
121 if (getDepthSensorParam(
122 m_cameraParamsDepth, m_user_device_number) ==
false)
129 if (getColorSensorParam(m_cameraParamsRGB, m_user_device_number) ==
136 catch (std::logic_error& e)
142 #endif // MRPT_HAS_OPENNI2
154 bool thereIs, hwError;
157 std::make_shared<CObservation3DRangeScan>();
159 assert(getNumDevices() > 0);
160 getNextObservation(*newObs, thereIs, hwError);
172 std::vector<mrpt::serialization::CSerializable::Ptr> objs;
173 if (m_grab_image || m_grab_depth || m_grab_3D_points)
174 objs.push_back(newObs);
176 appendObservations(objs);
180 #endif // MRPT_HAS_OPENNI2
191 const std::string& iniSection)
193 cout <<
"COpenNI2Sensor::loadConfig_sensorSpecific...\n";
195 m_sensorPoseOnRobot.setFromValues(
196 configSource.
read_float(iniSection,
"pose_x", 0),
197 configSource.
read_float(iniSection,
"pose_y", 0),
198 configSource.
read_float(iniSection,
"pose_z", 0),
204 configSource.
read_bool(iniSection,
"preview_window", m_preview_window);
206 m_width = configSource.
read_int(iniSection,
"width", 0);
207 m_height = configSource.
read_int(iniSection,
"height", 0);
208 m_fps = configSource.
read_float(iniSection,
"fps", 0);
209 std::cout <<
"width " << m_width <<
" height " << m_height <<
" fps "
212 bool hasRightCameraSection =
214 bool hasLeftCameraSection =
216 bool hasLeft2RightPose =
217 configSource.
sectionExists(iniSection +
string(
"_LEFT2RIGHT_POSE"));
225 catch (
const std::exception& e)
227 std::cout <<
"[COpenNI2Sensor::loadConfig_sensorSpecific] Warning: "
228 "Ignoring error loading calibration parameters:\n"
231 if (hasRightCameraSection)
235 if (hasLeftCameraSection)
239 if (hasLeft2RightPose)
242 0, 0, 0, -90.0_deg, 0.0_deg, -90.0_deg);
243 m_relativePoseIntensityWRTDepth =
249 m_user_device_number = configSource.
read_int(
250 iniSection,
"device_number", m_user_device_number);
253 configSource.
read_int(iniSection,
"serial_number", m_serial_number);
256 configSource.
read_bool(iniSection,
"grab_image", m_grab_image);
258 configSource.
read_bool(iniSection,
"grab_depth", m_grab_depth);
260 configSource.
read_bool(iniSection,
"grab_3D_points", m_grab_3D_points);
264 iniSection,
"relativePoseIntensityWRTDepth",
"");
265 if (!s.empty()) m_relativePoseIntensityWRTDepth.fromString(s);
279 [[maybe_unused]]
bool& there_is_obs, [[maybe_unused]]
bool& hardware_error)
286 out_obs, there_is_obs, hardware_error, m_user_device_number);
290 out_obs.sensorLabel = m_sensorLabel;
291 out_obs.sensorPose = m_sensorPoseOnRobot;
292 out_obs.relativePoseIntensityWRTDepth = m_relativePoseIntensityWRTDepth;
294 out_obs.cameraParams = m_cameraParamsDepth;
295 out_obs.cameraParamsIntensity = m_cameraParamsRGB;
298 if (out_obs.hasRangeImage && m_grab_3D_points)
300 out_obs.unprojectInto(out_obs);
304 out_obs.hasRangeImage =
false;
305 out_obs.rangeImage.resize(0, 0);
310 if (m_preview_window)
312 if (out_obs.hasRangeImage)
314 if (++m_preview_decim_counter_range > m_preview_window_decimation)
316 m_preview_decim_counter_range = 0;
321 m_win_range->setPos(5, 5);
324 m_win_range->showImage(out_obs.rangeImage_getAsImage());
327 if (out_obs.hasIntensityImage)
329 if (++m_preview_decim_counter_rgb > m_preview_window_decimation)
331 m_preview_decim_counter_rgb = 0;
336 m_win_int->setPos(300, 5);
338 m_win_int->showImage(out_obs.intensityImage);
344 if (m_win_range) m_win_range.reset();
345 if (m_win_int) m_win_int.reset();
351 #endif // MRPT_HAS_OPENNI2
358 [maybe_unused]]
const std::string& directory)