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

rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Dec 14 2020 03:34:58