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  sensors[1].set_option(rs2_option::RS2_OPTION_EMITTER_ENABLED, emitterEnabled_);
636  }
637  else if ("Coded-Light Depth Sensor" == module_name)
638  {
639  }
640  else if ("RGB Camera" == module_name)
641  {
642  if(!ir_)
643  {
644  sensors[0] = elem;
645  }
646  }
647  else if ("Wide FOV Camera" == module_name)
648  {
649  }
650  else if ("Motion Module" == module_name)
651  {
652  if(!dualMode_)
653  {
654  sensors.resize(3);
655  sensors[2] = elem;
656  }
657  }
658  else if ("Tracking Module" == module_name)
659  {
660  if(dualMode_)
661  {
662  sensors.resize(3);
663  }
664  else
665  {
666  sensors.resize(1);
667  stereo = true;
668  }
669  sensors.back() = elem;
670  sensors.back().set_option(rs2_option::RS2_OPTION_ENABLE_POSE_JUMPING, 0);
671  sensors.back().set_option(rs2_option::RS2_OPTION_ENABLE_RELOCALIZATION, 0);
672  }
673  else if ("L500 Depth Sensor" == module_name)
674  {
675  sensors[1] = elem;
676  isL500 = true;
677  if(ir_)
678  {
679  cameraWidth_ = cameraDepthWidth_;
680  cameraHeight_ = cameraDepthHeight_;
681  cameraFps_ = cameraDepthFps_;
682  }
683  irDepth_ = true;
684  }
685  else
686  {
687  UERROR("Module Name \"%s\" isn't supported!", module_name.c_str());
688  return false;
689  }
690  UINFO("%s was found.", elem.get_info(RS2_CAMERA_INFO_NAME));
691  }
692 
693  UDEBUG("");
694 
695  model_ = CameraModel();
696  std::vector<std::vector<rs2::stream_profile> > profilesPerSensor(sensors.size());
697  for (unsigned int i=0; i<sensors.size(); ++i)
698  {
699  if(i==0 && ir_ && !stereo)
700  {
701  continue;
702  }
703  UINFO("Sensor %d \"%s\"", (int)i, sensors[i].get_info(RS2_CAMERA_INFO_NAME));
704  auto profiles = sensors[i].get_stream_profiles();
705  bool added = false;
706  UINFO("profiles=%d", (int)profiles.size());
707  if(ULogger::level()<ULogger::kWarning)
708  {
709  for (auto& profile : profiles)
710  {
711  auto video_profile = profile.as<rs2::video_stream_profile>();
712  UINFO("%s %d %d %d %d %s type=%d", rs2_format_to_string(
713  video_profile.format()),
714  video_profile.width(),
715  video_profile.height(),
716  video_profile.fps(),
717  video_profile.stream_index(),
718  video_profile.stream_name().c_str(),
719  video_profile.stream_type());
720  }
721  }
722  int pi = 0;
723  for (auto& profile : profiles)
724  {
725  auto video_profile = profile.as<rs2::video_stream_profile>();
726  if(!stereo)
727  {
728  if( (video_profile.width() == cameraWidth_ &&
729  video_profile.height() == cameraHeight_ &&
730  video_profile.fps() == cameraFps_) ||
731  (strcmp(sensors[i].get_info(RS2_CAMERA_INFO_NAME), "L500 Depth Sensor")==0 &&
732  video_profile.width() == cameraDepthWidth_ &&
733  video_profile.height() == cameraDepthHeight_ &&
734  video_profile.fps() == cameraDepthFps_))
735  {
736  auto intrinsic = video_profile.get_intrinsics();
737 
738  // rgb or ir left
739  if((!ir_ && video_profile.format() == RS2_FORMAT_RGB8 && video_profile.stream_type() == RS2_STREAM_COLOR) ||
740  (ir_ && video_profile.format() == RS2_FORMAT_Y8 && (video_profile.stream_index() == 1 || isL500)))
741  {
742  if(!profilesPerSensor[i].empty())
743  {
744  // IR right already there, push ir left front
745  profilesPerSensor[i].push_back(profilesPerSensor[i].back());
746  profilesPerSensor[i].front() = profile;
747  }
748  else
749  {
750  profilesPerSensor[i].push_back(profile);
751  }
752  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));
753  model_ = CameraModel(camera_name, intrinsic.fx, intrinsic.fy, intrinsic.ppx, intrinsic.ppy, this->getLocalTransform(), 0, cv::Size(intrinsic.width, intrinsic.height));
754  UINFO("Model: %dx%d fx=%f fy=%f cx=%f cy=%f dist model=%d coeff=%f %f %f %f %f",
755  intrinsic.width, intrinsic.height,
756  intrinsic.fx, intrinsic.fy, intrinsic.ppx, intrinsic.ppy,
757  intrinsic.model,
758  intrinsic.coeffs[0], intrinsic.coeffs[1], intrinsic.coeffs[2], intrinsic.coeffs[3], intrinsic.coeffs[4]);
759  added = true;
760  if(video_profile.format() == RS2_FORMAT_RGB8 || profilesPerSensor[i].size()==2)
761  {
762  break;
763  }
764  }
765  // depth or ir right
766  else if(((!ir_ || irDepth_) && video_profile.format() == RS2_FORMAT_Z16) ||
767  (ir_ && !irDepth_ && video_profile.format() == RS2_FORMAT_Y8 && video_profile.stream_index() == 2))
768  {
769  profilesPerSensor[i].push_back(profile);
770  depthBuffer_ = cv::Mat(cv::Size(cameraWidth_, cameraHeight_), video_profile.format() == RS2_FORMAT_Y8?CV_8UC1:CV_16UC1, cv::Scalar(0));
771  added = true;
772  if(!ir_ || irDepth_ || profilesPerSensor[i].size()==2)
773  {
774  break;
775  }
776  }
777  }
778  else if(video_profile.format() == RS2_FORMAT_MOTION_XYZ32F || video_profile.format() == RS2_FORMAT_6DOF)
779  {
780  //D435i:
781  //MOTION_XYZ32F 0 0 200 (gyro)
782  //MOTION_XYZ32F 0 0 400 (gyro)
783  //MOTION_XYZ32F 0 0 63 6 (accel)
784  //MOTION_XYZ32F 0 0 250 6 (accel)
785  // or dualMode_ T265:
786  //MOTION_XYZ32F 0 0 200 5 (gyro)
787  //MOTION_XYZ32F 0 0 62 6 (accel)
788  //6DOF 0 0 200 4 (pose)
789  bool modified = false;
790  for (size_t j= 0; j < profilesPerSensor[i].size(); ++j)
791  {
792  if (profilesPerSensor[i][j].stream_type() == profile.stream_type())
793  {
794  if (profile.stream_type() == RS2_STREAM_ACCEL)
795  {
796  if(profile.fps() > profilesPerSensor[i][j].fps())
797  profilesPerSensor[i][j] = profile;
798  modified = true;
799  }
800  else if (profile.stream_type() == RS2_STREAM_GYRO)
801  {
802  if(profile.fps() < profilesPerSensor[i][j].fps())
803  profilesPerSensor[i][j] = profile;
804  modified = true;
805  }
806  }
807  }
808  if(!modified)
809  profilesPerSensor[i].push_back(profile);
810  added = true;
811  }
812  }
813  else if(stereo || dualMode_)
814  {
815  //T265:
816  if(!dualMode_ &&
817  video_profile.format() == RS2_FORMAT_Y8 &&
818  video_profile.width() == 848 &&
819  video_profile.height() == 800 &&
820  video_profile.fps() == 30)
821  {
822  UASSERT(i<2);
823  profilesPerSensor[i].push_back(profile);
824  if(pi==0)
825  {
826  // LEFT FISHEYE
827  rgbBuffer_ = cv::Mat(cv::Size(848, 800), CV_8UC1, cv::Scalar(0));
828  if(odometryOnlyLeftStream_)
829  {
830  auto intrinsic = video_profile.get_intrinsics();
831  UINFO("Model: %dx%d fx=%f fy=%f cx=%f cy=%f dist model=%d coeff=%f %f %f %f",
832  intrinsic.width, intrinsic.height,
833  intrinsic.fx, intrinsic.fy, intrinsic.ppx, intrinsic.ppy,
834  intrinsic.model,
835  intrinsic.coeffs[0], intrinsic.coeffs[1], intrinsic.coeffs[2], intrinsic.coeffs[3]);
836  cv::Mat K = cv::Mat::eye(3,3,CV_64FC1);
837  K.at<double>(0,0) = intrinsic.fx;
838  K.at<double>(1,1) = intrinsic.fy;
839  K.at<double>(0,2) = intrinsic.ppx;
840  K.at<double>(1,2) = intrinsic.ppy;
841  UASSERT(intrinsic.model == RS2_DISTORTION_KANNALA_BRANDT4); // we expect fisheye 4 values
842  cv::Mat D = cv::Mat::zeros(1,6,CV_64FC1);
843  D.at<double>(0,0) = intrinsic.coeffs[0];
844  D.at<double>(0,1) = intrinsic.coeffs[1];
845  D.at<double>(0,4) = intrinsic.coeffs[2];
846  D.at<double>(0,5) = intrinsic.coeffs[3];
847  cv::Mat P = cv::Mat::eye(3, 4, CV_64FC1);
848  P.at<double>(0,0) = intrinsic.fx;
849  P.at<double>(1,1) = intrinsic.fy;
850  P.at<double>(0,2) = intrinsic.ppx;
851  P.at<double>(1,2) = intrinsic.ppy;
852  cv::Mat R = cv::Mat::eye(3, 3, CV_64FC1);
853  model_ = CameraModel(camera_name, cv::Size(intrinsic.width, intrinsic.height), K, D, R, P, this->getLocalTransform());
854  if(rectifyImages_)
855  model_.initRectificationMap();
856  }
857  }
858  else if(!odometryOnlyLeftStream_)
859  {
860  // RIGHT FISHEYE
861  depthBuffer_ = cv::Mat(cv::Size(848, 800), CV_8UC1, cv::Scalar(0));
862  }
863  added = true;
864  }
865  else if(video_profile.format() == RS2_FORMAT_MOTION_XYZ32F || video_profile.format() == RS2_FORMAT_6DOF)
866  {
867  //MOTION_XYZ32F 0 0 200
868  //MOTION_XYZ32F 0 0 62
869  //6DOF 0 0 200
870  profilesPerSensor[i].push_back(profile);
871  added = true;
872  }
873  }
874  ++pi;
875  }
876  if (!added)
877  {
878  UERROR("Given stream configuration is not supported by the device! "
879  "Stream Index: %d, Width: %d, Height: %d, FPS: %d", i, cameraWidth_, cameraHeight_, cameraFps_);
880  UERROR("Available configurations:");
881  for (auto& profile : profiles)
882  {
883  auto video_profile = profile.as<rs2::video_stream_profile>();
884  UERROR("%s %d %d %d %d %s type=%d", rs2_format_to_string(
885  video_profile.format()),
886  video_profile.width(),
887  video_profile.height(),
888  video_profile.fps(),
889  video_profile.stream_index(),
890  video_profile.stream_name().c_str(),
891  video_profile.stream_type());
892  }
893  return false;
894  }
895  }
896  UDEBUG("");
897  if(!stereo)
898  {
899  if(!model_.isValidForProjection())
900  {
901  UERROR("Calibration info not valid!");
902  std::cout<< model_ << std::endl;
903  return false;
904  }
905 
906  if(dualMode_)
907  {
908  UINFO("Set base to pose");
909  this->setLocalTransform(this->getLocalTransform()*CameraModel::opticalRotation().inverse());
910  UINFO("dualExtrinsics_ = %s", dualExtrinsics_.prettyPrint().c_str());
911  Transform baseToCam = this->getLocalTransform()*dualExtrinsics_;
912  UASSERT(profilesPerSensor.size()>=2);
913  UASSERT(profilesPerSensor.back().size() == 3);
914  rs2_extrinsics poseToIMU = profilesPerSensor.back()[0].get_extrinsics_to(profilesPerSensor.back()[2]);
915 
916  Transform poseToIMUT(
917  poseToIMU.rotation[0], poseToIMU.rotation[1], poseToIMU.rotation[2], poseToIMU.translation[0],
918  poseToIMU.rotation[3], poseToIMU.rotation[4], poseToIMU.rotation[5], poseToIMU.translation[1],
919  poseToIMU.rotation[6], poseToIMU.rotation[7], poseToIMU.rotation[8], poseToIMU.translation[2]);
920  poseToIMUT = realsense2PoseRotation_ * poseToIMUT;
921  UINFO("poseToIMU = %s", poseToIMUT.prettyPrint().c_str());
922 
923  UINFO("BaseToCam = %s", baseToCam.prettyPrint().c_str());
924  model_.setLocalTransform(baseToCam);
925  imuLocalTransform_ = this->getLocalTransform() * poseToIMUT;
926  }
927 
928  if(ir_ && !irDepth_ && profilesPerSensor.size() >= 2 && profilesPerSensor[1].size() >= 2)
929  {
930  rs2_extrinsics leftToRight = profilesPerSensor[1][1].get_extrinsics_to(profilesPerSensor[1][0]);
931  Transform leftToRightT(
932  leftToRight.rotation[0], leftToRight.rotation[1], leftToRight.rotation[2], leftToRight.translation[0],
933  leftToRight.rotation[3], leftToRight.rotation[4], leftToRight.rotation[5], leftToRight.translation[1],
934  leftToRight.rotation[6], leftToRight.rotation[7], leftToRight.rotation[8], leftToRight.translation[2]);
935 
936  UINFO("left to right transform = %s", leftToRightT.prettyPrint().c_str());
937 
938  // Create stereo camera model from left and right ir of D435
939  stereoModel_ = StereoCameraModel(model_.fx(), model_.fy(), model_.cx(), model_.cy(), leftToRightT.x(), model_.localTransform(), model_.imageSize());
940  UINFO("Stereo parameters: fx=%f cx=%f cy=%f baseline=%f",
941  stereoModel_.left().fx(),
942  stereoModel_.left().cx(),
943  stereoModel_.left().cy(),
944  stereoModel_.baseline());
945  }
946 
947  if(!dualMode_ && profilesPerSensor.size() == 3)
948  {
949  if(!profilesPerSensor[2].empty() && !profilesPerSensor[0].empty())
950  {
951  rs2_extrinsics leftToIMU = profilesPerSensor[2][0].get_extrinsics_to(profilesPerSensor[0][0]);
952  Transform leftToIMUT(
953  leftToIMU.rotation[0], leftToIMU.rotation[1], leftToIMU.rotation[2], leftToIMU.translation[0],
954  leftToIMU.rotation[3], leftToIMU.rotation[4], leftToIMU.rotation[5], leftToIMU.translation[1],
955  leftToIMU.rotation[6], leftToIMU.rotation[7], leftToIMU.rotation[8], leftToIMU.translation[2]);
956  imuLocalTransform_ = this->getLocalTransform() * leftToIMUT;
957  UINFO("imu local transform = %s", imuLocalTransform_.prettyPrint().c_str());
958  }
959  else if(!profilesPerSensor[2].empty() && !profilesPerSensor[1].empty())
960  {
961  rs2_extrinsics leftToIMU = profilesPerSensor[2][0].get_extrinsics_to(profilesPerSensor[1][0]);
962  Transform leftToIMUT(
963  leftToIMU.rotation[0], leftToIMU.rotation[1], leftToIMU.rotation[2], leftToIMU.translation[0],
964  leftToIMU.rotation[3], leftToIMU.rotation[4], leftToIMU.rotation[5], leftToIMU.translation[1],
965  leftToIMU.rotation[6], leftToIMU.rotation[7], leftToIMU.rotation[8], leftToIMU.translation[2]);
966 
967  imuLocalTransform_ = this->getLocalTransform() * leftToIMUT;
968  UINFO("imu local transform = %s", imuLocalTransform_.prettyPrint().c_str());
969  }
970  }
971  }
972  else // T265
973  {
974 
975  // look for calibration files
976  std::string serial = sn;
977  if(!cameraName.empty())
978  {
979  serial = cameraName;
980  }
981  if(!odometryImagesDisabled_ &&
982  !odometryOnlyLeftStream_ &&
983  !calibrationFolder.empty() && !serial.empty())
984  {
985  if(!stereoModel_.load(calibrationFolder, serial, false))
986  {
987  UWARN("Missing calibration files for camera \"%s\" in \"%s\" folder, you should calibrate the camera!",
988  serial.c_str(), calibrationFolder.c_str());
989  }
990  else
991  {
992  UINFO("Stereo parameters: fx=%f cx=%f cy=%f baseline=%f",
993  stereoModel_.left().fx(),
994  stereoModel_.left().cx(),
995  stereoModel_.left().cy(),
996  stereoModel_.baseline());
997  }
998  }
999 
1000  // Get extrinsics with pose as the base frame:
1001  // 0=Left fisheye
1002  // 1=Right fisheye
1003  // 2=GYRO
1004  // 3=ACC
1005  // 4=POSE
1006  UASSERT(profilesPerSensor[0].size() == 5);
1007  if(odometryProvided_)
1008  {
1009  rs2_extrinsics poseToLeft = profilesPerSensor[0][0].get_extrinsics_to(profilesPerSensor[0][4]);
1010  rs2_extrinsics poseToIMU = profilesPerSensor[0][2].get_extrinsics_to(profilesPerSensor[0][4]);
1011  Transform poseToLeftT(
1012  poseToLeft.rotation[0], poseToLeft.rotation[1], poseToLeft.rotation[2], poseToLeft.translation[0],
1013  poseToLeft.rotation[3], poseToLeft.rotation[4], poseToLeft.rotation[5], poseToLeft.translation[1],
1014  poseToLeft.rotation[6], poseToLeft.rotation[7], poseToLeft.rotation[8], poseToLeft.translation[2]);
1015  poseToLeftT = realsense2PoseRotation_ * poseToLeftT;
1016  UINFO("poseToLeft = %s", poseToLeftT.prettyPrint().c_str());
1017 
1018  Transform poseToIMUT(
1019  poseToIMU.rotation[0], poseToIMU.rotation[1], poseToIMU.rotation[2], poseToIMU.translation[0],
1020  poseToIMU.rotation[3], poseToIMU.rotation[4], poseToIMU.rotation[5], poseToIMU.translation[1],
1021  poseToIMU.rotation[6], poseToIMU.rotation[7], poseToIMU.rotation[8], poseToIMU.translation[2]);
1022  poseToIMUT = realsense2PoseRotation_ * poseToIMUT;
1023  UINFO("poseToIMU = %s", poseToIMUT.prettyPrint().c_str());
1024 
1025  Transform opticalTransform(0, 0, 1, 0, -1, 0, 0, 0, 0, -1, 0, 0);
1026  this->setLocalTransform(this->getLocalTransform() * opticalTransform.inverse());
1027  if(odometryOnlyLeftStream_)
1028  model_.setLocalTransform(this->getLocalTransform()*poseToLeftT);
1029  else
1030  stereoModel_.setLocalTransform(this->getLocalTransform()*poseToLeftT);
1031  imuLocalTransform_ = this->getLocalTransform()* poseToIMUT;
1032 
1033  if(odometryImagesDisabled_)
1034  {
1035  // keep only pose stream
1036  std::vector<rs2::stream_profile> profiles;
1037  profiles.push_back(profilesPerSensor[0][4]);
1038  profilesPerSensor[0] = profiles;
1039  }
1040  }
1041  else
1042  {
1043  // Set imu transform based on the left camera instead of pose
1044  rs2_extrinsics leftToIMU = profilesPerSensor[0][2].get_extrinsics_to(profilesPerSensor[0][0]);
1045  Transform leftToIMUT(
1046  leftToIMU.rotation[0], leftToIMU.rotation[1], leftToIMU.rotation[2], leftToIMU.translation[0],
1047  leftToIMU.rotation[3], leftToIMU.rotation[4], leftToIMU.rotation[5], leftToIMU.translation[1],
1048  leftToIMU.rotation[6], leftToIMU.rotation[7], leftToIMU.rotation[8], leftToIMU.translation[2]);
1049  UINFO("leftToIMU = %s", leftToIMUT.prettyPrint().c_str());
1050  imuLocalTransform_ = this->getLocalTransform() * leftToIMUT;
1051  UINFO("imu local transform = %s", imuLocalTransform_.prettyPrint().c_str());
1052  if(odometryOnlyLeftStream_)
1053  model_.setLocalTransform(this->getLocalTransform());
1054  else
1055  stereoModel_.setLocalTransform(this->getLocalTransform());
1056  }
1057  if(!odometryImagesDisabled_ && rectifyImages_ && !model_.isValidForRectification() && !stereoModel_.isValidForRectification())
1058  {
1059  UERROR("Parameter \"rectifyImages\" is set, but no stereo model is loaded or valid.");
1060  return false;
1061  }
1062  }
1063 
1064  std::function<void(rs2::frame)> multiple_message_callback_function = [this](rs2::frame frame){multiple_message_callback(frame);};
1065 
1066  for (unsigned int i=0; i<sensors.size(); ++i)
1067  {
1068  if(profilesPerSensor[i].size())
1069  {
1070  UINFO("Starting sensor %d with %d profiles", (int)i, (int)profilesPerSensor[i].size());
1071  for (size_t j = 0; j < profilesPerSensor[i].size(); ++j)
1072  {
1073  auto video_profile = profilesPerSensor[i][j].as<rs2::video_stream_profile>();
1074  UINFO("Opening: %s %d %d %d %d %s type=%d", rs2_format_to_string(
1075  video_profile.format()),
1076  video_profile.width(),
1077  video_profile.height(),
1078  video_profile.fps(),
1079  video_profile.stream_index(),
1080  video_profile.stream_name().c_str(),
1081  video_profile.stream_type());
1082  }
1083  if(globalTimeSync_ && sensors[i].supports(rs2_option::RS2_OPTION_GLOBAL_TIME_ENABLED))
1084  {
1085  float value = sensors[i].get_option(rs2_option::RS2_OPTION_GLOBAL_TIME_ENABLED);
1086  UINFO("Set RS2_OPTION_GLOBAL_TIME_ENABLED=1 (was %f) for sensor %d", value, (int)i);
1087  sensors[i].set_option(rs2_option::RS2_OPTION_GLOBAL_TIME_ENABLED, 1);
1088  }
1089  sensors[i].open(profilesPerSensor[i]);
1090  if(sensors[i].is<rs2::depth_sensor>())
1091  {
1092  auto depth_sensor = sensors[i].as<rs2::depth_sensor>();
1093  depth_scale_meters_ = depth_sensor.get_depth_scale();
1094  UINFO("Depth scale %f for sensor %d", depth_scale_meters_, (int)i);
1095  }
1096  sensors[i].start(multiple_message_callback_function);
1097  }
1098  }
1099 
1100  uSleep(1000); // ignore the first frames
1101  UINFO("Enabling streams...done!");
1102 
1103  return true;
1104 
1105 #else
1106  UERROR("CameraRealSense: RTAB-Map is not built with RealSense2 support!");
1107  return false;
1108 #endif
1109 }
1110 
1111 bool CameraRealSense2::isCalibrated() const
1112 {
1113 #ifdef RTABMAP_REALSENSE2
1114  return model_.isValidForProjection() || stereoModel_.isValidForRectification();
1115 #else
1116  return false;
1117 #endif
1118 }
1119 
1120 std::string CameraRealSense2::getSerial() const
1121 {
1122 #ifdef RTABMAP_REALSENSE2
1123  if(!dev_.empty())
1124  {
1125  return dev_[0].get_info(RS2_CAMERA_INFO_SERIAL_NUMBER);
1126  }
1127 #endif
1128  return "NA";
1129 }
1130 
1131 bool CameraRealSense2::odomProvided() const
1132 {
1133 #ifdef RTABMAP_REALSENSE2
1134  return odometryProvided_;
1135 #else
1136  return false;
1137 #endif
1138 }
1139 
1140 bool CameraRealSense2::getPose(double stamp, Transform & pose, cv::Mat & covariance, double maxWaitTime)
1141 {
1142 #ifdef RTABMAP_REALSENSE2
1143  IMU imu;
1144  unsigned int confidence = 0;
1145  double rsStamp = stamp*1000.0;
1146  Transform p;
1147  getPoseAndIMU(rsStamp, p, confidence, imu, maxWaitTime*1000);
1148 
1149  if(!p.isNull())
1150  {
1151  // Transform in base frame
1152  pose = this->getLocalTransform() * p * this->getLocalTransform().inverse();
1153 
1154  covariance = cv::Mat::eye(6,6,CV_64FC1) * 0.0001;
1155  covariance.rowRange(0,3) *= pow(10, 3-(int)confidence);
1156  covariance.rowRange(3,6) *= pow(10, 1-(int)confidence);
1157  return true;
1158  }
1159 #endif
1160  return false;
1161 }
1162 
1163 void CameraRealSense2::setEmitterEnabled(bool enabled)
1164 {
1165 #ifdef RTABMAP_REALSENSE2
1166  emitterEnabled_ = enabled;
1167 #endif
1168 }
1169 
1170 void CameraRealSense2::setIRFormat(bool enabled, bool useDepthInsteadOfRightImage)
1171 {
1172 #ifdef RTABMAP_REALSENSE2
1173  ir_ = enabled;
1174  irDepth_ = useDepthInsteadOfRightImage;
1175 #endif
1176 }
1177 
1178 void CameraRealSense2::setResolution(int width, int height, int fps)
1179 {
1180 #ifdef RTABMAP_REALSENSE2
1181  cameraWidth_ = width;
1182  cameraHeight_ = height;
1183  cameraFps_ = fps;
1184 #endif
1185 }
1186 
1187 void CameraRealSense2::setDepthResolution(int width, int height, int fps)
1188 {
1189 #ifdef RTABMAP_REALSENSE2
1190  cameraDepthWidth_ = width;
1191  cameraDepthHeight_ = height;
1192  cameraDepthFps_ = fps;
1193 #endif
1194 }
1195 
1196 void CameraRealSense2::setGlobalTimeSync(bool enabled)
1197 {
1198 #ifdef RTABMAP_REALSENSE2
1199  globalTimeSync_ = enabled;
1200 #endif
1201 }
1202 
1203 void CameraRealSense2::setDualMode(bool enabled, const Transform & extrinsics)
1204 {
1205 #ifdef RTABMAP_REALSENSE2
1206  UASSERT(!enabled || !extrinsics.isNull());
1207  dualMode_ = enabled;
1208  dualExtrinsics_ = extrinsics*CameraModel::opticalRotation();
1209  if(dualMode_)
1210  {
1211  odometryProvided_ = true;
1212  odometryImagesDisabled_ = false;
1213  odometryOnlyLeftStream_ = false;
1214  }
1215 #endif
1216 }
1217 
1218 void CameraRealSense2::setJsonConfig(const std::string & json)
1219 {
1220 #ifdef RTABMAP_REALSENSE2
1221  jsonConfig_ = json;
1222 #endif
1223 }
1224 
1225 void CameraRealSense2::setImagesRectified(bool enabled)
1226 {
1227 #ifdef RTABMAP_REALSENSE2
1228  rectifyImages_ = enabled;
1229 #endif
1230 }
1231 
1232 void CameraRealSense2::setOdomProvided(bool enabled, bool imageStreamsDisabled, bool onlyLeftStream)
1233 {
1234 #ifdef RTABMAP_REALSENSE2
1235  if(dualMode_ && !enabled)
1236  {
1237  UERROR("Odometry is disabled but dual mode was enabled, disabling dual mode.");
1238  dualMode_ = false;
1239  }
1240  odometryProvided_ = enabled;
1241  odometryImagesDisabled_ = enabled && imageStreamsDisabled;
1242  odometryOnlyLeftStream_ = enabled && !imageStreamsDisabled && onlyLeftStream;
1243 #endif
1244 }
1245 
1246 SensorData CameraRealSense2::captureImage(SensorCaptureInfo * info)
1247 {
1248  SensorData data;
1249 #ifdef RTABMAP_REALSENSE2
1250 
1251  try{
1252  auto frameset = syncer_.wait_for_frames(5000);
1253  UTimer timer;
1254  int desiredFramesetSize = 2;
1255  while ((int)frameset.size() != desiredFramesetSize && timer.elapsed() < 2.0)
1256  {
1257  // maybe there is a latency with the USB, try again in 100 ms (for the next 2 seconds)
1258  frameset = syncer_.wait_for_frames(100);
1259  }
1260  if ((int)frameset.size() == desiredFramesetSize)
1261  {
1262  double now = UTimer::now();
1263  bool is_rgb_arrived = false;
1264  bool is_depth_arrived = false;
1265  bool is_left_fisheye_arrived = false;
1266  bool is_right_fisheye_arrived = false;
1267  rs2::frame rgb_frame;
1268  rs2::frame depth_frame;
1269  double stamp = frameset.get_timestamp();
1270  for (auto it = frameset.begin(); it != frameset.end(); ++it)
1271  {
1272  auto f = (*it);
1273  if(stamp > f.get_timestamp())
1274  {
1275  stamp = f.get_timestamp();
1276  }
1277  auto stream_type = f.get_profile().stream_type();
1278  if (stream_type == RS2_STREAM_COLOR || stream_type == RS2_STREAM_INFRARED)
1279  {
1280  if(ir_ && !irDepth_)
1281  {
1282  //stereo D435
1283  if(!is_depth_arrived)
1284  {
1285  depth_frame = f; // right image
1286  is_depth_arrived = true;
1287  }
1288  else
1289  {
1290  rgb_frame = f; // left image
1291  is_rgb_arrived = true;
1292  }
1293  }
1294  else
1295  {
1296  rgb_frame = f;
1297  is_rgb_arrived = true;
1298  }
1299  }
1300  else if (stream_type == RS2_STREAM_DEPTH)
1301  {
1302  depth_frame = f;
1303  is_depth_arrived = true;
1304  }
1305  else if (stream_type == RS2_STREAM_FISHEYE)
1306  {
1307  if(!is_right_fisheye_arrived)
1308  {
1309  depth_frame = f;
1310  is_right_fisheye_arrived = true;
1311  }
1312  else
1313  {
1314  rgb_frame = f;
1315  is_left_fisheye_arrived = true;
1316  }
1317  }
1318  }
1319 
1320  stamp /= 1000.0; // put in seconds
1321  UDEBUG("Frameset arrived. system=%fs frame=%fs", now, stamp);
1322  if(stamp - now > 1000000000.0)
1323  {
1324  if(!clockSyncWarningShown_)
1325  {
1326  UWARN("Clocks are not sync with host computer! Detected stamps in far "
1327  "future %f, thus using host time instead (%f)! This message "
1328  "will only appear once. "
1329  "See https://github.com/IntelRealSense/librealsense/issues/4505 "
1330  "for more info", stamp, now);
1331  clockSyncWarningShown_ = true;
1332  }
1333  stamp = now;
1334  }
1335 
1336  if(is_rgb_arrived && is_depth_arrived)
1337  {
1338  cv::Mat depth;
1339  if(ir_ && !irDepth_)
1340  {
1341  depth = cv::Mat(depthBuffer_.size(), depthBuffer_.type(), (void*)depth_frame.get_data()).clone();
1342  }
1343  else
1344  {
1345  rs2::align align(rgb_frame.get_profile().stream_type());
1346  rs2::frameset processed = frameset.apply_filter(align);
1347  rs2::depth_frame aligned_depth_frame = processed.get_depth_frame();
1348  if(frameset.get_depth_frame().get_width() < aligned_depth_frame.get_width() &&
1349  frameset.get_depth_frame().get_height() < aligned_depth_frame.get_height())
1350  {
1351  int decimationWidth = int(float(aligned_depth_frame.get_width())/float(frameset.get_depth_frame().get_width())+0.5f);
1352  int decimationHeight = int(float(aligned_depth_frame.get_height())/float(frameset.get_depth_frame().get_height())+0.5f);
1353  if(decimationWidth>1 || decimationHeight>1)
1354  {
1355  depth = util2d::decimate(cv::Mat(depthBuffer_.size(), depthBuffer_.type(), (void*)aligned_depth_frame.get_data()), decimationWidth>decimationHeight?decimationWidth:decimationHeight);
1356  }
1357  else
1358  {
1359  depth = cv::Mat(depthBuffer_.size(), depthBuffer_.type(), (void*)aligned_depth_frame.get_data()).clone();
1360  }
1361  }
1362  else
1363  {
1364  depth = cv::Mat(depthBuffer_.size(), depthBuffer_.type(), (void*)aligned_depth_frame.get_data()).clone();
1365  }
1366  if(depth_scale_meters_ != 0.001f)
1367  { // convert to mm
1368  if(depth.type() == CV_16UC1)
1369  {
1370  float scaleMM = depth_scale_meters_ / 0.001f;
1371  depth = scaleMM * depth;
1372  }
1373  }
1374  }
1375 
1376  cv::Mat rgb = cv::Mat(rgbBuffer_.size(), rgbBuffer_.type(), (void*)rgb_frame.get_data());
1377  cv::Mat bgr;
1378  if(rgb.channels() == 3)
1379  {
1380  cv::cvtColor(rgb, bgr, CV_RGB2BGR);
1381  }
1382  else
1383  {
1384  bgr = rgb.clone();
1385  }
1386 
1387  if(ir_ && !irDepth_)
1388  {
1389  //stereo D435
1390  data = SensorData(bgr, depth, stereoModel_, this->getNextSeqID(), stamp);
1391  }
1392  else
1393  {
1394  data = SensorData(bgr, depth, model_, this->getNextSeqID(), stamp);
1395  }
1396  }
1397  else if(is_left_fisheye_arrived)
1398  {
1399  if(odometryOnlyLeftStream_)
1400  {
1401  cv::Mat left;
1402  if(rectifyImages_ && model_.isValidForRectification())
1403  {
1404  left = model_.rectifyImage(cv::Mat(rgbBuffer_.size(), rgbBuffer_.type(), (void*)rgb_frame.get_data()));
1405  }
1406  else
1407  {
1408  left = cv::Mat(rgbBuffer_.size(), rgbBuffer_.type(), (void*)rgb_frame.get_data()).clone();
1409  }
1410 
1411  if(model_.imageHeight() == 0 || model_.imageWidth() == 0)
1412  {
1413  model_.setImageSize(left.size());
1414  }
1415 
1416  data = SensorData(left, cv::Mat(), model_, this->getNextSeqID(), stamp);
1417  }
1418  else if(is_right_fisheye_arrived)
1419  {
1420  cv::Mat left,right;
1421  if(rectifyImages_ && stereoModel_.left().isValidForRectification() && stereoModel_.right().isValidForRectification())
1422  {
1423  left = stereoModel_.left().rectifyImage(cv::Mat(rgbBuffer_.size(), rgbBuffer_.type(), (void*)rgb_frame.get_data()));
1424  right = stereoModel_.right().rectifyImage(cv::Mat(depthBuffer_.size(), depthBuffer_.type(), (void*)depth_frame.get_data()));
1425  }
1426  else
1427  {
1428  left = cv::Mat(rgbBuffer_.size(), rgbBuffer_.type(), (void*)rgb_frame.get_data()).clone();
1429  right = cv::Mat(depthBuffer_.size(), depthBuffer_.type(), (void*)depth_frame.get_data()).clone();
1430  }
1431 
1432  if(stereoModel_.left().imageHeight() == 0 || stereoModel_.left().imageWidth() == 0)
1433  {
1434  stereoModel_.setImageSize(left.size());
1435  }
1436 
1437  data = SensorData(left, right, stereoModel_, this->getNextSeqID(), stamp);
1438  }
1439  }
1440  else
1441  {
1442  UERROR("Not received depth and rgb");
1443  }
1444 
1445  IMU imu;
1446  unsigned int confidence = 0;
1447  double imuStamp = stamp*1000.0;
1448  Transform pose;
1449  getPoseAndIMU(imuStamp, pose, confidence, imu);
1450 
1451  if(info && odometryProvided_ && !pose.isNull())
1452  {
1453  // Transform in base frame (local transform should contain base to pose transform)
1454  info->odomPose = this->getLocalTransform() * pose * this->getLocalTransform().inverse();
1455 
1456  info->odomCovariance = cv::Mat::eye(6,6,CV_64FC1) * 0.0001;
1457  info->odomCovariance.rowRange(0,3) *= pow(10, 3-(int)confidence);
1458  info->odomCovariance.rowRange(3,6) *= pow(10, 1-(int)confidence);
1459  }
1460  if(!imu.empty() && !isInterIMUPublishing())
1461  {
1462  data.setIMU(imu);
1463  }
1464  else if(isInterIMUPublishing() && !gyroBuffer_.empty())
1465  {
1466  if(lastImuStamp_ > 0.0)
1467  {
1468  UASSERT(imuStamp > lastImuStamp_);
1469  imuMutex_.lock();
1470  std::map<double, cv::Vec3f>::iterator iterA = gyroBuffer_.upper_bound(lastImuStamp_);
1471  std::map<double, cv::Vec3f>::iterator iterB = gyroBuffer_.lower_bound(imuStamp);
1472  if(iterA != gyroBuffer_.end())
1473  {
1474  ++iterA;
1475  }
1476  if(iterB != gyroBuffer_.end())
1477  {
1478  ++iterB;
1479  }
1480  std::vector<double> stamps;
1481  for(;iterA != iterB;++iterA)
1482  {
1483  stamps.push_back(iterA->first);
1484  }
1485  imuMutex_.unlock();
1486 
1487  int pub = 0;
1488  for(size_t i=0; i<stamps.size(); ++i)
1489  {
1490  Transform tmp;
1491  IMU imuTmp;
1492  getPoseAndIMU(stamps[i], tmp, confidence, imuTmp);
1493  if(!imuTmp.empty())
1494  {
1495  this->postInterIMU(imuTmp, stamps[i]/1000.0);
1496  pub++;
1497  }
1498  else
1499  {
1500  break;
1501  }
1502  }
1503  if(stamps.size())
1504  {
1505  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);
1506  }
1507  else
1508  {
1509  UWARN("No inter imu published!?");
1510  }
1511  }
1512  lastImuStamp_ = imuStamp;
1513  }
1514  }
1515  else if(frameset.size()==1 && frameset[0].get_profile().stream_type() == RS2_STREAM_FISHEYE)
1516  {
1517  UERROR("Missing frames (received %d, needed=%d). For T265 camera, "
1518  "either use realsense sdk v2.42.0, or apply "
1519  "this patch (https://github.com/IntelRealSense/librealsense/issues/9030#issuecomment-962223017) "
1520  "to fix this problem.", (int)frameset.size(), desiredFramesetSize);
1521  }
1522  else
1523  {
1524  UERROR("Missing frames (received %d, needed=%d)", (int)frameset.size(), desiredFramesetSize);
1525  }
1526  }
1527  catch(const std::exception& ex)
1528  {
1529  UERROR("An error has occurred during frame callback: %s", ex.what());
1530  }
1531 #else
1532  UERROR("CameraRealSense2: RTAB-Map is not built with RealSense2 support!");
1533 #endif
1534  return data;
1535 }
1536 
1537 } // 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
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 >)
K
K
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 Thu Jul 25 2024 02:50:07