CameraRealSense.cpp
Go to the documentation of this file.
1 /*
2 Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
3 All rights reserved.
4 
5 Redistribution and use in source and binary forms, with or without
6 modification, are permitted provided that the following conditions are met:
7  * Redistributions of source code must retain the above copyright
8  notice, this list of conditions and the following disclaimer.
9  * Redistributions in binary form must reproduce the above copyright
10  notice, this list of conditions and the following disclaimer in the
11  documentation and/or other materials provided with the distribution.
12  * Neither the name of the Universite de Sherbrooke nor the
13  names of its contributors may be used to endorse or promote products
14  derived from this software without specific prior written permission.
15 
16 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
17 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
18 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
19 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
20 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
21 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
22 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
23 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
24 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
25 */
26 
27 #include <rtabmap/core/camera/CameraRealSense.h>
28 #include <rtabmap/utilite/UTimer.h>
29 #include <rtabmap/utilite/UMath.h>
30 #include <rtabmap/utilite/UThreadC.h>
31 #include <rtabmap/core/util2d.h>
32 #include <opencv2/imgproc/types_c.h>
33 
34 #ifdef RTABMAP_REALSENSE
35 #include <librealsense/rs.hpp>
36 #ifdef RTABMAP_REALSENSE_SLAM
37 #include <rs_core.h>
38 #include <rs_utils.h>
39 #include <librealsense/slam/slam.h>
40 #endif
41 #endif
42 
43 
44 namespace rtabmap
45 {
46 
47 bool CameraRealSense::available()
48 {
49 #ifdef RTABMAP_REALSENSE
50  return true;
51 #else
52  return false;
53 #endif
54 }
55 
56 CameraRealSense::CameraRealSense(
57  int device,
58  int presetRGB,
59  int presetDepth,
60  bool computeOdometry,
61  float imageRate,
62  const rtabmap::Transform & localTransform) :
63  Camera(imageRate, localTransform)
64 #ifdef RTABMAP_REALSENSE
65  ,
66  ctx_(0),
67  dev_(0),
68  deviceId_(device),
69  presetRGB_(presetRGB),
70  presetDepth_(presetDepth),
71  computeOdometry_(computeOdometry),
72  depthScaledToRGBSize_(false),
73  rgbSource_(kColor),
74  slam_(0)
75 #endif
76 {
77  UDEBUG("");
78 }
79 
80 CameraRealSense::~CameraRealSense()
81 {
82  UDEBUG("");
83 #ifdef RTABMAP_REALSENSE
84  UDEBUG("");
85  if(dev_)
86  {
87  try
88  {
89  if(slam_!=0)
90  {
91  dev_->stop(rs::source::all_sources);
92  }
93  else
94  {
95  dev_->stop();
96  }
97  }
98  catch(const rs::error & error){UWARN("%s", error.what());}
99  dev_ = 0;
100  }
101  UDEBUG("");
102  if (ctx_)
103  {
104  delete ctx_;
105  }
106 #ifdef RTABMAP_REALSENSE_SLAM
107  UDEBUG("");
108  if(slam_)
109  {
110  UScopeMutex lock(slamLock_);
111  slam_->flush_resources();
112  delete slam_;
113  slam_ = 0;
114  }
115 #endif
116 #endif
117 }
118 
119 #ifdef RTABMAP_REALSENSE_SLAM
120 bool setStreamConfigIntrin(
121  rs::core::stream_type stream,
122  std::map< rs::core::stream_type, rs::core::intrinsics > intrinsics,
123  rs::core::video_module_interface::supported_module_config & supported_config,
124  rs::core::video_module_interface::actual_module_config & actual_config)
125 {
126  auto & supported_stream_config = supported_config[stream];
127  if (!supported_stream_config.is_enabled || supported_stream_config.size.width != intrinsics[stream].width || supported_stream_config.size.height != intrinsics[stream].height)
128  {
129  UERROR("size of stream is not supported by slam");
130  UERROR(" supported: stream %d, width: %d height: %d", (uint32_t) stream, supported_stream_config.size.width, supported_stream_config.size.height);
131  UERROR(" received: stream %d, width: %d height: %d", (uint32_t) stream, intrinsics[stream].width, intrinsics[stream].height);
132 
133  return false;
134  }
135  rs::core::video_module_interface::actual_image_stream_config &actual_stream_config = actual_config[stream];
136  actual_config[stream].size.width = intrinsics[stream].width;
137  actual_config[stream].size.height = intrinsics[stream].height;
138  actual_stream_config.frame_rate = supported_stream_config.frame_rate;
139  actual_stream_config.intrinsics = intrinsics[stream];
140  actual_stream_config.is_enabled = true;
141  return true;
142 }
143 #endif
144 
145 void CameraRealSense::setDepthScaledToRGBSize(bool enabled) {
146 #ifdef RTABMAP_REALSENSE
147  depthScaledToRGBSize_ = enabled;
148 #endif
149 }
150 void CameraRealSense::setRGBSource(RGBSource source)
151 {
152 #ifdef RTABMAP_REALSENSE
153  rgbSource_ = source;
154 #endif
155 }
156 
157 #ifdef RTABMAP_REALSENSE
158 template<class GET_DEPTH, class TRANSFER_PIXEL> void align_images(const rs_intrinsics & depth_intrin, const rs_extrinsics & depth_to_other, const rs_intrinsics & other_intrin, GET_DEPTH get_depth, TRANSFER_PIXEL transfer_pixel)
159 {
160  // Iterate over the pixels of the depth image
161 #pragma omp parallel for schedule(dynamic)
162  for(int depth_y = 0; depth_y < depth_intrin.height; ++depth_y)
163  {
164  int depth_pixel_index = depth_y * depth_intrin.width;
165  for(int depth_x = 0; depth_x < depth_intrin.width; ++depth_x, ++depth_pixel_index)
166  {
167  // Skip over depth pixels with the value of zero, we have no depth data so we will not write anything into our aligned images
168  if(float depth = get_depth(depth_pixel_index))
169  {
170  // Map the top-left corner of the depth pixel onto the other image
171  float depth_pixel[2] = {depth_x-0.5f, depth_y-0.5f}, depth_point[3], other_point[3], other_pixel[2];
172  rs_deproject_pixel_to_point(depth_point, &depth_intrin, depth_pixel, depth);
173  rs_transform_point_to_point(other_point, &depth_to_other, depth_point);
174  rs_project_point_to_pixel(other_pixel, &other_intrin, other_point);
175  const int other_x0 = static_cast<int>(other_pixel[0] + 0.5f);
176  const int other_y0 = static_cast<int>(other_pixel[1] + 0.5f);
177 
178  // Map the bottom-right corner of the depth pixel onto the other image
179  depth_pixel[0] = depth_x+0.5f; depth_pixel[1] = depth_y+0.5f;
180  rs_deproject_pixel_to_point(depth_point, &depth_intrin, depth_pixel, depth);
181  rs_transform_point_to_point(other_point, &depth_to_other, depth_point);
182  rs_project_point_to_pixel(other_pixel, &other_intrin, other_point);
183  const int other_x1 = static_cast<int>(other_pixel[0] + 0.5f);
184  const int other_y1 = static_cast<int>(other_pixel[1] + 0.5f);
185 
186  if(other_x0 < 0 || other_y0 < 0 || other_x1 >= other_intrin.width || other_y1 >= other_intrin.height) continue;
187 
188  // Transfer between the depth pixels and the pixels inside the rectangle on the other image
189  for(int y=other_y0; y<=other_y1; ++y) for(int x=other_x0; x<=other_x1; ++x) transfer_pixel(depth_pixel_index, y * other_intrin.width + x);
190  }
191  }
192  }
193 }
194 typedef uint8_t byte;
195 
196 void align_z_to_other(byte * z_aligned_to_other, const uint16_t * z_pixels, float z_scale, const rs_intrinsics & z_intrin, const rs_extrinsics & z_to_other, const rs_intrinsics & other_intrin)
197 {
198  auto out_z = (uint16_t *)(z_aligned_to_other);
199  align_images(z_intrin, z_to_other, other_intrin,
200  [z_pixels, z_scale](int z_pixel_index) { return z_scale * z_pixels[z_pixel_index]; },
201  [out_z, z_pixels](int z_pixel_index, int other_pixel_index) { out_z[other_pixel_index] = out_z[other_pixel_index] ? std::min(out_z[other_pixel_index],z_pixels[z_pixel_index]) : z_pixels[z_pixel_index]; });
202 }
203 
204 void align_disparity_to_other(byte * disparity_aligned_to_other, const uint16_t * disparity_pixels, float disparity_scale, const rs_intrinsics & disparity_intrin, const rs_extrinsics & disparity_to_other, const rs_intrinsics & other_intrin)
205 {
206  auto out_disparity = (uint16_t *)(disparity_aligned_to_other);
207  align_images(disparity_intrin, disparity_to_other, other_intrin,
208  [disparity_pixels, disparity_scale](int disparity_pixel_index) { return disparity_scale / disparity_pixels[disparity_pixel_index]; },
209  [out_disparity, disparity_pixels](int disparity_pixel_index, int other_pixel_index) { out_disparity[other_pixel_index] = disparity_pixels[disparity_pixel_index]; });
210 }
211 
212 template<int N> struct bytes { char b[N]; };
213 template<int N, class GET_DEPTH> void align_other_to_depth_bytes(byte * other_aligned_to_depth, GET_DEPTH get_depth, const rs_intrinsics & depth_intrin, const rs_extrinsics & depth_to_other, const rs_intrinsics & other_intrin, const byte * other_pixels)
214 {
215  auto in_other = (const bytes<N> *)(other_pixels);
216  auto out_other = (bytes<N> *)(other_aligned_to_depth);
217  align_images(depth_intrin, depth_to_other, other_intrin, get_depth,
218  [out_other, in_other](int depth_pixel_index, int other_pixel_index) { out_other[depth_pixel_index] = in_other[other_pixel_index]; });
219 }
220 
222 // Image rectification //
224 
225 std::vector<int> compute_rectification_table(const rs_intrinsics & rect_intrin, const rs_extrinsics & rect_to_unrect, const rs_intrinsics & unrect_intrin)
226 {
227  std::vector<int> rectification_table;
228  rectification_table.resize(rect_intrin.width * rect_intrin.height);
229  align_images(rect_intrin, rect_to_unrect, unrect_intrin, [](int) { return 1.0f; },
230  [&rectification_table](int rect_pixel_index, int unrect_pixel_index) { rectification_table[rect_pixel_index] = unrect_pixel_index; });
231  return rectification_table;
232 }
233 
234 template<class T> void rectify_image_pixels(T * rect_pixels, const std::vector<int> & rectification_table, const T * unrect_pixels)
235 {
236  for(auto entry : rectification_table) *rect_pixels++ = unrect_pixels[entry];
237 }
238 
239 void rectify_image(uint8_t * rect_pixels, const std::vector<int> & rectification_table, const uint8_t * unrect_pixels, rs_format format)
240 {
241  switch(format)
242  {
243  case RS_FORMAT_Y8:
244  return rectify_image_pixels((bytes<1> *)rect_pixels, rectification_table, (const bytes<1> *)unrect_pixels);
245  case RS_FORMAT_Y16: case RS_FORMAT_Z16:
246  return rectify_image_pixels((bytes<2> *)rect_pixels, rectification_table, (const bytes<2> *)unrect_pixels);
247  case RS_FORMAT_RGB8: case RS_FORMAT_BGR8:
248  return rectify_image_pixels((bytes<3> *)rect_pixels, rectification_table, (const bytes<3> *)unrect_pixels);
249  case RS_FORMAT_RGBA8: case RS_FORMAT_BGRA8:
250  return rectify_image_pixels((bytes<4> *)rect_pixels, rectification_table, (const bytes<4> *)unrect_pixels);
251  default:
252  assert(false); // NOTE: rectify_image_pixels(...) is not appropriate for RS_FORMAT_YUYV images, no logic prevents U/V channels from being written to one another
253  }
254 }
255 #endif
256 
257 bool CameraRealSense::init(const std::string & calibrationFolder, const std::string & cameraName)
258 {
259  UDEBUG("");
260 #ifdef RTABMAP_REALSENSE
261 
262  if(dev_)
263  {
264  try
265  {
266  if(slam_!=0)
267  {
268  dev_->stop(rs::source::all_sources);
269  }
270  else
271  {
272  dev_->stop();
273  }
274  }
275  catch(const rs::error & error){UWARN("%s", error.what());}
276  dev_ = 0;
277  }
278  bufferedFrames_.clear();
279  rsRectificationTable_.clear();
280 
281 #ifdef RTABMAP_REALSENSE_SLAM
282  motionSeq_[0] = motionSeq_[1] = 0;
283  if(slam_)
284  {
285  UScopeMutex lock(slamLock_);
286  UDEBUG("Flush slam");
287  slam_->flush_resources();
288  delete slam_;
289  slam_ = 0;
290  }
291 #endif
292 
293  if (ctx_ == 0)
294  {
295  ctx_ = new rs::context();
296  }
297 
298  UDEBUG("");
299  if (ctx_->get_device_count() == 0)
300  {
301  UERROR("No RealSense device detected!");
302  return false;
303  }
304 
305  UDEBUG("");
306  dev_ = ctx_->get_device(deviceId_);
307  if (dev_ == 0)
308  {
309  UERROR("Cannot connect to device %d", deviceId_);
310  return false;
311  }
312  std::string name = dev_->get_name();
313  UINFO("Using device %d, an %s", deviceId_, name.c_str());
314  UINFO(" Serial number: %s", dev_->get_serial());
315  UINFO(" Firmware version: %s", dev_->get_firmware_version());
316  UINFO(" Preset RGB: %d", presetRGB_);
317  UINFO(" Preset Depth: %d", presetDepth_);
318 
319 #ifndef RTABMAP_REALSENSE_SLAM
320  computeOdometry_ = false;
321 #endif
322 
323  if (name.find("ZR300") == std::string::npos)
324  {
325  // Only enable ZR300 functionality odometry if fisheye stream is enabled.
326  // Accel/Gyro automatically enabled when fisheye requested
327  computeOdometry_ = false;
328  // Only ZR300 has fisheye
329  if(rgbSource_ == kFishEye)
330  {
331  UWARN("Fisheye cannot be used with %s camera, using color instead...", name.c_str());
332  rgbSource_ = kColor;
333  }
334  }
335 
336  rs::intrinsics depth_intrin;
337  rs::intrinsics fisheye_intrin;
338  rs::intrinsics color_intrin;
339  // Configure depth and color to run with the device's preferred settings
340  UINFO("Enabling streams...");
341  // R200:
342  // 0=640x480 vs 480x360
343  // 1=1920x1080 vs 640x480
344  // 2=640x480 vs 320x240
345  try {
346 
347  // left/rgb stream
348  if(rgbSource_==kFishEye || computeOdometry_)
349  {
350  dev_->enable_stream(rs::stream::fisheye, 640, 480, rs::format::raw8, 30);
351  if(computeOdometry_)
352  {
353  // Needed to align image timestamps to common clock-domain with the motion events
354  dev_->set_option(rs::option::fisheye_strobe, 1);
355  }
356  // This option causes the fisheye image to be acquired in-sync with the depth image.
357  dev_->set_option(rs::option::fisheye_external_trigger, 1);
358  dev_->set_option(rs::option::fisheye_color_auto_exposure, 1);
359  fisheye_intrin = dev_->get_stream_intrinsics(rs::stream::fisheye);
360  UINFO(" Fisheye: %dx%d", fisheye_intrin.width, fisheye_intrin.height);
361  if(rgbSource_==kFishEye)
362  {
363  color_intrin = fisheye_intrin; // not rectified
364  }
365  }
366  if(rgbSource_!=kFishEye)
367  {
368  dev_->enable_stream(rs::stream::color, (rs::preset)presetRGB_);
369  color_intrin = dev_->get_stream_intrinsics(rs::stream::rectified_color); // rectified
370  UINFO(" RGB: %dx%d", color_intrin.width, color_intrin.height);
371 
372  if(rgbSource_==kInfrared)
373  {
374  dev_->enable_stream(rs::stream::infrared, (rs::preset)presetRGB_);
375  color_intrin = dev_->get_stream_intrinsics(rs::stream::infrared); // rectified
376  UINFO(" IR left: %dx%d", color_intrin.width, color_intrin.height);
377  }
378  }
379 
380  dev_->enable_stream(rs::stream::depth, (rs::preset)presetDepth_);
381  depth_intrin = dev_->get_stream_intrinsics(rs::stream::depth); // rectified
382  UINFO(" Depth: %dx%d", depth_intrin.width, depth_intrin.height);
383  }
384  catch(const rs::error & error)
385  {
386  UERROR("Failed starting the streams: %s", error.what());
387  return false;
388  }
389 
390  Transform imu2Camera = Transform::getIdentity();
391 
392 #ifdef RTABMAP_REALSENSE_SLAM
393  UDEBUG("Setup frame callback");
394  // Define lambda callback for receiving stream data
395  std::function<void(rs::frame)> frameCallback = [this](rs::frame frame)
396  {
397  if(slam_ != 0)
398  {
399  const auto timestampDomain = frame.get_frame_timestamp_domain();
400  if (rs::timestamp_domain::microcontroller != timestampDomain)
401  {
402  UERROR("error: Junk time stamp in stream: %d\twith frame counter: %d",
403  (int)(frame.get_stream_type()), frame.get_frame_number());
404  return ;
405  }
406  }
407 
408  int width = frame.get_width();
409  int height = frame.get_height();
410  rs::core::correlated_sample_set sample_set = {};
411 
412  rs::core::image_info info =
413  {
414  width,
415  height,
416  rs::utils::convert_pixel_format(frame.get_format()),
417  frame.get_stride()
418  };
419  cv::Mat image;
420  if(frame.get_format() == rs::format::raw8 || frame.get_format() == rs::format::y8)
421  {
422  image = cv::Mat(height, width, CV_8UC1, (unsigned char*)frame.get_data());
423  if(frame.get_stream_type() == rs::stream::fisheye)
424  {
425  // fisheye always received just after the depth image (doesn't have exact timestamp with depth)
426  if(bufferedFrames_.size())
427  {
428  bufferedFrames_.rbegin()->second.first = image.clone();
429  UScopeMutex lock(dataMutex_);
430  bool notify = lastSyncFrames_.first.empty();
431  lastSyncFrames_ = bufferedFrames_.rbegin()->second;
432  if(notify)
433  {
434  dataReady_.release();
435  }
436  bufferedFrames_.clear();
437  }
438  }
439  else if(frame.get_stream_type() == rs::stream::infrared) // infrared (does have exact timestamp with depth)
440  {
441  if(bufferedFrames_.find(frame.get_timestamp()) != bufferedFrames_.end())
442  {
443  bufferedFrames_.find(frame.get_timestamp())->second.first = image.clone();
444  UScopeMutex lock(dataMutex_);
445  bool notify = lastSyncFrames_.first.empty();
446  lastSyncFrames_ = bufferedFrames_.find(frame.get_timestamp())->second;
447  if(notify)
448  {
449  dataReady_.release();
450  }
451  bufferedFrames_.erase(frame.get_timestamp());
452  }
453  else
454  {
455  bufferedFrames_.insert(std::make_pair(frame.get_timestamp(), std::make_pair(image.clone(), cv::Mat())));
456  }
457  if(bufferedFrames_.size()>5)
458  {
459  UWARN("Frames cannot be synchronized!");
460  bufferedFrames_.clear();
461  }
462  return;
463  }
464  }
465  else if(frame.get_format() == rs::format::z16)
466  {
467  image = cv::Mat(height, width, CV_16UC1, (unsigned char*)frame.get_data());
468  if(bufferedFrames_.find(frame.get_timestamp()) != bufferedFrames_.end())
469  {
470  bufferedFrames_.find(frame.get_timestamp())->second.second = image.clone();
471  UScopeMutex lock(dataMutex_);
472  bool notify = lastSyncFrames_.first.empty();
473  lastSyncFrames_ = bufferedFrames_.find(frame.get_timestamp())->second;
474  if(notify)
475  {
476  dataReady_.release();
477  }
478  bufferedFrames_.erase(frame.get_timestamp());
479  }
480  else
481  {
482  bufferedFrames_.insert(std::make_pair(frame.get_timestamp(), std::make_pair(cv::Mat(), image.clone())));
483  }
484  if(bufferedFrames_.size()>5)
485  {
486  UWARN("Frames cannot be synchronized!");
487  bufferedFrames_.clear();
488  }
489  }
490  else if(frame.get_format() == rs::format::rgb8)
491  {
492  if(rsRectificationTable_.size())
493  {
494  image = cv::Mat(height, width, CV_8UC3);
495  rectify_image(image.data, rsRectificationTable_, (unsigned char*)frame.get_data(), (rs_format)frame.get_format());
496  }
497  else
498  {
499  image = cv::Mat(height, width, CV_8UC3, (unsigned char*)frame.get_data());
500  }
501  if(bufferedFrames_.find(frame.get_timestamp()) != bufferedFrames_.end())
502  {
503  bufferedFrames_.find(frame.get_timestamp())->second.first = image.clone();
504  UScopeMutex lock(dataMutex_);
505  bool notify = lastSyncFrames_.first.empty();
506  lastSyncFrames_ = bufferedFrames_.find(frame.get_timestamp())->second;
507  if(notify)
508  {
509  dataReady_.release();
510  }
511  bufferedFrames_.erase(frame.get_timestamp());
512  }
513  else
514  {
515  bufferedFrames_.insert(std::make_pair(frame.get_timestamp(), std::make_pair(image.clone(), cv::Mat())));
516  }
517  if(bufferedFrames_.size()>5)
518  {
519  UWARN("Frames cannot be synchronized!");
520  bufferedFrames_.clear();
521  }
522  return;
523  }
524  else
525  {
526  return;
527  }
528 
529  if(slam_ != 0)
530  {
531  rs::core::stream_type stream = rs::utils::convert_stream_type(frame.get_stream_type());
532  sample_set[stream] = rs::core::image_interface::create_instance_from_raw_data(
533  & info,
534  image.data,
535  stream,
536  rs::core::image_interface::flag::any,
537  frame.get_timestamp(),
538  (uint64_t)frame.get_frame_number(),
539  rs::core::timestamp_domain::microcontroller);
540 
541  UScopeMutex lock(slamLock_);
542  if (slam_->process_sample_set(sample_set) < rs::core::status_no_error)
543  {
544  UERROR("error: failed to process sample");
545  }
546  sample_set[stream]->release();
547  }
548  };
549 
550  UDEBUG("");
551  // Setup stream callback for stream
552  if(computeOdometry_ || rgbSource_ == kFishEye)
553  {
554  dev_->set_frame_callback(rs::stream::fisheye, frameCallback);
555  }
556  if(rgbSource_ == kInfrared)
557  {
558  dev_->set_frame_callback(rs::stream::infrared, frameCallback);
559  }
560  else if(rgbSource_ == kColor)
561  {
562  dev_->set_frame_callback(rs::stream::color, frameCallback);
563  }
564 
565  dev_->set_frame_callback(rs::stream::depth, frameCallback);
566 
567  UDEBUG("");
568 
569  if (computeOdometry_)
570  {
571  UDEBUG("Setup motion callback");
572  //define callback to the motion events and set it.
573  std::function<void(rs::motion_data)> motion_callback;
574  motion_callback = [this](rs::motion_data entry)
575  {
576  if ((entry.timestamp_data.source_id != RS_EVENT_IMU_GYRO) &&
577  (entry.timestamp_data.source_id != RS_EVENT_IMU_ACCEL))
578  return;
579 
580  rs_event_source motionType = entry.timestamp_data.source_id;
581 
582  rs::core::correlated_sample_set sample_set = {};
583  if (motionType == RS_EVENT_IMU_ACCEL)
584  {
585  sample_set[rs::core::motion_type::accel].timestamp = entry.timestamp_data.timestamp;
586  sample_set[rs::core::motion_type::accel].data[0] = (float)entry.axes[0];
587  sample_set[rs::core::motion_type::accel].data[1] = (float)entry.axes[1];
588  sample_set[rs::core::motion_type::accel].data[2] = (float)entry.axes[2];
589  sample_set[rs::core::motion_type::accel].type = rs::core::motion_type::accel;
590  ++motionSeq_[0];
591  sample_set[rs::core::motion_type::accel].frame_number = motionSeq_[0];
592  }
593  else if (motionType == RS_EVENT_IMU_GYRO)
594  {
595  sample_set[rs::core::motion_type::gyro].timestamp = entry.timestamp_data.timestamp;
596  sample_set[rs::core::motion_type::gyro].data[0] = (float)entry.axes[0];
597  sample_set[rs::core::motion_type::gyro].data[1] = (float)entry.axes[1];
598  sample_set[rs::core::motion_type::gyro].data[2] = (float)entry.axes[2];
599  sample_set[rs::core::motion_type::gyro].type = rs::core::motion_type::gyro;
600  ++motionSeq_[1];
601  sample_set[rs::core::motion_type::gyro].frame_number = motionSeq_[1];
602  }
603 
604  UScopeMutex lock(slamLock_);
605  if (slam_->process_sample_set(sample_set) < rs::core::status_no_error)
606  {
607  UERROR("error: failed to process sample");
608  }
609  };
610 
611  std::function<void(rs::timestamp_data)> timestamp_callback;
612  timestamp_callback = [](rs::timestamp_data entry) {};
613 
614  dev_->enable_motion_tracking(motion_callback, timestamp_callback);
615  UINFO(" enabled accel and gyro stream");
616 
617  rs::motion_intrinsics imuIntrinsics;
618  rs::extrinsics fisheye2ImuExtrinsics;
619  rs::extrinsics fisheye2DepthExtrinsics;
620  try
621  {
622  imuIntrinsics = dev_->get_motion_intrinsics();
623  fisheye2ImuExtrinsics = dev_->get_motion_extrinsics_from(rs::stream::fisheye);
624  fisheye2DepthExtrinsics = dev_->get_extrinsics(rs::stream::depth, rs::stream::fisheye);
625  }
626  catch (const rs::error & e) {
627  UERROR("Exception: %s (try to unplug/plug the camera)", e.what());
628  return false;
629  }
630 
631  UDEBUG("Setup SLAM");
632  UScopeMutex lock(slamLock_);
633  slam_ = new rs::slam::slam();
634  slam_->set_auto_occupancy_map_building(false);
635  slam_->force_relocalization_pose(false);
636 
637  rs::core::video_module_interface::supported_module_config supported_config = {};
638  if (slam_->query_supported_module_config(0, supported_config) < rs::core::status_no_error)
639  {
640  UERROR("Failed to query the first supported module configuration");
641  return false;
642  }
643 
644  rs::core::video_module_interface::actual_module_config actual_config = {};
645 
646  // Set camera intrinsics
647  std::map< rs::core::stream_type, rs::core::intrinsics > intrinsics;
648  intrinsics[rs::core::stream_type::fisheye] = rs::utils::convert_intrinsics(fisheye_intrin);
649  intrinsics[rs::core::stream_type::depth] = rs::utils::convert_intrinsics(depth_intrin);
650 
651  if(!setStreamConfigIntrin(rs::core::stream_type::fisheye, intrinsics, supported_config, actual_config))
652  {
653  return false;
654  }
655  if(!setStreamConfigIntrin(rs::core::stream_type::depth, intrinsics, supported_config, actual_config))
656  {
657  return false;
658  }
659 
660  // Set IMU intrinsics
661  actual_config[rs::core::motion_type::accel].is_enabled = true;
662  actual_config[rs::core::motion_type::gyro].is_enabled = true;
663  actual_config[rs::core::motion_type::gyro].intrinsics = rs::utils::convert_motion_device_intrinsics(imuIntrinsics.gyro);
664  actual_config[rs::core::motion_type::accel].intrinsics = rs::utils::convert_motion_device_intrinsics(imuIntrinsics.acc);
665 
666  // Set extrinsics
667  actual_config[rs::core::stream_type::fisheye].extrinsics_motion = rs::utils::convert_extrinsics(fisheye2ImuExtrinsics);
668  actual_config[rs::core::stream_type::fisheye].extrinsics = rs::utils::convert_extrinsics(fisheye2DepthExtrinsics);
669 
670  UDEBUG("Set SLAM config");
671  // Set actual config
672  if (slam_->set_module_config(actual_config) < rs::core::status_no_error)
673  {
674  UERROR("error : failed to set the enabled module configuration");
675  return false;
676  }
677 
678  rs::extrinsics fisheye2imu = dev_->get_motion_extrinsics_from(rs::stream::fisheye);
679  imu2Camera = Transform(
680  fisheye2imu.rotation[0], fisheye2imu.rotation[1], fisheye2imu.rotation[2], fisheye2imu.translation[0],
681  fisheye2imu.rotation[3], fisheye2imu.rotation[4], fisheye2imu.rotation[5], fisheye2imu.translation[1],
682  fisheye2imu.rotation[6], fisheye2imu.rotation[7], fisheye2imu.rotation[8], fisheye2imu.translation[2]).inverse();
683 
684  if(rgbSource_ == kInfrared)
685  {
686  rs::extrinsics color2Fisheye = dev_->get_extrinsics(rs::stream::fisheye, rs::stream::infrared);
687  Transform fisheye2Color = Transform(
688  color2Fisheye.rotation[0], color2Fisheye.rotation[1], color2Fisheye.rotation[2], color2Fisheye.translation[0],
689  color2Fisheye.rotation[3], color2Fisheye.rotation[4], color2Fisheye.rotation[5], color2Fisheye.translation[1],
690  color2Fisheye.rotation[6], color2Fisheye.rotation[7], color2Fisheye.rotation[8], color2Fisheye.translation[2]).inverse();
691  imu2Camera *= fisheye2Color;
692  }
693  else if(rgbSource_ == kColor)
694  {
695  rs::extrinsics color2Fisheye = dev_->get_extrinsics(rs::stream::fisheye, rs::stream::rectified_color);
696  Transform fisheye2Color = Transform(
697  color2Fisheye.rotation[0], color2Fisheye.rotation[1], color2Fisheye.rotation[2], color2Fisheye.translation[0],
698  color2Fisheye.rotation[3], color2Fisheye.rotation[4], color2Fisheye.rotation[5], color2Fisheye.translation[1],
699  color2Fisheye.rotation[6], color2Fisheye.rotation[7], color2Fisheye.rotation[8], color2Fisheye.translation[2]).inverse();
700  imu2Camera *= fisheye2Color;
701  }
702 
703  UDEBUG("start device!");
704  try
705  {
706  dev_->start(rs::source::all_sources);
707  }
708  catch(const rs::error & error)
709  {
710  UERROR("Failed starting the device: %s (try to unplug/plug the camera)", error.what());
711  return false;
712  }
713  }
714  else
715  {
716  UDEBUG("start device!");
717  try
718  {
719  dev_->start();
720  }
721  catch(const rs::error & error)
722  {
723  UERROR("Failed starting the device: %s (try to unplug/plug the camera)", error.what());
724  return false;
725  }
726  }
727 #else
728  try {
729  dev_->start();
730  dev_->wait_for_frames();
731  }
732  catch (const rs::error & e)
733  {
734  UERROR("Exception: %s (try to unplug/plug the camera)", e.what());
735  }
736 #endif
737 
738  cv::Mat D;
739  if(rgbSource_ == kFishEye)
740  {
741  // ftheta/equidistant model
742  D = cv::Mat::zeros(1,6,CV_64FC1);
743  D.at<double>(0,0) = color_intrin.coeffs[0];
744  D.at<double>(0,1) = color_intrin.coeffs[1];
745  D.at<double>(0,4) = color_intrin.coeffs[2];
746  D.at<double>(0,5) = color_intrin.coeffs[3];
747  }
748  else
749  {
750  // Brown-Conrady / radtan
751  D = cv::Mat::zeros(1,5,CV_64FC1);
752  D.at<double>(0,0) = color_intrin.coeffs[0];
753  D.at<double>(0,1) = color_intrin.coeffs[1];
754  D.at<double>(0,2) = color_intrin.coeffs[2];
755  D.at<double>(0,3) = color_intrin.coeffs[3];
756  D.at<double>(0,4) = color_intrin.coeffs[4];
757  }
758  cv::Mat K = cv::Mat::eye(3, 3, CV_64FC1);
759  K.at<double>(0,0) = color_intrin.fx;
760  K.at<double>(1,1) = color_intrin.fy;
761  K.at<double>(0,2) = color_intrin.ppx;
762  K.at<double>(1,2) = color_intrin.ppy;
763  cv::Mat R = cv::Mat::eye(3, 3, CV_64FC1);
764  cv::Mat P = cv::Mat::eye(3, 4, CV_64FC1);
765  K(cv::Range(0,2), cv::Range(0,3)).copyTo(P(cv::Range(0,2), cv::Range(0,3)));
766  cameraModel_ = CameraModel(
767  dev_->get_name(),
768  cv::Size(color_intrin.width, color_intrin.height),
769  K,
770  D,
771  R,
772  P,
773  this->getLocalTransform()*imu2Camera);
774 
775  UDEBUG("");
776 
777  if(rgbSource_ == kColor)
778  {
779  rs::extrinsics rect_to_unrect = dev_->get_extrinsics(rs::stream::rectified_color, rs::stream::color);
780  rs::intrinsics unrect_intrin = dev_->get_stream_intrinsics(rs::stream::color);
781  rsRectificationTable_ = compute_rectification_table(color_intrin, rect_to_unrect, unrect_intrin);
782  }
783  else if(rgbSource_ == kFishEye)
784  {
785  UINFO("calibration folder=%s name=%s", calibrationFolder.c_str(), cameraName.c_str());
786  if(!calibrationFolder.empty() && !cameraName.empty())
787  {
788  CameraModel model;
789  if(!model.load(calibrationFolder, cameraName))
790  {
791  UWARN("Failed to load calibration \"%s\" in \"%s\" folder, you should calibrate the camera!",
792  cameraName.c_str(), calibrationFolder.c_str());
793  }
794  else
795  {
796  UINFO("Camera parameters: fx=%f fy=%f cx=%f cy=%f",
797  model.fx(),
798  model.fy(),
799  model.cx(),
800  model.cy());
801  cameraModel_ = model;
802  cameraModel_.setName(cameraName);
803  cameraModel_.initRectificationMap();
804  cameraModel_.setLocalTransform(this->getLocalTransform()*imu2Camera);
805  }
806  }
807  }
808 
809  uSleep(1000); // ignore the first frames
810  UINFO("Enabling streams...done!");
811 
812  return true;
813 
814 #else
815  UERROR("CameraRealSense: RTAB-Map is not built with RealSense support!");
816  return false;
817 #endif
818 }
819 
820 bool CameraRealSense::isCalibrated() const
821 {
822  return true;
823 }
824 
825 std::string CameraRealSense::getSerial() const
826 {
827 #ifdef RTABMAP_REALSENSE
828  if (dev_)
829  {
830  return dev_->get_serial();
831  }
832  else
833  {
834  UERROR("Cannot get a serial before initialization. Call init() before.");
835  }
836 #endif
837  return "NA";
838 }
839 
840 bool CameraRealSense::odomProvided() const
841 {
842 #ifdef RTABMAP_REALSENSE_SLAM
843  return slam_!=0;
844 #else
845  return false;
846 #endif
847 }
848 
849 #ifdef RTABMAP_REALSENSE_SLAM
850 Transform rsPoseToTransform(const rs::slam::PoseMatrix4f & pose)
851 {
852  return Transform(
853  pose.m_data[0], pose.m_data[1], pose.m_data[2], pose.m_data[3],
854  pose.m_data[4], pose.m_data[5], pose.m_data[6], pose.m_data[7],
855  pose.m_data[8], pose.m_data[9], pose.m_data[10], pose.m_data[11]);
856 }
857 #endif
858 
859 SensorData CameraRealSense::captureImage(CameraInfo * info)
860 {
861  SensorData data;
862 #ifdef RTABMAP_REALSENSE
863  if (dev_)
864  {
865  cv::Mat rgb;
866  cv::Mat depthIn;
867 
868  // Retrieve camera parameters for mapping between depth and color
869  rs::intrinsics depth_intrin = dev_->get_stream_intrinsics(rs::stream::depth);
870  rs::extrinsics depth_to_color;
871  rs::intrinsics color_intrin;
872  if(rgbSource_ == kFishEye)
873  {
874  depth_to_color = dev_->get_extrinsics(rs::stream::depth, rs::stream::fisheye);
875  color_intrin = dev_->get_stream_intrinsics(rs::stream::fisheye);
876  }
877  else if(rgbSource_ == kInfrared)
878  {
879  depth_to_color = dev_->get_extrinsics(rs::stream::depth, rs::stream::infrared);
880  color_intrin = dev_->get_stream_intrinsics(rs::stream::infrared);
881  }
882  else // color
883  {
884  depth_to_color = dev_->get_extrinsics(rs::stream::depth, rs::stream::rectified_color);
885  color_intrin = dev_->get_stream_intrinsics(rs::stream::rectified_color);
886  }
887 
888 #ifdef RTABMAP_REALSENSE_SLAM
889  if(!dataReady_.acquire(1, 5000))
890  {
891  UWARN("Not received new frames since 5 seconds, end of stream reached!");
892  return data;
893  }
894 
895  {
896  UScopeMutex lock(dataMutex_);
897  rgb = lastSyncFrames_.first;
898  depthIn = lastSyncFrames_.second;
899  lastSyncFrames_.first = cv::Mat();
900  lastSyncFrames_.second = cv::Mat();
901  }
902 
903  if(rgb.empty() || depthIn.empty())
904  {
905  return data;
906  }
907 #else
908  try {
909  dev_->wait_for_frames();
910  }
911  catch (const rs::error & e)
912  {
913  UERROR("Exception: %s", e.what());
914  return data;
915  }
916 
917  // Retrieve our images
918  depthIn = cv::Mat(depth_intrin.height, depth_intrin.width, CV_16UC1, (unsigned char*)dev_->get_frame_data(rs::stream::depth));
919  if(rgbSource_ == kFishEye)
920  {
921  rgb = cv::Mat(color_intrin.height, color_intrin.width, CV_8UC1, (unsigned char*)dev_->get_frame_data(rs::stream::fisheye));
922  }
923  else if(rgbSource_ == kInfrared)
924  {
925  rgb = cv::Mat(color_intrin.height, color_intrin.width, CV_8UC1, (unsigned char*)dev_->get_frame_data(rs::stream::infrared));
926  }
927  else
928  {
929  rgb = cv::Mat(color_intrin.height, color_intrin.width, CV_8UC3, (unsigned char*)dev_->get_frame_data(rs::stream::color));
930  }
931 #endif
932 
933  // factory registration...
934  cv::Mat bgr;
935  if(rgbSource_ != kColor)
936  {
937  bgr = rgb;
938  }
939  else
940  {
941  cv::cvtColor(rgb, bgr, CV_RGB2BGR);
942  }
943 
944  bool rectified = false;
945  if(rgbSource_ == kFishEye && cameraModel_.isRectificationMapInitialized())
946  {
947  bgr = cameraModel_.rectifyImage(bgr);
948  rectified = true;
949  color_intrin.fx = cameraModel_.fx();
950  color_intrin.fy = cameraModel_.fy();
951  color_intrin.ppx = cameraModel_.cx();
952  color_intrin.ppy = cameraModel_.cy();
953  UASSERT_MSG(color_intrin.width == cameraModel_.imageWidth() && color_intrin.height == cameraModel_.imageHeight(),
954  uFormat("color_intrin=%dx%d cameraModel_=%dx%d",
955  color_intrin.width, color_intrin.height, cameraModel_.imageWidth(), cameraModel_.imageHeight()).c_str());
956  ((rs_intrinsics*)&color_intrin)->model = RS_DISTORTION_NONE;
957  }
958 #ifndef RTABMAP_REALSENSE_SLAM
959  else if(rgbSource_ != kColor)
960  {
961  bgr = bgr.clone();
962  }
963 #endif
964 
965  cv::Mat depth;
966  if(rgbSource_ != kFishEye || rectified)
967  {
968  if (color_intrin.width % depth_intrin.width == 0 && color_intrin.height % depth_intrin.height == 0 &&
969  depth_intrin.width < color_intrin.width &&
970  depth_intrin.height < color_intrin.height &&
971  !depthScaledToRGBSize_)
972  {
973  //we can keep the depth image size as is
974  depth = cv::Mat::zeros(cv::Size(depth_intrin.width, depth_intrin.height), CV_16UC1);
975  float scaleX = float(depth_intrin.width) / float(color_intrin.width);
976  float scaleY = float(depth_intrin.height) / float(color_intrin.height);
977  color_intrin.fx *= scaleX;
978  color_intrin.fy *= scaleY;
979  color_intrin.ppx *= scaleX;
980  color_intrin.ppy *= scaleY;
981  color_intrin.height = depth_intrin.height;
982  color_intrin.width = depth_intrin.width;
983  }
984  else
985  {
986  //depth to color
987  depth = cv::Mat::zeros(bgr.size(), CV_16UC1);
988  }
989 
990  float scale = dev_->get_depth_scale();
991  for (int dy = 0; dy < depth_intrin.height; ++dy)
992  {
993  for (int dx = 0; dx < depth_intrin.width; ++dx)
994  {
995  // Retrieve the 16-bit depth value and map it into a depth in meters
996  uint16_t depth_value = depthIn.at<unsigned short>(dy,dx);
997  float depth_in_meters = depth_value * scale;
998 
999  // Skip over pixels with a depth value of zero, which is used to indicate no data
1000  if (depth_value == 0 || depth_in_meters>10.0f) continue;
1001 
1002  // Map from pixel coordinates in the depth image to pixel coordinates in the color image
1003  int pdx = dx;
1004  int pdy = dy;
1005  if(rgbSource_ == kColor || rgbSource_ == kFishEye)
1006  {
1007  rs::float2 depth_pixel = { (float)dx, (float)dy };
1008  rs::float3 depth_point = depth_intrin.deproject(depth_pixel, depth_in_meters);
1009  rs::float3 color_point = depth_to_color.transform(depth_point);
1010  rs::float2 color_pixel = color_intrin.project(color_point);
1011 
1012  pdx = color_pixel.x;
1013  pdy = color_pixel.y;
1014  }
1015  //else infrared is already registered
1016 
1017  if (uIsInBounds(pdx, 0, depth.cols) && uIsInBounds(pdy, 0, depth.rows))
1018  {
1019  depth.at<unsigned short>(pdy, pdx) = (unsigned short)(depth_in_meters*1000.0f); // convert to mm
1020  }
1021  }
1022  }
1023 
1024  if (color_intrin.width > depth_intrin.width)
1025  {
1026  // Fill holes
1027  UTimer time;
1028  util2d::fillRegisteredDepthHoles(depth, true, true, color_intrin.width > depth_intrin.width * 2);
1029  util2d::fillRegisteredDepthHoles(depth, true, true, color_intrin.width > depth_intrin.width * 2);//second pass
1030  UDEBUG("Filling depth holes: %fs", time.ticks());
1031  }
1032  }
1033 
1034  if (!bgr.empty() && ((rgbSource_==kFishEye && !rectified) || !depth.empty()))
1035  {
1036  data = SensorData(bgr, depth, cameraModel_, this->getNextSeqID(), UTimer::now());
1037 #ifdef RTABMAP_REALSENSE_SLAM
1038  if(info && slam_)
1039  {
1040  UScopeMutex lock(slamLock_);
1041  rs::slam::PoseMatrix4f pose;
1042  if(slam_->get_camera_pose(pose) == rs::core::status_no_error)
1043  {
1044  /*rs::slam::tracking_accuracy accuracy = slam_->get_tracking_accuracy();
1045  if( accuracy == rs::slam::tracking_accuracy::low ||
1046  accuracy == rs::slam::tracking_accuracy::medium ||
1047  accuracy == rs::slam::tracking_accuracy::high)*/
1048  {
1049  // the pose is in camera link or IMU frame, get pose of the color camera
1050  Transform opticalRotation(0,0,1,0, -1,0,0,0, 0,-1,0,0);
1051  info->odomPose = opticalRotation * rsPoseToTransform(pose) * opticalRotation.inverse();
1052  info->odomCovariance = cv::Mat::eye(6, 6, CV_64FC1) * 0.0005;
1053  }
1054  /*else
1055  {
1056  UERROR("Odometry failed: accuracy=%d", accuracy);
1057  }*/
1058  }
1059  else
1060  {
1061  UERROR("Failed getting odometry pose");
1062  }
1063  }
1064 #endif
1065  }
1066  }
1067  else
1068  {
1069  ULOGGER_WARN("The camera must be initialized before requesting an image.");
1070  }
1071 #else
1072  UERROR("CameraRealSense: RTAB-Map is not built with RealSense support!");
1073 #endif
1074  return data;
1075 }
1076 
1077 } // namespace rtabmap
rtabmap::CameraInfo::odomCovariance
cv::Mat odomCovariance
Definition: CameraInfo.h:70
UTimer
Definition: UTimer.h:46
glm::any
GLM_FUNC_DECL bool any(vecType< bool, P > const &v)
glm::min
GLM_FUNC_DECL genType min(genType const &x, genType const &y)
glm::float2
detail::tvec2< float, highp > float2
single-precision floating-point vector with 2 components. (From GLM_GTX_compatibility extension) ...
Definition: compatibility.hpp:122
f
f
uIsInBounds
bool uIsInBounds(const T &value, const T &low, const T &high)
Definition: UMath.h:934
x
x
rtabmap::CameraRealSense::setRGBSource
void setRGBSource(RGBSource source)
Definition: CameraRealSense.cpp:150
rtabmap::CameraRealSense::setDepthScaledToRGBSize
void setDepthScaledToRGBSize(bool enabled)
Definition: CameraRealSense.cpp:145
T
T
rtabmap::Transform::getIdentity
static Transform getIdentity()
Definition: Transform.cpp:401
rtabmap::CameraRealSense::odomProvided
virtual bool odomProvided() const
Definition: CameraRealSense.cpp:840
data
data
glm::e
GLM_FUNC_DECL genType e()
glm::scale
GLM_FUNC_DECL detail::tmat4x4< T, P > scale(detail::tmat4x4< T, P > const &m, detail::tvec3< T, P > const &v)
glm::byte
detail::uint8 byte
Definition: raw_data.hpp:55
rtabmap::CameraRealSense::isCalibrated
virtual bool isCalibrated() const
Definition: CameraRealSense.cpp:820
rtabmap::CameraRealSense::getSerial
virtual std::string getSerial() const
Definition: CameraRealSense.cpp:825
rtabmap::CameraModel::fx
double fx() const
Definition: CameraModel.h:102
UMath.h
Basic mathematics functions.
rtabmap::CameraModel::load
bool load(const std::string &filePath)
Definition: CameraModel.cpp:215
rtabmap::CameraRealSense::captureImage
virtual SensorData captureImage(CameraInfo *info=0)
Definition: CameraRealSense.cpp:859
glm::uint16_t
detail::uint16 uint16_t
Definition: fwd.hpp:912
glm::uint8_t
detail::uint8 uint8_t
Definition: fwd.hpp:908
UASSERT_MSG
#define UASSERT_MSG(condition, msg_str)
Definition: ULogger.h:67
name
string name
source
const char * source
Definition: lz4hc.h:181
glm::uint64_t
detail::uint64 uint64_t
Definition: fwd.hpp:920
rtabmap_superglue.device
string device
Definition: rtabmap_superglue.py:21
rtabmap::Camera::getLocalTransform
const Transform & getLocalTransform() const
Definition: Camera.h:65
rtabmap::util2d::fillRegisteredDepthHoles
void RTABMAP_EXP fillRegisteredDepthHoles(cv::Mat &depthRegistered, bool vertical, bool horizontal, bool fillDoubleHoles=false)
Definition: util2d.cpp:1598
rtabmap::CameraModel::cx
double cx() const
Definition: CameraModel.h:104
glm::uint32_t
detail::uint32 uint32_t
Definition: fwd.hpp:916
false
#define false
Definition: ConvertUTF.c:56
uSleep
void uSleep(unsigned int ms)
Definition: Posix/UThreadC.h:23
rtabmap::Camera::getNextSeqID
int getNextSeqID()
Definition: Camera.h:86
rtabmap::CameraModel::cy
double cy() const
Definition: CameraModel.h:105
UDEBUG
#define UDEBUG(...)
UERROR
#define UERROR(...)
ULOGGER_WARN
#define ULOGGER_WARN(...)
Definition: ULogger.h:55
UWARN
#define UWARN(...)
UTimer::ticks
double ticks()
Definition: UTimer.cpp:117
UTimer::now
static double now()
Definition: UTimer.cpp:80
rtabmap::CameraModel::fy
double fy() const
Definition: CameraModel.h:103
rtabmap::CameraRealSense::init
virtual bool init(const std::string &calibrationFolder=".", const std::string &cameraName="")
Definition: CameraRealSense.cpp:257
rtabmap::CameraRealSense::CameraRealSense
CameraRealSense(int deviceId=0, int presetRGB=0, int presetDepth=0, bool computeOdometry=false, float imageRate=0, const Transform &localTransform=Transform::getIdentity())
Definition: CameraRealSense.cpp:56
trace.model
model
Definition: trace.py:4
glm::float3
detail::tvec3< float, highp > float3
single-precision floating-point vector with 3 components. (From GLM_GTX_compatibility extension) ...
Definition: compatibility.hpp:123
rtabmap::CameraInfo::odomPose
Transform odomPose
Definition: CameraInfo.h:69
uFormat
std::string UTILITE_EXP uFormat(const char *fmt,...)
Definition: UConversion.cpp:365
rtabmap::Transform::inverse
Transform inverse() const
Definition: Transform.cpp:178
UINFO
#define UINFO(...)

rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Jan 23 2023 03:37:28