Debian Codegraph @

libs/hwdrivers/src/CKinect.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/hwdrivers/CKinect.h>
13 #include <mrpt/img/TStereoCamera.h>
14 #include <mrpt/poses/CPose3DQuat.h>
15 #include <mrpt/system/CTimeLogger.h>
16
17 // Universal include for all versions of OpenCV
18 #include <mrpt/3rdparty/do_opencv_includes.h>
19
20 using namespace mrpt::hwdrivers;
21 using namespace mrpt::system;
22 using namespace mrpt::serialization;
23 using namespace mrpt::math;
24 using namespace mrpt::obs;
25 using namespace mrpt::poses;
26 using namespace std;
27
28 IMPLEMENTS_GENERIC_SENSOR(CKinect, mrpt::hwdrivers)
29
30 // Whether to profile memory allocations:
31 //#define KINECT_PROFILE_MEM_ALLOC
32
33 #if MRPT_HAS_KINECT_FREENECT
34 #if defined(__APPLE__) && MRPT_HAS_KINECT_FREENECT_SYSTEM
35 #include <libfreenect/libfreenect.h>
36 #else
37 #include <libfreenect.h>
38 #endif
39 #else
40 #define KINECT_W 640
41 #define KINECT_H 480
42 #endif
43
44 #if MRPT_HAS_KINECT_FREENECT
45 // Macros to convert the opaque pointers in the class header:
46 #define f_ctx reinterpret_cast<freenect_context*>(m_f_ctx)
47 #define f_ctx_ptr reinterpret_cast<freenect_context**>(&m_f_ctx)
48 #define f_dev reinterpret_cast<freenect_device*>(m_f_dev)
49 #define f_dev_ptr reinterpret_cast<freenect_device**>(&m_f_dev)
50 #endif // MRPT_HAS_KINECT_FREENECT
51
52 #ifdef KINECT_PROFILE_MEM_ALLOC
53 mrpt::system::CTimeLogger alloc_tim;
54 #endif
55 // int int a;
56
57 void CKinect::calculate_range2meters()
58 {
59 #ifdef MRPT_KINECT_DEPTH_10BIT
60 const float k1 = 1.1863f;
61 const float k2 = 2842.5f;
62 const float k3 = 0.1236f;
63
64 for (size_t i = 0; i < KINECT_RANGES_TABLE_LEN; i++)
65 m_range2meters[i] = static_cast<uint16_t>(k3 * tanf(i / k2 + k1) / 1e-3f);
66
67 #else
68 for (size_t i = 0; i < KINECT_RANGES_TABLE_LEN; i++)
69 m_range2meters[i] = static_cast<uint16_t>(1e+3f / (i * (-0.0030711016) + 3.3309495161));
70 #endif
71
72 // Minimum/Maximum range means error:
73 m_range2meters[0] = 0;
74 m_range2meters[KINECT_RANGES_TABLE_LEN - 1] = 0;
75 }
76
77 /*-------------------------------------------------------------
78 ctor
79 -------------------------------------------------------------*/
80 CKinect::CKinect() :
81 m_sensorPoseOnRobot(),
82 m_relativePoseIntensityWRTDepth(0, -0.02, 0, -90.0_deg, 0.0_deg, -90.0_deg)
83 {
84 calculate_range2meters();
85
86 // Get maximum range:
87 // Recall: r[Max-1] means error.
88 m_maxRange = m_range2meters[KINECT_RANGES_TABLE_LEN - 2];
89
90 // Default label:
91 m_sensorLabel = "KINECT";
92
93 // =========== Default params ===========
94 // ----- RGB -----
95 m_cameraParamsRGB.ncols = 640; // By default set 640x480, on connect we'll update this.
96 m_cameraParamsRGB.nrows = 480;
97
98 m_cameraParamsRGB.cx(328.94272028759258);
99 m_cameraParamsRGB.cy(267.48068171871557);
100 m_cameraParamsRGB.fx(529.2151);
101 m_cameraParamsRGB.fy(525.5639);
102
103 m_cameraParamsRGB.dist.fill(0);
104
105 // ----- Depth -----
106 m_cameraParamsDepth.ncols = 640;
107 m_cameraParamsDepth.nrows = 488;
108
109 m_cameraParamsDepth.cx(339.30781);
110 m_cameraParamsDepth.cy(242.7391);
111 m_cameraParamsDepth.fx(594.21434);
112 m_cameraParamsDepth.fy(591.04054);
113
114 m_cameraParamsDepth.dist.fill(0);
115
116 #if !MRPT_HAS_KINECT
117 THROW_EXCEPTION("MRPT was compiled without support for Kinect. Please, rebuild it.");
118 #endif
119 }
120
121 /*-------------------------------------------------------------
122 dtor
123 -------------------------------------------------------------*/
124 CKinect::~CKinect() { this->close(); }
125 /** This method can or cannot be implemented in the derived class, depending on
126 * the need for it.
127 * \exception This method must throw an exception with a descriptive message if
128 * some critical error is found.
129 */
130 void CKinect::initialize() { open(); }
131 /** This method will be invoked at a minimum rate of "process_rate" (Hz)
132 * \exception This method must throw an exception with a descriptive message if
133 * some critical error is found.
134 */
135 void CKinect::doProcess()
136 {
137 bool thereIs, hwError;
138
139 CObservation3DRangeScan::Ptr newObs = std::make_shared<CObservation3DRangeScan>();
140 CObservationIMU::Ptr newObs_imu = std::make_shared<CObservationIMU>();
141
142 getNextObservation(*newObs, *newObs_imu, thereIs, hwError);
143
144 if (hwError)
145 {
146 m_state = ssError;
147 THROW_EXCEPTION("Couldn't communicate to the Kinect sensor!");
148 }
149
150 if (thereIs)
151 {
152 m_state = ssWorking;
153
154 vector<CSerializable::Ptr> objs;
155 if (m_grab_image || m_grab_depth || m_grab_3D_points) objs.push_back(newObs);
156 if (m_grab_IMU) objs.push_back(newObs_imu);
157
158 appendObservations(objs);
159 }
160 }
161
162 /** Loads specific configuration for the device from a given source of
163 * configuration parameters, for example, an ".ini" file, loading from the
164 * section "[iniSection]" (see config::CConfigFileBase and derived classes)
165 * \exception This method must throw an exception with a descriptive message if
166 * some critical parameter is missing or has an invalid value.
167 */
168 void CKinect::loadConfig_sensorSpecific(
169 const mrpt::config::CConfigFileBase& configSource, const std::string& iniSection)
170 {
171 m_sensorPoseOnRobot.setFromValues(
172 configSource.read_float(iniSection, "pose_x", 0),
173 configSource.read_float(iniSection, "pose_y", 0),
174 configSource.read_float(iniSection, "pose_z", 0),
175 DEG2RAD(configSource.read_float(iniSection, "pose_yaw", 0)),
176 DEG2RAD(configSource.read_float(iniSection, "pose_pitch", 0)),
177 DEG2RAD(configSource.read_float(iniSection, "pose_roll", 0)));
178
179 m_preview_window = configSource.read_bool(iniSection, "preview_window", m_preview_window);
180
181 // "Stereo" calibration data:
182 // [<SECTION>_LEFT] // Depth
183 // ...
184 // [<SECTION>_RIGHT] // RGB
185 // ...
186 // [<SECTION>_LEFT2RIGHT_POSE]
187 // pose_quaternion = [x y z qr qx qy qz]
188
189 const mrpt::poses::CPose3D twist(0, 0, 0, -90.0_deg, 0.0_deg, -90.0_deg);
190
191 mrpt::img::TStereoCamera sc;
192 sc.leftCamera = m_cameraParamsDepth; // Load default values so that if we
193 // fail to load from cfg at least we
194 // have some reasonable numbers.
195 sc.rightCamera = m_cameraParamsRGB;
196 sc.rightCameraPose = mrpt::poses::CPose3DQuat(m_relativePoseIntensityWRTDepth - twist).asTPose();
197
198 try
199 {
200 sc.loadFromConfigFile(iniSection, configSource);
201 }
202 catch (const std::exception& e)
203 {
204 std::cout << "[CKinect::loadConfig_sensorSpecific] Warning: Ignoring "
205 "error loading calibration parameters:\n"
206 << e.what();
207 }
208 m_cameraParamsDepth = sc.leftCamera;
209 m_cameraParamsRGB = sc.rightCamera;
210 m_relativePoseIntensityWRTDepth = twist + mrpt::poses::CPose3D(sc.rightCameraPose);
211
212 // Id:
213 m_user_device_number = configSource.read_int(iniSection, "device_number", m_user_device_number);
214
215 m_grab_image = configSource.read_bool(iniSection, "grab_image", m_grab_image);
216 m_grab_depth = configSource.read_bool(iniSection, "grab_depth", m_grab_depth);
217 m_grab_3D_points = configSource.read_bool(iniSection, "grab_3D_points", m_grab_3D_points);
218 m_grab_IMU = configSource.read_bool(iniSection, "grab_IMU", m_grab_IMU);
219
220 m_video_channel =
221 configSource.read_enum<TVideoChannel>(iniSection, "video_channel", m_video_channel);
222
223 {
224 std::string s = configSource.read_string(iniSection, "relativePoseIntensityWRTDepth", "");
225 if (!s.empty()) m_relativePoseIntensityWRTDepth.fromString(s);
226 }
227
228 m_initial_tilt_angle =
229 configSource.read_int(iniSection, "initial_tilt_angle", m_initial_tilt_angle);
230 }
231
232 bool CKinect::isOpen() const
233 {
234 #if MRPT_HAS_KINECT_FREENECT
235 return f_dev != nullptr;
236 #else
237 return false;
238 #endif
239
240 #
241 }
242
243 #if MRPT_HAS_KINECT_FREENECT
244 // ======== GLOBAL CALLBACK FUNCTIONS ========
245 void depth_cb(freenect_device* dev, void* v_depth, uint32_t timestamp)
246 {
247 const freenect_frame_mode frMode = freenect_get_current_video_mode(dev);
248
249 auto* depth = reinterpret_cast<uint16_t*>(v_depth);
250
251 auto* obj = reinterpret_cast<CKinect*>(freenect_get_user(dev));
252
253 // Update of the timestamps at the end:
254 std::lock_guard<std::mutex> lock(obj->internal_latest_obs_cs());
255 CObservation3DRangeScan& obs = obj->internal_latest_obs();
256
257 obs.hasRangeImage = true;
258 obs.range_is_depth = true;
259
260 #ifdef KINECT_PROFILE_MEM_ALLOC
261 alloc_tim.enter("depth_cb alloc");
262 #endif
263
264 // This method will try to exploit memory pooling if possible:
265 obs.rangeImage_setSize(frMode.height, frMode.width);
266
267 #ifdef KINECT_PROFILE_MEM_ALLOC
268 alloc_tim.leave("depth_cb alloc");
269 #endif
270
271 const CKinect::TDepth2RangeArray& r2m = obj->getRawDepth2RangeConversion();
272 obs.rangeUnits = 1e-3f; // we use mm as units
273
274 for (int r = 0; r < frMode.height; r++)
275 for (int c = 0; c < frMode.width; c++)
276 {
277 // For now, quickly save the depth as it comes from the sensor,
278 // it'll
279 // transformed later on in getNextObservation()
280 const uint16_t v = *depth++;
281 obs.rangeImage.coeffRef(r, c) = r2m[v & KINECT_RANGES_TABLE_MASK];
282 }
283 obj->internal_tim_latest_depth() = timestamp;
284 }
285
286 void rgb_cb(freenect_device* dev, void* img_data, uint32_t timestamp)
287 {
288 auto* obj = reinterpret_cast<CKinect*>(freenect_get_user(dev));
289 const freenect_frame_mode frMode = freenect_get_current_video_mode(dev);
290
291 // Update of the timestamps at the end:
292 std::lock_guard<std::mutex> lock(obj->internal_latest_obs_cs());
293 CObservation3DRangeScan& obs = obj->internal_latest_obs();
294
295 #ifdef KINECT_PROFILE_MEM_ALLOC
296 alloc_tim.enter("depth_rgb loadFromMemoryBuffer");
297 #endif
298
299 obs.hasIntensityImage = true;
300 if (obj->getVideoChannel() == CKinect::VIDEO_CHANNEL_RGB)
301 {
302 // Color image: We asked for Bayer data, so we can decode it outselves
303 // here
304 // and avoid having to reorder Green<->Red channels, as would be needed
305 // with
306 // the RGB image from freenect.
307 obs.intensityImageChannel = mrpt::obs::CObservation3DRangeScan::CH_VISIBLE;
308 obs.intensityImage.resize(frMode.width, frMode.height, mrpt::img::CH_RGB);
309
310 #if MRPT_HAS_OPENCV
311 const cv::Mat src_img_bayer(frMode.height, frMode.width, CV_8UC1, img_data, frMode.width);
312
313 cv::Mat& dst_img_RGB = obs.intensityImage.asCvMatRef();
314
315 // Decode Bayer image:
316 cv::cvtColor(src_img_bayer, dst_img_RGB, cv::COLOR_BayerGB2BGR);
317 #else
318 THROW_EXCEPTION("Need building with OpenCV!");
319 #endif
320 }
321 else
322 {
323 // IR data: grayscale 8bit
324 obs.intensityImageChannel = mrpt::obs::CObservation3DRangeScan::CH_IR;
325 obs.intensityImage.loadFromMemoryBuffer(
326 frMode.width, frMode.height,
327 false, // Color image?
328 reinterpret_cast<unsigned char*>(img_data));
329 }
330
331 // obs.intensityImage.setChannelsOrder_RGB();
332
333 #ifdef KINECT_PROFILE_MEM_ALLOC
334 alloc_tim.leave("depth_rgb loadFromMemoryBuffer");
335 #endif
336
337 obj->internal_tim_latest_rgb() = timestamp;
338 }
339 // ======== END OF GLOBAL CALLBACK FUNCTIONS ========
340 #endif // MRPT_HAS_KINECT_FREENECT
341
342 void CKinect::open()
343 {
344 if (isOpen()) close();
345
346 // Alloc memory, if this is the first time:
347 m_buf_depth.resize(640 * 480 * 3); // We'll resize this below if needed
348 m_buf_rgb.resize(640 * 480 * 3);
349
350 #if MRPT_HAS_KINECT_FREENECT // ----> libfreenect
351 // Try to open the device:
352 if (freenect_init(f_ctx_ptr, nullptr) < 0) THROW_EXCEPTION("freenect_init() failed");
353
354 freenect_set_log_level(
355 f_ctx,
356 #ifdef _DEBUG
357 FREENECT_LOG_DEBUG
358 #else
359 FREENECT_LOG_WARNING
360 #endif
361 );
362
363 int nr_devices = freenect_num_devices(f_ctx);
364 // printf("[CKinect] Number of devices found: %d\n", nr_devices);
365
366 if (!nr_devices) THROW_EXCEPTION("No Kinect devices found.");
367
368 // Open the given device number:
369 if (freenect_open_device(f_ctx, f_dev_ptr, m_user_device_number) < 0)
370 THROW_EXCEPTION_FMT("Error opening Kinect sensor with index: %d", m_user_device_number);
371
372 // Setup:
373 if (m_initial_tilt_angle != 360) // 360 means no motor command.
374 setTiltAngleDegrees(m_initial_tilt_angle);
375 freenect_set_led(f_dev, LED_RED);
376 freenect_set_depth_callback(f_dev, depth_cb);
377 freenect_set_video_callback(f_dev, rgb_cb);
378
379 // rgb or IR channel:
380 const freenect_frame_mode desiredFrMode = freenect_find_video_mode(
381 FREENECT_RESOLUTION_MEDIUM,
382 m_video_channel == VIDEO_CHANNEL_IR
383 ? FREENECT_VIDEO_IR_8BIT
384 : FREENECT_VIDEO_BAYER // FREENECT_VIDEO_RGB: Use Bayer instead
385 // so we can directly decode it here
386 );
387
388 // Switch to that video mode:
389 if (freenect_set_video_mode(f_dev, desiredFrMode) < 0)
390 THROW_EXCEPTION("Error setting Kinect video mode.");
391
392 // Get video mode:
393 const freenect_frame_mode frMode = freenect_get_current_video_mode(f_dev);
394
395 // Realloc mem:
396 m_buf_depth.resize(frMode.width * frMode.height * 3);
397 m_buf_rgb.resize(frMode.width * frMode.height * 3);
398
399 // Save resolution:
400 m_cameraParamsRGB.ncols = frMode.width;
401 m_cameraParamsRGB.nrows = frMode.height;
402
403 m_cameraParamsDepth.ncols = frMode.width;
404 m_cameraParamsDepth.nrows = frMode.height;
405
406 freenect_set_video_buffer(f_dev, &m_buf_rgb[0]);
407 freenect_set_depth_buffer(f_dev, &m_buf_depth[0]);
408
409 freenect_set_depth_mode(
410 f_dev, freenect_find_depth_mode(FREENECT_RESOLUTION_MEDIUM, FREENECT_DEPTH_10BIT));
411
412 // Set user data = pointer to "this":
413 freenect_set_user(f_dev, this);
414
415 if (freenect_start_depth(f_dev) < 0) THROW_EXCEPTION("Error starting depth streaming.");
416
417 if (freenect_start_video(f_dev) < 0) THROW_EXCEPTION("Error starting video streaming.");
418
419 #endif // MRPT_HAS_KINECT_FREENECT
420 }
421
422 void CKinect::close()
423 {
424 #if MRPT_HAS_KINECT_FREENECT
425 if (f_dev)
426 {
427 freenect_stop_depth(f_dev);
428 freenect_stop_video(f_dev);
429 freenect_close_device(f_dev);
430 }
431 m_f_dev = nullptr;
432
433 if (f_ctx) freenect_shutdown(f_ctx);
434 m_f_ctx = nullptr;
435 #endif // MRPT_HAS_KINECT_FREENECT
436 }
437
438 /** Changes the video channel to open (RGB or IR) - you can call this method
439 before start grabbing or in the middle of streaming and the video source will
440 change on the fly.
441 Default is RGB channel.
442 */
443 void CKinect::setVideoChannel([[maybe_unused]] const TVideoChannel vch)
444 {
445 #if MRPT_HAS_KINECT_FREENECT
446 m_video_channel = vch;
447 if (!isOpen()) return; // Nothing else to do here.
448
449 // rgb or IR channel:
450 freenect_stop_video(f_dev);
451
452 // rgb or IR channel:
453 const freenect_frame_mode desiredFrMode = freenect_find_video_mode(
454 FREENECT_RESOLUTION_MEDIUM,
455 m_video_channel == VIDEO_CHANNEL_IR
456 ? FREENECT_VIDEO_IR_8BIT
457 : FREENECT_VIDEO_BAYER // FREENECT_VIDEO_RGB: Use Bayer instead
458 // so we can directly decode it here
459 );
460
461 // Switch to that video mode:
462 if (freenect_set_video_mode(f_dev, desiredFrMode) < 0)
463 THROW_EXCEPTION("Error setting Kinect video mode.");
464
465 freenect_start_video(f_dev);
466
467 #endif // MRPT_HAS_KINECT_FREENECT
468 }
469
470 /** The main data retrieving function, to be called after calling loadConfig()
471 * and initialize().
472 * \param out_obs The output retrieved observation (only if
473 * there_is_obs=true).
474 * \param there_is_obs If set to false, there was no new observation.
475 * \param hardware_error True on hardware/comms error.
476 *
477 * \sa doProcess
478 */
479 void CKinect::getNextObservation(
480 mrpt::obs::CObservation3DRangeScan& m_out_obs, bool& there_is_obs, bool& hardware_error)
481 {
482 there_is_obs = false;
483 hardware_error = false;
484
485 #if MRPT_HAS_KINECT_FREENECT
486
487 using namespace std::chrono_literals;
488 const auto max_wait = 40ms; // 1/25 FPS
489
490 // Mark previous observation's timestamp as out-dated:
491 m_latest_obs.hasPoints3D = false;
492 m_latest_obs.hasRangeImage = false;
493 m_latest_obs.hasIntensityImage = false;
494 m_latest_obs.hasConfidenceImage = false;
495
496 const TTimeStamp tim0 = mrpt::Clock::now();
497
498 // Reset these timestamp flags so if they are !=0 in the next call we're
499 // sure they're new frames.
500 m_latest_obs_cs.lock();
501 m_tim_latest_rgb = 0;
502 m_tim_latest_depth = 0;
503 m_latest_obs_cs.unlock();
504
505 while (freenect_process_events(f_ctx) >= 0 && mrpt::Clock::now() < (tim0 + max_wait))
506 {
507 // Got a new frame?
508 if ((!m_grab_image || m_tim_latest_rgb != 0) && // If we are NOT grabbing RGB or we are
509 // and there's a new frame...
510 (!m_grab_depth || m_tim_latest_depth != 0) // If we are NOT grabbing Depth or we are
511 // and there's a new frame...
512 )
513 {
514 // Approx: 0.5ms delay between depth frame (first) and RGB frame
515 // (second).
516 // cout << "m_tim_latest_rgb: " << m_tim_latest_rgb << "
517 // m_tim_latest_depth: "<< m_tim_latest_depth <<endl;
518 there_is_obs = true;
519 break;
520 }
521 }
522
523 // Handle the case when there is NOT depth frames (if there's something very
524 // close blocking the IR sensor) but we have RGB:
525 if ((m_grab_image && m_tim_latest_rgb != 0) && (m_grab_depth && m_tim_latest_depth == 0))
526 {
527 // Mark the entire range data as invalid:
528 m_latest_obs.hasRangeImage = true;
529 m_latest_obs.range_is_depth = true;
530
531 m_latest_obs_cs.lock(); // Important: if system is running slow, etc.
532 // we cannot tell for sure that the depth
533 // buffer is not beeing filled right now:
534 m_latest_obs.rangeImage.setSize(m_cameraParamsDepth.nrows, m_cameraParamsDepth.ncols);
535 m_latest_obs.rangeImage.fill(0); // "0" means: error in range
536 m_latest_obs_cs.unlock();
537 there_is_obs = true;
538 }
539
540 if (!there_is_obs) return;
541
542 // We DO have a fresh new observation:
543
544 // Quick save the observation to the user's object:
545 m_latest_obs_cs.lock();
546 m_out_obs.swap(m_latest_obs);
547 m_latest_obs_cs.unlock();
548 #endif
549
550 // Set common data into observation:
551 // --------------------------------------
552 m_out_obs.sensorLabel = m_sensorLabel;
553 m_out_obs.timestamp = mrpt::Clock::now();
554 m_out_obs.sensorPose = m_sensorPoseOnRobot;
555 m_out_obs.relativePoseIntensityWRTDepth = m_relativePoseIntensityWRTDepth;
556
557 m_out_obs.cameraParams = m_cameraParamsDepth;
558 m_out_obs.cameraParamsIntensity = m_cameraParamsRGB;
559
560 // 3D point cloud:
561 if (m_out_obs.hasRangeImage && m_grab_3D_points)
562 {
563 m_out_obs.unprojectInto(m_out_obs);
564
565 if (!m_grab_depth)
566 {
567 m_out_obs.hasRangeImage = false;
568 m_out_obs.rangeImage.resize(0, 0);
569 }
570 }
571
572 // preview in real-time?
573 if (m_preview_window)
574 {
575 if (m_out_obs.hasRangeImage)
576 {
577 if (++m_preview_decim_counter_range > m_preview_window_decimation)
578 {
579 m_preview_decim_counter_range = 0;
580 if (!m_win_range)
581 {
582 m_win_range = mrpt::gui::CDisplayWindow::Create("Preview RANGE");
583 m_win_range->setPos(5, 5);
584 }
585
586 // Normalize the image
587 mrpt::img::CImage img = m_out_obs.rangeImage_getAsImage();
588 m_win_range->showImage(img);
589 }
590 }
591 if (m_out_obs.hasIntensityImage)
592 {
593 if (++m_preview_decim_counter_rgb > m_preview_window_decimation)
594 {
595 m_preview_decim_counter_rgb = 0;
596 if (!m_win_int)
597 {
598 m_win_int = mrpt::gui::CDisplayWindow::Create("Preview INTENSITY");
599 m_win_int->setPos(300, 5);
600 }
601 m_win_int->showImage(m_out_obs.intensityImage);
602 }
603 }
604 }
605 else
606 {
607 if (m_win_range) m_win_range.reset();
608 if (m_win_int) m_win_int.reset();
609 }
610 }
611
612 /* -----------------------------------------------------
613 getNextObservation (with IMU)
614 ----------------------------------------------------- */
615 void CKinect::getNextObservation(
616 mrpt::obs::CObservation3DRangeScan& out_obs,
617 mrpt::obs::CObservationIMU& out_obs_imu,
618 bool& there_is_obs,
619 bool& hardware_error)
620 {
621 // First, try getting the RGB+Depth data:
622 getNextObservation(out_obs, there_is_obs, hardware_error);
623
624 // If successful, fill out the accelerometer data:
625 if (there_is_obs && this->m_grab_IMU)
626 {
627 double acc_x = 0, acc_y = 0, acc_z = 0; // In m/s^2
628 bool has_good_acc = false;
629
630 #if MRPT_HAS_KINECT_FREENECT
631 {
632 freenect_update_tilt_state(f_dev);
633 freenect_raw_tilt_state* state = freenect_get_tilt_state(f_dev);
634 if (state)
635 {
636 has_good_acc = true;
637 double lx, ly, lz;
638 freenect_get_mks_accel(state, &lx, &ly, &lz);
639
640 // Convert to a unified coordinate system:
641 // +x: forward
642 // +y: left
643 // +z: upward
644 acc_x = -lz;
645 acc_y = -lx;
646 acc_z = -ly;
647 }
648 }
649 #endif
650
651 // Common part for any implementation:
652 if (has_good_acc)
653 {
654 out_obs_imu.sensorLabel = out_obs.sensorLabel + "_IMU";
655 out_obs_imu.timestamp = out_obs.timestamp;
656 out_obs_imu.sensorPose = out_obs.sensorPose;
657
658 for (auto&& i : out_obs_imu.dataIsPresent) i = false;
659
660 out_obs_imu.dataIsPresent[IMU_X_ACC] = true;
661 out_obs_imu.dataIsPresent[IMU_Y_ACC] = true;
662 out_obs_imu.dataIsPresent[IMU_Z_ACC] = true;
663
664 out_obs_imu.rawMeasurements[IMU_X_ACC] = acc_x;
665 out_obs_imu.rawMeasurements[IMU_Y_ACC] = acc_y;
666 out_obs_imu.rawMeasurements[IMU_Z_ACC] = acc_z;
667 }
668 }
669 }
670
671 /* -----------------------------------------------------
672 setPathForExternalImages
673 ----------------------------------------------------- */
674 void CKinect::setPathForExternalImages([[maybe_unused]] const std::string& directory)
675 {
676 // Ignore for now. It seems performance is better grabbing everything
677 // to a single big file than creating hundreds of smaller files per
678 // second...
679 return;
680
681 // if (!mrpt::system::createDirectory( directory ))
682 // {
683 // THROW_EXCEPTION_FMT("Error: Cannot create the directory for
684 // externally
685 // saved images: %s",directory.c_str() );
686 // }
687 // m_path_for_external_images = directory;
688 }
689
690 /** Change tilt angle \note Sensor must be open first. */
691 void CKinect::setTiltAngleDegrees([[maybe_unused]] double angle)
692 {
693 ASSERTMSG_(isOpen(), "Sensor must be open first");
694
695 #if MRPT_HAS_KINECT_FREENECT
696 freenect_set_tilt_degs(f_dev, angle);
697 #endif
698 }
699
700 double CKinect::getTiltAngleDegrees()
701 {
702 ASSERTMSG_(isOpen(), "Sensor must be open first");
703
704 #if MRPT_KINECT_WITH_FREENECT
705 freenect_update_tilt_state(f_dev);
706 freenect_raw_tilt_state* ts = freenect_get_tilt_state(f_dev);
707 return freenect_get_tilt_degs(ts);
708 #else
709 return 0;
710 #endif
711 }