CameraRealSense2.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/CameraRealSense2.h>
28 #include <rtabmap/core/util2d.h>
29 #include <rtabmap/utilite/UTimer.h>
30 #include <rtabmap/utilite/UThreadC.h>
31 #include <rtabmap/utilite/UConversion.h>
32 #include <rtabmap/utilite/UStl.h>
33 #include <opencv2/imgproc/types_c.h>
34 
35 #ifdef RTABMAP_REALSENSE2
36 #include <librealsense2/rsutil.h>
37 #include <librealsense2/hpp/rs_processing.hpp>
38 #include <librealsense2/rs_advanced_mode.hpp>
39 #include <fstream>
40 #endif
41 
42 namespace rtabmap
43 {
44 
45 bool CameraRealSense2::available()
46 {
47 #ifdef RTABMAP_REALSENSE2
48  return true;
49 #else
50  return false;
51 #endif
52 }
53 
54 CameraRealSense2::CameraRealSense2(
55  const std::string & device,
56  float imageRate,
57  const rtabmap::Transform & localTransform) :
58  Camera(imageRate, localTransform)
59 #ifdef RTABMAP_REALSENSE2
60  ,
61  deviceId_(device),
62  depth_scale_meters_(1.0f),
63  lastImuStamp_(0.0),
64  clockSyncWarningShown_(false),
65  imuGlobalSyncWarningShown_(false),
66  emitterEnabled_(true),
67  ir_(false),
68  irDepth_(true),
69  rectifyImages_(true),
70  odometryProvided_(false),
71  odometryImagesDisabled_(false),
72  odometryOnlyLeftStream_(false),
73  cameraWidth_(640),
74  cameraHeight_(480),
75  cameraFps_(30),
76  cameraDepthWidth_(640),
77  cameraDepthHeight_(480),
78  cameraDepthFps_(30),
79  globalTimeSync_(true),
80  dualMode_(false),
81  closing_(false)
82 #endif
83 {
84  UDEBUG("");
85 }
86 
87 CameraRealSense2::~CameraRealSense2()
88 {
89 #ifdef RTABMAP_REALSENSE2
90  close();
91 #endif
92 }
93 
94 #ifdef RTABMAP_REALSENSE2
95 void CameraRealSense2::close()
96 {
97  closing_ = true;
98  try
99  {
100  UDEBUG("Closing device(s)...");
101  for(size_t i=0; i<dev_.size(); ++i)
102  {
103  UDEBUG("Closing %d sensor(s) from device %d...", (int)dev_[i].query_sensors().size(), (int)i);
104  for(rs2::sensor _sensor : dev_[i].query_sensors())
105  {
106  if(!_sensor.get_active_streams().empty())
107  {
108  try
109  {
110  _sensor.stop();
111  _sensor.close();
112  }
113  catch(const rs2::error & error)
114  {
115  UWARN("%s", error.what());
116  }
117  }
118  }
119 #ifdef WIN32
120  dev_[i].hardware_reset(); // To avoid freezing on some Windows computers in the following destructor
121  // Don't do this on linux (tested on Ubuntu 18.04, realsense v2.41.0): T265 cannot be restarted
122 #endif
123  }
124  UDEBUG("Clearing devices...");
125  dev_.clear();
126  }
127  catch(const rs2::error & error)
128  {
129  UINFO("%s", error.what());
130  }
131 
132  closing_ = false;
133 }
134 
135 void CameraRealSense2::imu_callback(rs2::frame frame)
136 {
137  auto stream = frame.get_profile().stream_type();
138  cv::Vec3f crnt_reading = *reinterpret_cast<const cv::Vec3f*>(frame.get_data());
139  //UDEBUG("%s callback! %f (%f %f %f)",
140  // stream == RS2_STREAM_GYRO?"GYRO":"ACC",
141  // frame.get_timestamp(),
142  // crnt_reading[0],
143  // crnt_reading[1],
144  // crnt_reading[2]);
145  UScopeMutex sm(imuMutex_);
146  if(stream == RS2_STREAM_GYRO)
147  {
148  gyroBuffer_.insert(gyroBuffer_.end(), std::make_pair(frame.get_timestamp(), crnt_reading));
149  if(gyroBuffer_.size() > 1000)
150  {
151  gyroBuffer_.erase(gyroBuffer_.begin());
152  }
153  }
154  else
155  {
156  accBuffer_.insert(accBuffer_.end(), std::make_pair(frame.get_timestamp(), crnt_reading));
157  if(accBuffer_.size() > 1000)
158  {
159  accBuffer_.erase(accBuffer_.begin());
160  }
161  }
162 }
163 
164 // See https://github.com/IntelRealSense/realsense-ros/blob/2a45f09003c98a5bdf39ee89df032bdb9c9bcd2d/realsense2_camera/src/base_realsense_node.cpp#L1397-L1404
165 Transform CameraRealSense2::realsense2PoseRotation_ = Transform(
166  0, 0,-1,0,
167  -1, 0, 0,0,
168  0, 1, 0,0);
169 
170 void CameraRealSense2::pose_callback(rs2::frame frame)
171 {
172  rs2_pose pose = frame.as<rs2::pose_frame>().get_pose_data();
173  // See https://github.com/IntelRealSense/realsense-ros/blob/2a45f09003c98a5bdf39ee89df032bdb9c9bcd2d/realsense2_camera/src/base_realsense_node.cpp#L1397-L1404
174  Transform poseT = Transform(
175  -pose.translation.z,
176  -pose.translation.x,
177  pose.translation.y,
178  -pose.rotation.z,
179  -pose.rotation.x,
180  pose.rotation.y,
181  pose.rotation.w);
182 
183  //UDEBUG("POSE callback! %f %s (confidence=%d)", frame.get_timestamp(), poseT.prettyPrint().c_str(), (int)pose.tracker_confidence);
184 
185  UScopeMutex sm(poseMutex_);
186  poseBuffer_.insert(poseBuffer_.end(), std::make_pair(frame.get_timestamp(), std::make_pair(poseT, pose.tracker_confidence)));
187  if(poseBuffer_.size() > 1000)
188  {
189  poseBuffer_.erase(poseBuffer_.begin());
190  }
191 }
192 
193 void CameraRealSense2::frame_callback(rs2::frame frame)
194 {
195  //UDEBUG("Frame callback! %f", frame.get_timestamp());
196  syncer_(frame);
197 }
198 void CameraRealSense2::multiple_message_callback(rs2::frame frame)
199 {
200  if(closing_)
201  {
202  return;
203  }
204  auto stream = frame.get_profile().stream_type();
205  switch (stream)
206  {
207  case RS2_STREAM_GYRO:
208  case RS2_STREAM_ACCEL:
209  //UWARN("IMU : Domain=%d time=%f host=%f", frame.get_frame_timestamp_domain(), frame.get_timestamp()/1000.0, UTimer::now());
210  imu_callback(frame);
211  break;
212  case RS2_STREAM_POSE:
213  //UWARN("POSE : Domain=%d time=%f host=%f", frame.get_frame_timestamp_domain(), frame.get_timestamp()/1000.0, UTimer::now());
214  if(odometryProvided_)
215  {
216  pose_callback(frame);
217  }
218  break;
219  default:
220  //UWARN("IMG : Domain=%d time=%f host=%f", frame.get_frame_timestamp_domain(), frame.get_timestamp()/1000.0, UTimer::now());
221  frame_callback(frame);
222  }
223 }
224 
225 void CameraRealSense2::getPoseAndIMU(
226  const double & stamp,
227  Transform & pose,
228  unsigned int & poseConfidence,
229  IMU & imu,
230  int maxWaitTimeMs)
231 {
232  pose.setNull();
233  imu = IMU();
234  poseConfidence = 0;
235 
236  // Interpolate pose
237  if(!poseBuffer_.empty())
238  {
239  poseMutex_.lock();
240  int waitTry = 0;
241  while(maxWaitTimeMs>0 && poseBuffer_.rbegin()->first < stamp && waitTry < maxWaitTimeMs)
242  {
243  poseMutex_.unlock();
244  ++waitTry;
245  uSleep(1);
246  poseMutex_.lock();
247  }
248  if(poseBuffer_.rbegin()->first < stamp)
249  {
250  if(maxWaitTimeMs > 0)
251  {
252  UWARN("Could not find poses to interpolate at image time %f after waiting %d ms (last is %f)...", stamp/1000.0, maxWaitTimeMs, poseBuffer_.rbegin()->first/1000.0);
253  }
254  }
255  else
256  {
257  std::map<double, std::pair<Transform, unsigned int> >::const_iterator iterB = poseBuffer_.lower_bound(stamp);
258  std::map<double, std::pair<Transform, unsigned int> >::const_iterator iterA = iterB;
259  if(iterA != poseBuffer_.begin())
260  {
261  iterA = --iterA;
262  }
263  if(iterB == poseBuffer_.end())
264  {
265  iterB = --iterB;
266  }
267  if(iterA == iterB && stamp == iterA->first)
268  {
269  pose = iterA->second.first;
270  poseConfidence = iterA->second.second;
271  }
272  else if(stamp >= iterA->first && stamp <= iterB->first)
273  {
274  pose = iterA->second.first.interpolate((stamp-iterA->first) / (iterB->first-iterA->first), iterB->second.first);
275  poseConfidence = iterA->second.second;
276  }
277  else
278  {
279  if(!imuGlobalSyncWarningShown_)
280  {
281  if(stamp < iterA->first)
282  {
283  UWARN("Could not find pose data to interpolate at image time %f (earliest is %f). Are sensors synchronized?", stamp/1000.0, iterA->first/1000.0);
284  }
285  else
286  {
287  UWARN("Could not find pose data to interpolate at image time %f (between %f and %f). Are sensors synchronized?", stamp/1000.0, iterA->first/1000.0, iterB->first/1000.0);
288  }
289  }
290  if(!globalTimeSync_)
291  {
292  if(!imuGlobalSyncWarningShown_)
293  {
294  UWARN("As globalTimeSync option is off, the received pose, gyro and accelerometer will be re-stamped with image time. This message is only shown once.");
295  imuGlobalSyncWarningShown_ = true;
296  }
297  std::map<double, std::pair<Transform, unsigned int> >::const_reverse_iterator iterC = poseBuffer_.rbegin();
298  pose = iterC->second.first;
299  poseConfidence = iterC->second.second;
300  }
301  }
302  }
303  poseMutex_.unlock();
304  }
305 
306  if(accBuffer_.empty() || gyroBuffer_.empty())
307  {
308  return;
309  }
310 
311  // Interpolate acc
312  cv::Vec3d acc;
313  {
314  imuMutex_.lock();
315  if(globalTimeSync_)
316  {
317  int waitTry = 0;
318  while(maxWaitTimeMs > 0 && accBuffer_.rbegin()->first < stamp && waitTry < maxWaitTimeMs)
319  {
320  imuMutex_.unlock();
321  ++waitTry;
322  uSleep(1);
323  imuMutex_.lock();
324  }
325  }
326  if(globalTimeSync_ && accBuffer_.rbegin()->first < stamp)
327  {
328  if(maxWaitTimeMs>0)
329  {
330  UWARN("Could not find acc data to interpolate at image time %f after waiting %d ms (last is %f)...", stamp/1000.0, maxWaitTimeMs, accBuffer_.rbegin()->first/1000.0);
331  }
332  imuMutex_.unlock();
333  return;
334  }
335  else
336  {
337  std::map<double, cv::Vec3f>::const_iterator iterB = accBuffer_.lower_bound(stamp);
338  std::map<double, cv::Vec3f>::const_iterator iterA = iterB;
339  if(iterA != accBuffer_.begin())
340  {
341  iterA = --iterA;
342  }
343  if(iterB == accBuffer_.end())
344  {
345  iterB = --iterB;
346  }
347  if(iterA == iterB && stamp == iterA->first)
348  {
349  acc[0] = iterA->second[0];
350  acc[1] = iterA->second[1];
351  acc[2] = iterA->second[2];
352  }
353  else if(stamp >= iterA->first && stamp <= iterB->first)
354  {
355  float t = (stamp-iterA->first) / (iterB->first-iterA->first);
356  acc[0] = iterA->second[0] + t*(iterB->second[0] - iterA->second[0]);
357  acc[1] = iterA->second[1] + t*(iterB->second[1] - iterA->second[1]);
358  acc[2] = iterA->second[2] + t*(iterB->second[2] - iterA->second[2]);
359  }
360  else
361  {
362  if(!imuGlobalSyncWarningShown_)
363  {
364  if(stamp < iterA->first)
365  {
366  UWARN("Could not find acc data to interpolate at image time %f (earliest is %f). Are sensors synchronized?", stamp/1000.0, iterA->first/1000.0);
367  }
368  else
369  {
370  UWARN("Could not find acc data to interpolate at image time %f (between %f and %f). Are sensors synchronized?", stamp/1000.0, iterA->first/1000.0, iterB->first/1000.0);
371  }
372  }
373  if(!globalTimeSync_)
374  {
375  if(!imuGlobalSyncWarningShown_)
376  {
377  UWARN("As globalTimeSync option is off, the received gyro and accelerometer will be re-stamped with image time. This message is only shown once.");
378  imuGlobalSyncWarningShown_ = true;
379  }
380  std::map<double, cv::Vec3f>::const_reverse_iterator iterC = accBuffer_.rbegin();
381  acc[0] = iterC->second[0];
382  acc[1] = iterC->second[1];
383  acc[2] = iterC->second[2];
384  }
385  else
386  {
387  imuMutex_.unlock();
388  return;
389  }
390  }
391  }
392  imuMutex_.unlock();
393  }
394 
395  // Interpolate gyro
396  cv::Vec3d gyro;
397  {
398  imuMutex_.lock();
399  if(globalTimeSync_)
400  {
401  int waitTry = 0;
402  while(maxWaitTimeMs>0 && gyroBuffer_.rbegin()->first < stamp && waitTry < maxWaitTimeMs)
403  {
404  imuMutex_.unlock();
405  ++waitTry;
406  uSleep(1);
407  imuMutex_.lock();
408  }
409  }
410  if(globalTimeSync_ && gyroBuffer_.rbegin()->first < stamp)
411  {
412  if(maxWaitTimeMs>0)
413  {
414  UWARN("Could not find gyro data to interpolate at image time %f after waiting %d ms (last is %f)...", stamp/1000.0, maxWaitTimeMs, gyroBuffer_.rbegin()->first/1000.0);
415  }
416  imuMutex_.unlock();
417  return;
418  }
419  else
420  {
421  std::map<double, cv::Vec3f>::const_iterator iterB = gyroBuffer_.lower_bound(stamp);
422  std::map<double, cv::Vec3f>::const_iterator iterA = iterB;
423  if(iterA != gyroBuffer_.begin())
424  {
425  iterA = --iterA;
426  }
427  if(iterB == gyroBuffer_.end())
428  {
429  iterB = --iterB;
430  }
431  if(iterA == iterB && stamp == iterA->first)
432  {
433  gyro[0] = iterA->second[0];
434  gyro[1] = iterA->second[1];
435  gyro[2] = iterA->second[2];
436  }
437  else if(stamp >= iterA->first && stamp <= iterB->first)
438  {
439  float t = (stamp-iterA->first) / (iterB->first-iterA->first);
440  gyro[0] = iterA->second[0] + t*(iterB->second[0] - iterA->second[0]);
441  gyro[1] = iterA->second[1] + t*(iterB->second[1] - iterA->second[1]);
442  gyro[2] = iterA->second[2] + t*(iterB->second[2] - iterA->second[2]);
443  }
444  else
445  {
446  if(!imuGlobalSyncWarningShown_)
447  {
448  if(stamp < iterA->first)
449  {
450  UWARN("Could not find gyro data to interpolate at image time %f (earliest is %f). Are sensors synchronized?", stamp/1000.0, iterA->first/1000.0);
451  }
452  else
453  {
454  UWARN("Could not find gyro data to interpolate at image time %f (between %f and %f). Are sensors synchronized?", stamp/1000.0, iterA->first/1000.0, iterB->first/1000.0);
455  }
456  }
457  if(!globalTimeSync_)
458  {
459  if(!imuGlobalSyncWarningShown_)
460  {
461  UWARN("As globalTimeSync option is off, the latest received gyro and accelerometer will be re-stamped with image time. This message is only shown once.");
462  imuGlobalSyncWarningShown_ = true;
463  }
464  std::map<double, cv::Vec3f>::const_reverse_iterator iterC = gyroBuffer_.rbegin();
465  gyro[0] = iterC->second[0];
466  gyro[1] = iterC->second[1];
467  gyro[2] = iterC->second[2];
468  }
469  else
470  {
471  imuMutex_.unlock();
472  return;
473  }
474  }
475  }
476  imuMutex_.unlock();
477  }
478 
479  imu = IMU(gyro, cv::Mat::eye(3, 3, CV_64FC1), acc, cv::Mat::eye(3, 3, CV_64FC1), imuLocalTransform_);
480 }
481 #endif
482 
483 bool CameraRealSense2::init(const std::string & calibrationFolder, const std::string & cameraName)
484 {
485  UDEBUG("");
486 #ifdef RTABMAP_REALSENSE2
487 
488  UINFO("setupDevice...");
489 
490  close();
491 
492  clockSyncWarningShown_ = false;
493  imuGlobalSyncWarningShown_ = false;
494 
495  rs2::device_list list = ctx_.query_devices();
496  if (0 == list.size())
497  {
498  UERROR("No RealSense2 devices were found!");
499  return false;
500  }
501 
502  bool found=false;
503  try
504  {
505  for (rs2::device dev : list)
506  {
507  auto sn = dev.get_info(RS2_CAMERA_INFO_SERIAL_NUMBER);
508  auto pid_str = dev.get_info(RS2_CAMERA_INFO_PRODUCT_ID);
509  auto name = dev.get_info(RS2_CAMERA_INFO_NAME);
510 
511  uint16_t pid;
512  std::stringstream ss;
513  ss << std::hex << pid_str;
514  ss >> pid;
515  UINFO("Device \"%s\" with serial number %s was found with product ID=%d.", name, sn, (int)pid);
516  if(dualMode_ && pid == 0x0B37)
517  {
518  // Dual setup: device[0] = D400, device[1] = T265
519  // T265
520  dev_.resize(2);
521  dev_[1] = dev;
522  }
523  else if (!found && (deviceId_.empty() || deviceId_ == sn || uStrContains(name, uToUpperCase(deviceId_))))
524  {
525  if(dev_.empty())
526  {
527  dev_.resize(1);
528  }
529  dev_[0] = dev;
530  found=true;
531  }
532  }
533  }
534  catch(const rs2::error & error)
535  {
536  UWARN("%s. Is the camera already used with another app?", error.what());
537  }
538 
539  if (!found)
540  {
541  if(dualMode_ && dev_.size()==2)
542  {
543  UERROR("Dual setup is enabled, but a D400 camera is not detected!");
544  dev_.clear();
545  }
546  else
547  {
548  UERROR("The requested device \"%s\" is NOT found!", deviceId_.c_str());
549  }
550  return false;
551  }
552  else if(dualMode_ && dev_.size()!=2)
553  {
554  UERROR("Dual setup is enabled, but a T265 camera is not detected!");
555  dev_.clear();
556  return false;
557  }
558 
559  UASSERT(!dev_.empty());
560 
561  if (!jsonConfig_.empty())
562  {
563  if (dev_[0].is<rs400::advanced_mode>())
564  {
565  std::stringstream ss;
566  std::ifstream in(jsonConfig_);
567  if (in.is_open())
568  {
569  ss << in.rdbuf();
570  std::string json_file_content = ss.str();
571 
572  auto adv = dev_[0].as<rs400::advanced_mode>();
573  adv.load_json(json_file_content);
574  UINFO("JSON file is loaded! (%s)", jsonConfig_.c_str());
575  }
576  else
577  {
578  UWARN("JSON file provided doesn't exist! (%s)", jsonConfig_.c_str());
579  }
580  }
581  else
582  {
583  UWARN("A json config file is provided (%s), but device does not support advanced settings!", jsonConfig_.c_str());
584  }
585  }
586 
587  ctx_.set_devices_changed_callback([this](rs2::event_information& info)
588  {
589  for(size_t i=0; i<dev_.size(); ++i)
590  {
591  if (info.was_removed(dev_[i]))
592  {
593  if (closing_)
594  {
595  UDEBUG("The device %d has been disconnected!", i);
596  }
597  else
598  {
599  UERROR("The device %d has been disconnected!", i);
600  }
601  }
602  }
603  });
604 
605  auto sn = dev_[0].get_info(RS2_CAMERA_INFO_SERIAL_NUMBER);
606  UINFO("Using device with Serial No: %s", sn);
607 
608  auto camera_name = dev_[0].get_info(RS2_CAMERA_INFO_NAME);
609  UINFO("Device Name: %s", camera_name);
610 
611  auto fw_ver = dev_[0].get_info(RS2_CAMERA_INFO_FIRMWARE_VERSION);
612  UINFO("Device FW version: %s", fw_ver);
613 
614  auto pid = dev_[0].get_info(RS2_CAMERA_INFO_PRODUCT_ID);
615  UINFO("Device Product ID: 0x%s", pid);
616 
617  auto dev_sensors = dev_[0].query_sensors();
618  if(dualMode_)
619  {
620  UASSERT(dev_.size()>1);
621  auto dev_sensors2 = dev_[1].query_sensors();
622  dev_sensors.insert(dev_sensors.end(), dev_sensors2.begin(), dev_sensors2.end());
623  }
624 
625  UINFO("Device Sensors: ");
626  std::vector<rs2::sensor> sensors(2); //0=rgb 1=depth 2=(pose in dualMode_)
627  bool stereo = false;
628  bool isL500 = false;
629  for(auto&& elem : dev_sensors)
630  {
631  std::string module_name = elem.get_info(RS2_CAMERA_INFO_NAME);
632  if ("Stereo Module" == module_name)
633  {
634  sensors[1] = elem;
635  if(sensors[1].supports(rs2_option::RS2_OPTION_EMITTER_ENABLED))
636  {
637  sensors[1].set_option(rs2_option::RS2_OPTION_EMITTER_ENABLED, emitterEnabled_);
638  }
639  }
640  else if ("Coded-Light Depth Sensor" == module_name)
641  {
642  }
643  else if ("RGB Camera" == module_name)
644  {
645  if(!ir_)
646  {
647  sensors[0] = elem;
648  }
649  }
650  else if ("Wide FOV Camera" == module_name)
651  {
652  }
653  else if ("Motion Module" == module_name)
654  {
655  if(!dualMode_)
656  {
657  sensors.resize(3);
658  sensors[2] = elem;
659  }
660  }
661  else if ("Tracking Module" == module_name)
662  {
663  if(dualMode_)
664  {
665  sensors.resize(3);
666  }
667  else
668  {
669  sensors.resize(1);
670  stereo = true;
671  }
672  sensors.back() = elem;
673  sensors.back().set_option(rs2_option::RS2_OPTION_ENABLE_POSE_JUMPING, 0);
674  sensors.back().set_option(rs2_option::RS2_OPTION_ENABLE_RELOCALIZATION, 0);
675  }
676  else if ("L500 Depth Sensor" == module_name)
677  {
678  sensors[1] = elem;
679  isL500 = true;
680  if(ir_)
681  {
682  cameraWidth_ = cameraDepthWidth_;
683  cameraHeight_ = cameraDepthHeight_;
684  cameraFps_ = cameraDepthFps_;
685  }
686  irDepth_ = true;
687  }
688  else
689  {
690  UERROR("Module Name \"%s\" isn't supported!", module_name.c_str());
691  return false;
692  }
693  UINFO("%s was found.", elem.get_info(RS2_CAMERA_INFO_NAME));
694  }
695 
696  UDEBUG("");
697 
698  model_ = CameraModel();
699  std::vector<std::vector<rs2::stream_profile> > profilesPerSensor(sensors.size());
700  for (unsigned int i=0; i<sensors.size(); ++i)
701  {
702  if(i==0 && ir_ && !stereo)
703  {
704  continue;
705  }
706  UINFO("Sensor %d \"%s\"", (int)i, sensors[i].get_info(RS2_CAMERA_INFO_NAME));
707  auto profiles = sensors[i].get_stream_profiles();
708  bool added = false;
709  UINFO("profiles=%d", (int)profiles.size());
710  if(ULogger::level()<ULogger::kWarning)
711  {
712  for (auto& profile : profiles)
713  {
714  auto video_profile = profile.as<rs2::video_stream_profile>();
715  UINFO("%s %d %d %d %d %s type=%d", rs2_format_to_string(
716  video_profile.format()),
717  video_profile.width(),
718  video_profile.height(),
719  video_profile.fps(),
720  video_profile.stream_index(),
721  video_profile.stream_name().c_str(),
722  video_profile.stream_type());
723  }
724  }
725  int pi = 0;
726  for (auto& profile : profiles)
727  {
728  auto video_profile = profile.as<rs2::video_stream_profile>();
729  if(!stereo)
730  {
731  if( (video_profile.width() == cameraWidth_ &&
732  video_profile.height() == cameraHeight_ &&
733  video_profile.fps() == cameraFps_) ||
734  (strcmp(sensors[i].get_info(RS2_CAMERA_INFO_NAME), "L500 Depth Sensor")==0 &&
735  video_profile.width() == cameraDepthWidth_ &&
736  video_profile.height() == cameraDepthHeight_ &&
737  video_profile.fps() == cameraDepthFps_))
738  {
739  auto intrinsic = video_profile.get_intrinsics();
740 
741  // rgb or ir left
742  if((!ir_ && video_profile.format() == RS2_FORMAT_RGB8 && video_profile.stream_type() == RS2_STREAM_COLOR) ||
743  (ir_ && video_profile.format() == RS2_FORMAT_Y8 && (video_profile.stream_index() == 1 || isL500)))
744  {
745  if(!profilesPerSensor[i].empty())
746  {
747  // IR right already there, push ir left front
748  profilesPerSensor[i].push_back(profilesPerSensor[i].back());
749  profilesPerSensor[i].front() = profile;
750  }
751  else
752  {
753  profilesPerSensor[i].push_back(profile);
754  }
755  rgbBuffer_ = cv::Mat(cv::Size(cameraWidth_, cameraHeight_), video_profile.format() == RS2_FORMAT_Y8?CV_8UC1:CV_8UC3, ir_?cv::Scalar(0):cv::Scalar(0, 0, 0));
756  model_ = CameraModel(camera_name, intrinsic.fx, intrinsic.fy, intrinsic.ppx, intrinsic.ppy, this->getLocalTransform(), 0, cv::Size(intrinsic.width, intrinsic.height));
757  UINFO("Model: %dx%d fx=%f fy=%f cx=%f cy=%f dist model=%d coeff=%f %f %f %f %f",
758  intrinsic.width, intrinsic.height,
759  intrinsic.fx, intrinsic.fy, intrinsic.ppx, intrinsic.ppy,
760  intrinsic.model,
761  intrinsic.coeffs[0], intrinsic.coeffs[1], intrinsic.coeffs[2], intrinsic.coeffs[3], intrinsic.coeffs[4]);
762  added = true;
763  if(video_profile.format() == RS2_FORMAT_RGB8 || profilesPerSensor[i].size()==2)
764  {
765  break;
766  }
767  }
768  // depth or ir right
769  else if(((!ir_ || irDepth_) && video_profile.format() == RS2_FORMAT_Z16) ||
770  (ir_ && !irDepth_ && video_profile.format() == RS2_FORMAT_Y8 && video_profile.stream_index() == 2))
771  {
772  profilesPerSensor[i].push_back(profile);
773  depthBuffer_ = cv::Mat(cv::Size(cameraWidth_, cameraHeight_), video_profile.format() == RS2_FORMAT_Y8?CV_8UC1:CV_16UC1, cv::Scalar(0));
774  added = true;
775  if(!ir_ || irDepth_ || profilesPerSensor[i].size()==2)
776  {
777  break;
778  }
779  }
780  }
781  else if(video_profile.format() == RS2_FORMAT_MOTION_XYZ32F || video_profile.format() == RS2_FORMAT_6DOF)
782  {
783  //D435i:
784  //MOTION_XYZ32F 0 0 200 (gyro)
785  //MOTION_XYZ32F 0 0 400 (gyro)
786  //MOTION_XYZ32F 0 0 63 6 (accel)
787  //MOTION_XYZ32F 0 0 250 6 (accel)
788  // or dualMode_ T265:
789  //MOTION_XYZ32F 0 0 200 5 (gyro)
790  //MOTION_XYZ32F 0 0 62 6 (accel)
791  //6DOF 0 0 200 4 (pose)
792  bool modified = false;
793  for (size_t j= 0; j < profilesPerSensor[i].size(); ++j)
794  {
795  if (profilesPerSensor[i][j].stream_type() == profile.stream_type())
796  {
797  if (profile.stream_type() == RS2_STREAM_ACCEL)
798  {
799  if(profile.fps() > profilesPerSensor[i][j].fps())
800  profilesPerSensor[i][j] = profile;
801  modified = true;
802  }
803  else if (profile.stream_type() == RS2_STREAM_GYRO)
804  {
805  if(profile.fps() < profilesPerSensor[i][j].fps())
806  profilesPerSensor[i][j] = profile;
807  modified = true;
808  }
809  }
810  }
811  if(!modified)
812  profilesPerSensor[i].push_back(profile);
813  added = true;
814  }
815  }
816  else if(stereo || dualMode_)
817  {
818  //T265:
819  if(!dualMode_ &&
820  video_profile.format() == RS2_FORMAT_Y8 &&
821  video_profile.width() == 848 &&
822  video_profile.height() == 800 &&
823  video_profile.fps() == 30)
824  {
825  UASSERT(i<2);
826  profilesPerSensor[i].push_back(profile);
827  if(pi==0)
828  {
829  // LEFT FISHEYE
830  rgbBuffer_ = cv::Mat(cv::Size(848, 800), CV_8UC1, cv::Scalar(0));
831  if(odometryOnlyLeftStream_)
832  {
833  auto intrinsic = video_profile.get_intrinsics();
834  UINFO("Model: %dx%d fx=%f fy=%f cx=%f cy=%f dist model=%d coeff=%f %f %f %f",
835  intrinsic.width, intrinsic.height,
836  intrinsic.fx, intrinsic.fy, intrinsic.ppx, intrinsic.ppy,
837  intrinsic.model,
838  intrinsic.coeffs[0], intrinsic.coeffs[1], intrinsic.coeffs[2], intrinsic.coeffs[3]);
839  cv::Mat K = cv::Mat::eye(3,3,CV_64FC1);
840  K.at<double>(0,0) = intrinsic.fx;
841  K.at<double>(1,1) = intrinsic.fy;
842  K.at<double>(0,2) = intrinsic.ppx;
843  K.at<double>(1,2) = intrinsic.ppy;
844  UASSERT(intrinsic.model == RS2_DISTORTION_KANNALA_BRANDT4); // we expect fisheye 4 values
845  cv::Mat D = cv::Mat::zeros(1,6,CV_64FC1);
846  D.at<double>(0,0) = intrinsic.coeffs[0];
847  D.at<double>(0,1) = intrinsic.coeffs[1];
848  D.at<double>(0,4) = intrinsic.coeffs[2];
849  D.at<double>(0,5) = intrinsic.coeffs[3];
850  cv::Mat P = cv::Mat::eye(3, 4, CV_64FC1);
851  P.at<double>(0,0) = intrinsic.fx;
852  P.at<double>(1,1) = intrinsic.fy;
853  P.at<double>(0,2) = intrinsic.ppx;
854  P.at<double>(1,2) = intrinsic.ppy;
855  cv::Mat R = cv::Mat::eye(3, 3, CV_64FC1);
856  model_ = CameraModel(camera_name, cv::Size(intrinsic.width, intrinsic.height), K, D, R, P, this->getLocalTransform());
857  if(rectifyImages_)
858  model_.initRectificationMap();
859  }
860  }
861  else if(!odometryOnlyLeftStream_)
862  {
863  // RIGHT FISHEYE
864  depthBuffer_ = cv::Mat(cv::Size(848, 800), CV_8UC1, cv::Scalar(0));
865  }
866  added = true;
867  }
868  else if(video_profile.format() == RS2_FORMAT_MOTION_XYZ32F || video_profile.format() == RS2_FORMAT_6DOF)
869  {
870  //MOTION_XYZ32F 0 0 200
871  //MOTION_XYZ32F 0 0 62
872  //6DOF 0 0 200
873  profilesPerSensor[i].push_back(profile);
874  added = true;
875  }
876  }
877  ++pi;
878  }
879  if (!added)
880  {
881  UERROR("Given stream configuration is not supported by the device! "
882  "Stream Index: %d, Width: %d, Height: %d, FPS: %d", i, cameraWidth_, cameraHeight_, cameraFps_);
883  UERROR("Available configurations:");
884  for (auto& profile : profiles)
885  {
886  auto video_profile = profile.as<rs2::video_stream_profile>();
887  UERROR("%s %d %d %d %d %s type=%d", rs2_format_to_string(
888  video_profile.format()),
889  video_profile.width(),
890  video_profile.height(),
891  video_profile.fps(),
892  video_profile.stream_index(),
893  video_profile.stream_name().c_str(),
894  video_profile.stream_type());
895  }
896  return false;
897  }
898  }
899  UDEBUG("");
900  if(!stereo)
901  {
902  if(!model_.isValidForProjection())
903  {
904  UERROR("Calibration info not valid!");
905  std::cout<< model_ << std::endl;
906  return false;
907  }
908 
909  if(dualMode_)
910  {
911  UINFO("Set base to pose");
912  this->setLocalTransform(this->getLocalTransform()*CameraModel::opticalRotation().inverse());
913  UINFO("dualExtrinsics_ = %s", dualExtrinsics_.prettyPrint().c_str());
914  Transform baseToCam = this->getLocalTransform()*dualExtrinsics_;
915  UASSERT(profilesPerSensor.size()>=2);
916  UASSERT(profilesPerSensor.back().size() == 3);
917  rs2_extrinsics poseToIMU = profilesPerSensor.back()[0].get_extrinsics_to(profilesPerSensor.back()[2]);
918 
919  Transform poseToIMUT(
920  poseToIMU.rotation[0], poseToIMU.rotation[1], poseToIMU.rotation[2], poseToIMU.translation[0],
921  poseToIMU.rotation[3], poseToIMU.rotation[4], poseToIMU.rotation[5], poseToIMU.translation[1],
922  poseToIMU.rotation[6], poseToIMU.rotation[7], poseToIMU.rotation[8], poseToIMU.translation[2]);
923  poseToIMUT = realsense2PoseRotation_ * poseToIMUT;
924  UINFO("poseToIMU = %s", poseToIMUT.prettyPrint().c_str());
925 
926  UINFO("BaseToCam = %s", baseToCam.prettyPrint().c_str());
927  model_.setLocalTransform(baseToCam);
928  imuLocalTransform_ = this->getLocalTransform() * poseToIMUT;
929  }
930 
931  if(ir_ && !irDepth_ && profilesPerSensor.size() >= 2 && profilesPerSensor[1].size() >= 2)
932  {
933  rs2_extrinsics leftToRight = profilesPerSensor[1][1].get_extrinsics_to(profilesPerSensor[1][0]);
934  Transform leftToRightT(
935  leftToRight.rotation[0], leftToRight.rotation[1], leftToRight.rotation[2], leftToRight.translation[0],
936  leftToRight.rotation[3], leftToRight.rotation[4], leftToRight.rotation[5], leftToRight.translation[1],
937  leftToRight.rotation[6], leftToRight.rotation[7], leftToRight.rotation[8], leftToRight.translation[2]);
938 
939  UINFO("left to right transform = %s", leftToRightT.prettyPrint().c_str());
940 
941  // Create stereo camera model from left and right ir of D435
942  stereoModel_ = StereoCameraModel(model_.fx(), model_.fy(), model_.cx(), model_.cy(), leftToRightT.x(), model_.localTransform(), model_.imageSize());
943  UINFO("Stereo parameters: fx=%f cx=%f cy=%f baseline=%f",
944  stereoModel_.left().fx(),
945  stereoModel_.left().cx(),
946  stereoModel_.left().cy(),
947  stereoModel_.baseline());
948  }
949 
950  if(!dualMode_ && profilesPerSensor.size() == 3)
951  {
952  if(!profilesPerSensor[2].empty() && !profilesPerSensor[0].empty())
953  {
954  rs2_extrinsics leftToIMU = profilesPerSensor[2][0].get_extrinsics_to(profilesPerSensor[0][0]);
955  Transform leftToIMUT(
956  leftToIMU.rotation[0], leftToIMU.rotation[1], leftToIMU.rotation[2], leftToIMU.translation[0],
957  leftToIMU.rotation[3], leftToIMU.rotation[4], leftToIMU.rotation[5], leftToIMU.translation[1],
958  leftToIMU.rotation[6], leftToIMU.rotation[7], leftToIMU.rotation[8], leftToIMU.translation[2]);
959  imuLocalTransform_ = this->getLocalTransform() * leftToIMUT;
960  UINFO("imu local transform = %s", imuLocalTransform_.prettyPrint().c_str());
961  }
962  else if(!profilesPerSensor[2].empty() && !profilesPerSensor[1].empty())
963  {
964  rs2_extrinsics leftToIMU = profilesPerSensor[2][0].get_extrinsics_to(profilesPerSensor[1][0]);
965  Transform leftToIMUT(
966  leftToIMU.rotation[0], leftToIMU.rotation[1], leftToIMU.rotation[2], leftToIMU.translation[0],
967  leftToIMU.rotation[3], leftToIMU.rotation[4], leftToIMU.rotation[5], leftToIMU.translation[1],
968  leftToIMU.rotation[6], leftToIMU.rotation[7], leftToIMU.rotation[8], leftToIMU.translation[2]);
969 
970  imuLocalTransform_ = this->getLocalTransform() * leftToIMUT;
971  UINFO("imu local transform = %s", imuLocalTransform_.prettyPrint().c_str());
972  }
973  }
974  }
975  else // T265
976  {
977 
978  // look for calibration files
979  std::string serial = sn;
980  if(!cameraName.empty())
981  {
982  serial = cameraName;
983  }
984  if(!odometryImagesDisabled_ &&
985  !odometryOnlyLeftStream_ &&
986  !calibrationFolder.empty() && !serial.empty())
987  {
988  if(!stereoModel_.load(calibrationFolder, serial, false))
989  {
990  UWARN("Missing calibration files for camera \"%s\" in \"%s\" folder, you should calibrate the camera!",
991  serial.c_str(), calibrationFolder.c_str());
992  }
993  else
994  {
995  UINFO("Stereo parameters: fx=%f cx=%f cy=%f baseline=%f",
996  stereoModel_.left().fx(),
997  stereoModel_.left().cx(),
998  stereoModel_.left().cy(),
999  stereoModel_.baseline());
1000  }
1001  }
1002 
1003  // Get extrinsics with pose as the base frame:
1004  // 0=Left fisheye
1005  // 1=Right fisheye
1006  // 2=GYRO
1007  // 3=ACC
1008  // 4=POSE
1009  UASSERT(profilesPerSensor[0].size() == 5);
1010  if(odometryProvided_)
1011  {
1012  rs2_extrinsics poseToLeft = profilesPerSensor[0][0].get_extrinsics_to(profilesPerSensor[0][4]);
1013  rs2_extrinsics poseToIMU = profilesPerSensor[0][2].get_extrinsics_to(profilesPerSensor[0][4]);
1014  Transform poseToLeftT(
1015  poseToLeft.rotation[0], poseToLeft.rotation[1], poseToLeft.rotation[2], poseToLeft.translation[0],
1016  poseToLeft.rotation[3], poseToLeft.rotation[4], poseToLeft.rotation[5], poseToLeft.translation[1],
1017  poseToLeft.rotation[6], poseToLeft.rotation[7], poseToLeft.rotation[8], poseToLeft.translation[2]);
1018  poseToLeftT = realsense2PoseRotation_ * poseToLeftT;
1019  UINFO("poseToLeft = %s", poseToLeftT.prettyPrint().c_str());
1020 
1021  Transform poseToIMUT(
1022  poseToIMU.rotation[0], poseToIMU.rotation[1], poseToIMU.rotation[2], poseToIMU.translation[0],
1023  poseToIMU.rotation[3], poseToIMU.rotation[4], poseToIMU.rotation[5], poseToIMU.translation[1],
1024  poseToIMU.rotation[6], poseToIMU.rotation[7], poseToIMU.rotation[8], poseToIMU.translation[2]);
1025  poseToIMUT = realsense2PoseRotation_ * poseToIMUT;
1026  UINFO("poseToIMU = %s", poseToIMUT.prettyPrint().c_str());
1027 
1028  Transform opticalTransform(0, 0, 1, 0, -1, 0, 0, 0, 0, -1, 0, 0);
1029  this->setLocalTransform(this->getLocalTransform() * opticalTransform.inverse());
1030  if(odometryOnlyLeftStream_)
1031  model_.setLocalTransform(this->getLocalTransform()*poseToLeftT);
1032  else
1033  stereoModel_.setLocalTransform(this->getLocalTransform()*poseToLeftT);
1034  imuLocalTransform_ = this->getLocalTransform()* poseToIMUT;
1035 
1036  if(odometryImagesDisabled_)
1037  {
1038  // keep only pose stream
1039  std::vector<rs2::stream_profile> profiles;
1040  profiles.push_back(profilesPerSensor[0][4]);
1041  profilesPerSensor[0] = profiles;
1042  }
1043  }
1044  else
1045  {
1046  // Set imu transform based on the left camera instead of pose
1047  rs2_extrinsics leftToIMU = profilesPerSensor[0][2].get_extrinsics_to(profilesPerSensor[0][0]);
1048  Transform leftToIMUT(
1049  leftToIMU.rotation[0], leftToIMU.rotation[1], leftToIMU.rotation[2], leftToIMU.translation[0],
1050  leftToIMU.rotation[3], leftToIMU.rotation[4], leftToIMU.rotation[5], leftToIMU.translation[1],
1051  leftToIMU.rotation[6], leftToIMU.rotation[7], leftToIMU.rotation[8], leftToIMU.translation[2]);
1052  UINFO("leftToIMU = %s", leftToIMUT.prettyPrint().c_str());
1053  imuLocalTransform_ = this->getLocalTransform() * leftToIMUT;
1054  UINFO("imu local transform = %s", imuLocalTransform_.prettyPrint().c_str());
1055  if(odometryOnlyLeftStream_)
1056  model_.setLocalTransform(this->getLocalTransform());
1057  else
1058  stereoModel_.setLocalTransform(this->getLocalTransform());
1059  }
1060  if(!odometryImagesDisabled_ && rectifyImages_ && !model_.isValidForRectification() && !stereoModel_.isValidForRectification())
1061  {
1062  UERROR("Parameter \"rectifyImages\" is set, but no stereo model is loaded or valid.");
1063  return false;
1064  }
1065  }
1066 
1067  std::function<void(rs2::frame)> multiple_message_callback_function = [this](rs2::frame frame){multiple_message_callback(frame);};
1068 
1069  for (unsigned int i=0; i<sensors.size(); ++i)
1070  {
1071  if(profilesPerSensor[i].size())
1072  {
1073  UINFO("Starting sensor %d with %d profiles", (int)i, (int)profilesPerSensor[i].size());
1074  for (size_t j = 0; j < profilesPerSensor[i].size(); ++j)
1075  {
1076  auto video_profile = profilesPerSensor[i][j].as<rs2::video_stream_profile>();
1077  UINFO("Opening: %s %d %d %d %d %s type=%d", rs2_format_to_string(
1078  video_profile.format()),
1079  video_profile.width(),
1080  video_profile.height(),
1081  video_profile.fps(),
1082  video_profile.stream_index(),
1083  video_profile.stream_name().c_str(),
1084  video_profile.stream_type());
1085  }
1086  if(globalTimeSync_ && sensors[i].supports(rs2_option::RS2_OPTION_GLOBAL_TIME_ENABLED))
1087  {
1088  float value = sensors[i].get_option(rs2_option::RS2_OPTION_GLOBAL_TIME_ENABLED);
1089  UINFO("Set RS2_OPTION_GLOBAL_TIME_ENABLED=1 (was %f) for sensor %d", value, (int)i);
1090  sensors[i].set_option(rs2_option::RS2_OPTION_GLOBAL_TIME_ENABLED, 1);
1091  }
1092  sensors[i].open(profilesPerSensor[i]);
1093  if(sensors[i].is<rs2::depth_sensor>())
1094  {
1095  auto depth_sensor = sensors[i].as<rs2::depth_sensor>();
1096  depth_scale_meters_ = depth_sensor.get_depth_scale();
1097  UINFO("Depth scale %f for sensor %d", depth_scale_meters_, (int)i);
1098  }
1099  sensors[i].start(multiple_message_callback_function);
1100  }
1101  }
1102 
1103  uSleep(1000); // ignore the first frames
1104  UINFO("Enabling streams...done!");
1105 
1106  return true;
1107 
1108 #else
1109  UERROR("CameraRealSense: RTAB-Map is not built with RealSense2 support!");
1110  return false;
1111 #endif
1112 }
1113 
1114 bool CameraRealSense2::isCalibrated() const
1115 {
1116 #ifdef RTABMAP_REALSENSE2
1117  return model_.isValidForProjection() || stereoModel_.isValidForRectification();
1118 #else
1119  return false;
1120 #endif
1121 }
1122 
1123 std::string CameraRealSense2::getSerial() const
1124 {
1125 #ifdef RTABMAP_REALSENSE2
1126  if(!dev_.empty())
1127  {
1128  return dev_[0].get_info(RS2_CAMERA_INFO_SERIAL_NUMBER);
1129  }
1130 #endif
1131  return "NA";
1132 }
1133 
1134 bool CameraRealSense2::odomProvided() const
1135 {
1136 #ifdef RTABMAP_REALSENSE2
1137  return odometryProvided_;
1138 #else
1139  return false;
1140 #endif
1141 }
1142 
1143 bool CameraRealSense2::getPose(double stamp, Transform & pose, cv::Mat & covariance, double maxWaitTime)
1144 {
1145 #ifdef RTABMAP_REALSENSE2
1146  IMU imu;
1147  unsigned int confidence = 0;
1148  double rsStamp = stamp*1000.0;
1149  Transform p;
1150  getPoseAndIMU(rsStamp, p, confidence, imu, maxWaitTime*1000);
1151 
1152  if(!p.isNull())
1153  {
1154  // Transform in base frame
1155  pose = this->getLocalTransform() * p * this->getLocalTransform().inverse();
1156 
1157  covariance = cv::Mat::eye(6,6,CV_64FC1) * 0.0001;
1158  covariance.rowRange(0,3) *= pow(10, 3-(int)confidence);
1159  covariance.rowRange(3,6) *= pow(10, 1-(int)confidence);
1160  return true;
1161  }
1162 #endif
1163  return false;
1164 }
1165 
1166 void CameraRealSense2::setEmitterEnabled(bool enabled)
1167 {
1168 #ifdef RTABMAP_REALSENSE2
1169  emitterEnabled_ = enabled;
1170 #endif
1171 }
1172 
1173 void CameraRealSense2::setIRFormat(bool enabled, bool useDepthInsteadOfRightImage)
1174 {
1175 #ifdef RTABMAP_REALSENSE2
1176  ir_ = enabled;
1177  irDepth_ = useDepthInsteadOfRightImage;
1178 #endif
1179 }
1180 
1181 void CameraRealSense2::setResolution(int width, int height, int fps)
1182 {
1183 #ifdef RTABMAP_REALSENSE2
1184  cameraWidth_ = width;
1185  cameraHeight_ = height;
1186  cameraFps_ = fps;
1187 #endif
1188 }
1189 
1190 void CameraRealSense2::setDepthResolution(int width, int height, int fps)
1191 {
1192 #ifdef RTABMAP_REALSENSE2
1193  cameraDepthWidth_ = width;
1194  cameraDepthHeight_ = height;
1195  cameraDepthFps_ = fps;
1196 #endif
1197 }
1198 
1199 void CameraRealSense2::setGlobalTimeSync(bool enabled)
1200 {
1201 #ifdef RTABMAP_REALSENSE2
1202  globalTimeSync_ = enabled;
1203 #endif
1204 }
1205 
1206 void CameraRealSense2::setDualMode(bool enabled, const Transform & extrinsics)
1207 {
1208 #ifdef RTABMAP_REALSENSE2
1209  UASSERT(!enabled || !extrinsics.isNull());
1210  dualMode_ = enabled;
1211  dualExtrinsics_ = extrinsics*CameraModel::opticalRotation();
1212  if(dualMode_)
1213  {
1214  odometryProvided_ = true;
1215  odometryImagesDisabled_ = false;
1216  odometryOnlyLeftStream_ = false;
1217  }
1218 #endif
1219 }
1220 
1221 void CameraRealSense2::setJsonConfig(const std::string & json)
1222 {
1223 #ifdef RTABMAP_REALSENSE2
1224  jsonConfig_ = json;
1225 #endif
1226 }
1227 
1228 void CameraRealSense2::setImagesRectified(bool enabled)
1229 {
1230 #ifdef RTABMAP_REALSENSE2
1231  rectifyImages_ = enabled;
1232 #endif
1233 }
1234 
1235 void CameraRealSense2::setOdomProvided(bool enabled, bool imageStreamsDisabled, bool onlyLeftStream)
1236 {
1237 #ifdef RTABMAP_REALSENSE2
1238  if(dualMode_ && !enabled)
1239  {
1240  UERROR("Odometry is disabled but dual mode was enabled, disabling dual mode.");
1241  dualMode_ = false;
1242  }
1243  odometryProvided_ = enabled;
1244  odometryImagesDisabled_ = enabled && imageStreamsDisabled;
1245  odometryOnlyLeftStream_ = enabled && !imageStreamsDisabled && onlyLeftStream;
1246 #endif
1247 }
1248 
1249 SensorData CameraRealSense2::captureImage(SensorCaptureInfo * info)
1250 {
1251  SensorData data;
1252 #ifdef RTABMAP_REALSENSE2
1253 
1254  try{
1255  auto frameset = syncer_.wait_for_frames(5000);
1256  UTimer timer;
1257  int desiredFramesetSize = 2;
1258  while ((int)frameset.size() != desiredFramesetSize && timer.elapsed() < 2.0)
1259  {
1260  // maybe there is a latency with the USB, try again in 100 ms (for the next 2 seconds)
1261  frameset = syncer_.wait_for_frames(100);
1262  }
1263  if ((int)frameset.size() == desiredFramesetSize)
1264  {
1265  double now = UTimer::now();
1266  bool is_rgb_arrived = false;
1267  bool is_depth_arrived = false;
1268  bool is_left_fisheye_arrived = false;
1269  bool is_right_fisheye_arrived = false;
1270  rs2::frame rgb_frame;
1271  rs2::frame depth_frame;
1272  double stamp = frameset.get_timestamp();
1273  for (auto it = frameset.begin(); it != frameset.end(); ++it)
1274  {
1275  auto f = (*it);
1276  if(stamp > f.get_timestamp())
1277  {
1278  stamp = f.get_timestamp();
1279  }
1280  auto stream_type = f.get_profile().stream_type();
1281  if (stream_type == RS2_STREAM_COLOR || stream_type == RS2_STREAM_INFRARED)
1282  {
1283  if(ir_ && !irDepth_)
1284  {
1285  //stereo D435
1286  if(!is_depth_arrived)
1287  {
1288  depth_frame = f; // right image
1289  is_depth_arrived = true;
1290  }
1291  else
1292  {
1293  rgb_frame = f; // left image
1294  is_rgb_arrived = true;
1295  }
1296  }
1297  else
1298  {
1299  rgb_frame = f;
1300  is_rgb_arrived = true;
1301  }
1302  }
1303  else if (stream_type == RS2_STREAM_DEPTH)
1304  {
1305  depth_frame = f;
1306  is_depth_arrived = true;
1307  }
1308  else if (stream_type == RS2_STREAM_FISHEYE)
1309  {
1310  if(!is_right_fisheye_arrived)
1311  {
1312  depth_frame = f;
1313  is_right_fisheye_arrived = true;
1314  }
1315  else
1316  {
1317  rgb_frame = f;
1318  is_left_fisheye_arrived = true;
1319  }
1320  }
1321  }
1322 
1323  stamp /= 1000.0; // put in seconds
1324  UDEBUG("Frameset arrived. system=%fs frame=%fs", now, stamp);
1325  if(stamp - now > 1000000000.0)
1326  {
1327  if(!clockSyncWarningShown_)
1328  {
1329  UWARN("Clocks are not sync with host computer! Detected stamps in far "
1330  "future %f, thus using host time instead (%f)! This message "
1331  "will only appear once. "
1332  "See https://github.com/IntelRealSense/librealsense/issues/4505 "
1333  "for more info", stamp, now);
1334  clockSyncWarningShown_ = true;
1335  }
1336  stamp = now;
1337  }
1338 
1339  if(is_rgb_arrived && is_depth_arrived)
1340  {
1341  cv::Mat depth;
1342  if(ir_ && !irDepth_)
1343  {
1344  depth = cv::Mat(depthBuffer_.size(), depthBuffer_.type(), (void*)depth_frame.get_data()).clone();
1345  }
1346  else
1347  {
1348  rs2::align align(rgb_frame.get_profile().stream_type());
1349  rs2::frameset processed = frameset.apply_filter(align);
1350  rs2::depth_frame aligned_depth_frame = processed.get_depth_frame();
1351  if(frameset.get_depth_frame().get_width() < aligned_depth_frame.get_width() &&
1352  frameset.get_depth_frame().get_height() < aligned_depth_frame.get_height())
1353  {
1354  int decimationWidth = int(float(aligned_depth_frame.get_width())/float(frameset.get_depth_frame().get_width())+0.5f);
1355  int decimationHeight = int(float(aligned_depth_frame.get_height())/float(frameset.get_depth_frame().get_height())+0.5f);
1356  if(decimationWidth>1 || decimationHeight>1)
1357  {
1358  depth = util2d::decimate(cv::Mat(depthBuffer_.size(), depthBuffer_.type(), (void*)aligned_depth_frame.get_data()), decimationWidth>decimationHeight?decimationWidth:decimationHeight);
1359  }
1360  else
1361  {
1362  depth = cv::Mat(depthBuffer_.size(), depthBuffer_.type(), (void*)aligned_depth_frame.get_data()).clone();
1363  }
1364  }
1365  else
1366  {
1367  depth = cv::Mat(depthBuffer_.size(), depthBuffer_.type(), (void*)aligned_depth_frame.get_data()).clone();
1368  }
1369  if(depth_scale_meters_ != 0.001f)
1370  { // convert to mm
1371  if(depth.type() == CV_16UC1)
1372  {
1373  float scaleMM = depth_scale_meters_ / 0.001f;
1374  depth = scaleMM * depth;
1375  }
1376  }
1377  }
1378 
1379  cv::Mat rgb = cv::Mat(rgbBuffer_.size(), rgbBuffer_.type(), (void*)rgb_frame.get_data());
1380  cv::Mat bgr;
1381  if(rgb.channels() == 3)
1382  {
1383  cv::cvtColor(rgb, bgr, CV_RGB2BGR);
1384  }
1385  else
1386  {
1387  bgr = rgb.clone();
1388  }
1389 
1390  if(ir_ && !irDepth_)
1391  {
1392  //stereo D435
1393  data = SensorData(bgr, depth, stereoModel_, this->getNextSeqID(), stamp);
1394  }
1395  else
1396  {
1397  data = SensorData(bgr, depth, model_, this->getNextSeqID(), stamp);
1398  }
1399  }
1400  else if(is_left_fisheye_arrived)
1401  {
1402  if(odometryOnlyLeftStream_)
1403  {
1404  cv::Mat left;
1405  if(rectifyImages_ && model_.isValidForRectification())
1406  {
1407  left = model_.rectifyImage(cv::Mat(rgbBuffer_.size(), rgbBuffer_.type(), (void*)rgb_frame.get_data()));
1408  }
1409  else
1410  {
1411  left = cv::Mat(rgbBuffer_.size(), rgbBuffer_.type(), (void*)rgb_frame.get_data()).clone();
1412  }
1413 
1414  if(model_.imageHeight() == 0 || model_.imageWidth() == 0)
1415  {
1416  model_.setImageSize(left.size());
1417  }
1418 
1419  data = SensorData(left, cv::Mat(), model_, this->getNextSeqID(), stamp);
1420  }
1421  else if(is_right_fisheye_arrived)
1422  {
1423  cv::Mat left,right;
1424  if(rectifyImages_ && stereoModel_.left().isValidForRectification() && stereoModel_.right().isValidForRectification())
1425  {
1426  left = stereoModel_.left().rectifyImage(cv::Mat(rgbBuffer_.size(), rgbBuffer_.type(), (void*)rgb_frame.get_data()));
1427  right = stereoModel_.right().rectifyImage(cv::Mat(depthBuffer_.size(), depthBuffer_.type(), (void*)depth_frame.get_data()));
1428  }
1429  else
1430  {
1431  left = cv::Mat(rgbBuffer_.size(), rgbBuffer_.type(), (void*)rgb_frame.get_data()).clone();
1432  right = cv::Mat(depthBuffer_.size(), depthBuffer_.type(), (void*)depth_frame.get_data()).clone();
1433  }
1434 
1435  if(stereoModel_.left().imageHeight() == 0 || stereoModel_.left().imageWidth() == 0)
1436  {
1437  stereoModel_.setImageSize(left.size());
1438  }
1439 
1440  data = SensorData(left, right, stereoModel_, this->getNextSeqID(), stamp);
1441  }
1442  }
1443  else
1444  {
1445  UERROR("Not received depth and rgb");
1446  }
1447 
1448  IMU imu;
1449  unsigned int confidence = 0;
1450  double imuStamp = stamp*1000.0;
1451  Transform pose;
1452  getPoseAndIMU(imuStamp, pose, confidence, imu);
1453 
1454  if(info && odometryProvided_ && !pose.isNull())
1455  {
1456  // Transform in base frame (local transform should contain base to pose transform)
1457  info->odomPose = this->getLocalTransform() * pose * this->getLocalTransform().inverse();
1458 
1459  info->odomCovariance = cv::Mat::eye(6,6,CV_64FC1) * 0.0001;
1460  info->odomCovariance.rowRange(0,3) *= pow(10, 3-(int)confidence);
1461  info->odomCovariance.rowRange(3,6) *= pow(10, 1-(int)confidence);
1462  }
1463  if(!imu.empty() && !isInterIMUPublishing())
1464  {
1465  data.setIMU(imu);
1466  }
1467  else if(isInterIMUPublishing() && !gyroBuffer_.empty())
1468  {
1469  if(lastImuStamp_ > 0.0)
1470  {
1471  UASSERT(imuStamp > lastImuStamp_);
1472  imuMutex_.lock();
1473  std::map<double, cv::Vec3f>::iterator iterA = gyroBuffer_.upper_bound(lastImuStamp_);
1474  std::map<double, cv::Vec3f>::iterator iterB = gyroBuffer_.lower_bound(imuStamp);
1475  if(iterA != gyroBuffer_.end())
1476  {
1477  ++iterA;
1478  }
1479  if(iterB != gyroBuffer_.end())
1480  {
1481  ++iterB;
1482  }
1483  std::vector<double> stamps;
1484  for(;iterA != iterB;++iterA)
1485  {
1486  stamps.push_back(iterA->first);
1487  }
1488  imuMutex_.unlock();
1489 
1490  int pub = 0;
1491  for(size_t i=0; i<stamps.size(); ++i)
1492  {
1493  Transform tmp;
1494  IMU imuTmp;
1495  getPoseAndIMU(stamps[i], tmp, confidence, imuTmp);
1496  if(!imuTmp.empty())
1497  {
1498  this->postInterIMU(imuTmp, stamps[i]/1000.0);
1499  pub++;
1500  }
1501  else
1502  {
1503  break;
1504  }
1505  }
1506  if(stamps.size())
1507  {
1508  UDEBUG("inter imu published=%d (rate=%fHz), %f -> %f", pub, double(pub)/((stamps.back()-stamps.front())/1000.0), stamps.front()/1000.0, stamps.back()/1000.0);
1509  }
1510  else
1511  {
1512  UWARN("No inter imu published!?");
1513  }
1514  }
1515  lastImuStamp_ = imuStamp;
1516  }
1517  }
1518  else if(frameset.size()==1 && frameset[0].get_profile().stream_type() == RS2_STREAM_FISHEYE)
1519  {
1520  UERROR("Missing frames (received %d, needed=%d). For T265 camera, "
1521  "either use realsense sdk v2.42.0, or apply "
1522  "this patch (https://github.com/IntelRealSense/librealsense/issues/9030#issuecomment-962223017) "
1523  "to fix this problem.", (int)frameset.size(), desiredFramesetSize);
1524  }
1525  else
1526  {
1527  UERROR("Missing frames (received %d, needed=%d)", (int)frameset.size(), desiredFramesetSize);
1528  }
1529  }
1530  catch(const std::exception& ex)
1531  {
1532  UERROR("An error has occurred during frame callback: %s", ex.what());
1533  }
1534 #else
1535  UERROR("CameraRealSense2: RTAB-Map is not built with RealSense2 support!");
1536 #endif
1537  return data;
1538 }
1539 
1540 } // namespace rtabmap
rtabmap::SensorData
Definition: SensorData.h:51
int
int
UINFO
#define UINFO(...)
name
rtabmap::IMU::empty
bool empty() const
Definition: IMU.h:67
D
MatrixXcd D
UTimer::now
static double now()
Definition: UTimer.cpp:80
K
K
timer
list
stream
stream
size
Index size
this
this
glm::profile
profile
Definition: type_gentype.hpp:33
true
#define true
Definition: ConvertUTF.c:57
UTimer.h
rtabmap::util2d::decimate
cv::Mat RTABMAP_CORE_EXPORT decimate(const cv::Mat &image, int d)
Definition: util2d.cpp:1201
rtabmap::Transform::isNull
bool isNull() const
Definition: Transform.cpp:107
glm::pow
GLM_FUNC_DECL genType pow(genType const &base, genType const &exponent)
rtabmap::SensorCaptureInfo
Definition: SensorCaptureInfo.h:36
rtabmap::IMU
Definition: IMU.h:19
module_name
module_name
list::size
size_t size() const
uint16_t
::uint16_t uint16_t
data
int data[]
UScopeMutex
Definition: UMutex.h:157
j
std::ptrdiff_t j
UConversion.h
Some conversion functions.
rtabmap_superglue.device
string device
Definition: rtabmap_superglue.py:21
glm::pi
GLM_FUNC_DECL genType pi()
ULogger::kWarning
@ kWarning
Definition: ULogger.h:252
info
else if n * info
UASSERT
#define UASSERT(condition)
error
void error(const char *str)
rtabmap::CameraRealSense2::init
virtual bool init(const std::string &calibrationFolder=".", const std::string &cameraName="")
Definition: CameraRealSense2.cpp:483
p
Point3_ p(2)
uStrContains
bool uStrContains(const std::string &string, const std::string &substring)
Definition: UStl.h:785
rtabmap::CameraModel::opticalRotation
static Transform opticalRotation()
Definition: CameraModel.h:45
rtabmap::Camera
Definition: Camera.h:43
rtabmap::CameraRealSense2::CameraRealSense2
CameraRealSense2(const std::string &deviceId="", float imageRate=0, const Transform &localTransform=Transform::getIdentity())
Definition: CameraRealSense2.cpp:54
rtabmap::Transform::inverse
Transform inverse() const
Definition: Transform.cpp:178
UWARN
#define UWARN(...)
f
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
timer::elapsed
double elapsed() const
ULogger::level
static ULogger::Level level()
Definition: ULogger.h:340
rtabmap::Transform
Definition: Transform.h:41
empty
util2d.h
PointMatcherSupport::Parametrizable
rtabmap::CameraRealSense2::available
static bool available()
Definition: CameraRealSense2.cpp:45
UStl.h
Wrappers of STL for convenient functions.
uToUpperCase
std::string UTILITE_EXPORT uToUpperCase(const std::string &str)
Definition: UConversion.cpp:63
UDEBUG
#define UDEBUG(...)
CameraRealSense2.h
uSleep
void uSleep(unsigned int ms)
Definition: Posix/UThreadC.h:23
UTimer
Definition: UTimer.h:46
false
#define false
Definition: ConvertUTF.c:56
UThreadC.h
t
Point2 t(10, 10)
rtabmap
Definition: CameraARCore.cpp:35
UERROR
#define UERROR(...)
glm::inverse
GLM_FUNC_DECL matType< T, P > inverse(matType< T, P > const &m)
value
value
i
int i
rtabmap::CameraRealSense2::~CameraRealSense2
virtual ~CameraRealSense2()
Definition: CameraRealSense2.cpp:87
R
R

rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Apr 28 2025 02:45:51