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

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