CameraStereoZed.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 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
20 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 */
27 
28 #include <rtabmap/core/camera/CameraStereoZed.h>
29 #include <rtabmap/utilite/UTimer.h>
30 #include <rtabmap/utilite/UThread.h>
31 #include <rtabmap/utilite/UEventsManager.h>
32 #include <rtabmap/utilite/UConversion.h>
33 
34 #ifdef RTABMAP_ZED
35 #include <sl/Camera.hpp>
36 #endif
37 
38 namespace rtabmap
39 {
40 
41 #ifdef RTABMAP_ZED
42 
43 static cv::Mat slMat2cvMat(sl::Mat& input) {
44 #if ZED_SDK_MAJOR_VERSION < 3
45  //convert MAT_TYPE to CV_TYPE
46  int cv_type = -1;
47  switch (input.getDataType()) {
48  case sl::MAT_TYPE_32F_C1: cv_type = CV_32FC1; break;
49  case sl::MAT_TYPE_32F_C2: cv_type = CV_32FC2; break;
50  case sl::MAT_TYPE_32F_C3: cv_type = CV_32FC3; break;
51  case sl::MAT_TYPE_32F_C4: cv_type = CV_32FC4; break;
52  case sl::MAT_TYPE_8U_C1: cv_type = CV_8UC1; break;
53  case sl::MAT_TYPE_8U_C2: cv_type = CV_8UC2; break;
54  case sl::MAT_TYPE_8U_C3: cv_type = CV_8UC3; break;
55  case sl::MAT_TYPE_8U_C4: cv_type = CV_8UC4; break;
56  default: break;
57  }
58  // cv::Mat data requires a uchar* pointer. Therefore, we get the uchar1 pointer from sl::Mat (getPtr<T>())
59  //cv::Mat and sl::Mat will share the same memory pointer
60  return cv::Mat(input.getHeight(), input.getWidth(), cv_type, input.getPtr<sl::uchar1>(sl::MEM_CPU));
61 #else
62  //convert MAT_TYPE to CV_TYPE
63  int cv_type = -1;
64  switch (input.getDataType()) {
65  case sl::MAT_TYPE::F32_C1: cv_type = CV_32FC1; break;
66  case sl::MAT_TYPE::F32_C2: cv_type = CV_32FC2; break;
67  case sl::MAT_TYPE::F32_C3: cv_type = CV_32FC3; break;
68  case sl::MAT_TYPE::F32_C4: cv_type = CV_32FC4; break;
69  case sl::MAT_TYPE::U8_C1: cv_type = CV_8UC1; break;
70  case sl::MAT_TYPE::U8_C2: cv_type = CV_8UC2; break;
71  case sl::MAT_TYPE::U8_C3: cv_type = CV_8UC3; break;
72  case sl::MAT_TYPE::U8_C4: cv_type = CV_8UC4; break;
73  default: break;
74  }
75  // cv::Mat data requires a uchar* pointer. Therefore, we get the uchar1 pointer from sl::Mat (getPtr<T>())
76  //cv::Mat and sl::Mat will share the same memory pointer
77  return cv::Mat(input.getHeight(), input.getWidth(), cv_type, input.getPtr<sl::uchar1>(sl::MEM::CPU));
78 #endif
79 }
80 
81 Transform zedPoseToTransform(const sl::Pose & pose)
82 {
83  return Transform(
84  pose.pose_data.m[0], pose.pose_data.m[1], pose.pose_data.m[2], pose.pose_data.m[3],
85  pose.pose_data.m[4], pose.pose_data.m[5], pose.pose_data.m[6], pose.pose_data.m[7],
86  pose.pose_data.m[8], pose.pose_data.m[9], pose.pose_data.m[10], pose.pose_data.m[11]);
87 }
88 
89 #if ZED_SDK_MAJOR_VERSION < 3
90 IMU zedIMUtoIMU(const sl::IMUData & imuData, const Transform & imuLocalTransform)
91 {
92  sl::Orientation orientation = imuData.pose_data.getOrientation();
93 
94  //Convert zed imu orientation from camera frame to world frame ENU!
95  Transform opticalTransform(0, 0, 1, 0, -1, 0, 0, 0, 0, -1, 0, 0);
96  Transform orientationT(0,0,0, orientation.ox, orientation.oy, orientation.oz, orientation.ow);
97  orientationT = opticalTransform * orientationT;
98 
99  static double deg2rad = 0.017453293;
100  Eigen::Vector4d accT = Eigen::Vector4d(imuData.linear_acceleration.v[0], imuData.linear_acceleration.v[1], imuData.linear_acceleration.v[2], 1);
101  Eigen::Vector4d gyrT = Eigen::Vector4d(imuData.angular_velocity.v[0]*deg2rad, imuData.angular_velocity.v[1]*deg2rad, imuData.angular_velocity.v[2]*deg2rad, 1);
102 
103  cv::Mat orientationCov = (cv::Mat_<double>(3,3)<<
104  imuData.pose_covariance[21], imuData.pose_covariance[22], imuData.pose_covariance[23],
105  imuData.pose_covariance[27], imuData.pose_covariance[28], imuData.pose_covariance[29],
106  imuData.pose_covariance[33], imuData.pose_covariance[34], imuData.pose_covariance[35]);
107  cv::Mat angCov = (cv::Mat_<double>(3,3)<<
108  imuData.angular_velocity_convariance.r[0], imuData.angular_velocity_convariance.r[1], imuData.angular_velocity_convariance.r[2],
109  imuData.angular_velocity_convariance.r[3], imuData.angular_velocity_convariance.r[4], imuData.angular_velocity_convariance.r[5],
110  imuData.angular_velocity_convariance.r[6], imuData.angular_velocity_convariance.r[7], imuData.angular_velocity_convariance.r[8]);
111  cv::Mat accCov = (cv::Mat_<double>(3,3)<<
112  imuData.linear_acceleration_convariance.r[0], imuData.linear_acceleration_convariance.r[1], imuData.linear_acceleration_convariance.r[2],
113  imuData.linear_acceleration_convariance.r[3], imuData.linear_acceleration_convariance.r[4], imuData.linear_acceleration_convariance.r[5],
114  imuData.linear_acceleration_convariance.r[6], imuData.linear_acceleration_convariance.r[7], imuData.linear_acceleration_convariance.r[8]);
115 
116  Eigen::Quaternionf quat = orientationT.getQuaternionf();
117 
118  return IMU(
119  cv::Vec4d(quat.x(), quat.y(), quat.z(), quat.w()),
120  orientationCov,
121  cv::Vec3d(gyrT[0], gyrT[1], gyrT[2]),
122  angCov,
123  cv::Vec3d(accT[0], accT[1], accT[2]),
124  accCov,
125  imuLocalTransform);
126 }
127 #else
128 IMU zedIMUtoIMU(const sl::SensorsData & sensorData, const Transform & imuLocalTransform)
129 {
130  sl::Orientation orientation = sensorData.imu.pose.getOrientation();
131 
132  //Convert zed imu orientation from camera frame to world frame ENU!
133  Transform opticalTransform(0, 0, 1, 0, -1, 0, 0, 0, 0, -1, 0, 0);
134  Transform orientationT(0,0,0, orientation.ox, orientation.oy, orientation.oz, orientation.ow);
135  orientationT = opticalTransform * orientationT;
136 
137  static double deg2rad = 0.017453293;
138  Eigen::Vector4d accT = Eigen::Vector4d(sensorData.imu.linear_acceleration.v[0], sensorData.imu.linear_acceleration.v[1], sensorData.imu.linear_acceleration.v[2], 1);
139  Eigen::Vector4d gyrT = Eigen::Vector4d(sensorData.imu.angular_velocity.v[0]*deg2rad, sensorData.imu.angular_velocity.v[1]*deg2rad, sensorData.imu.angular_velocity.v[2]*deg2rad, 1);
140 
141  cv::Mat orientationCov = (cv::Mat_<double>(3,3)<<
142  sensorData.imu.pose_covariance.r[0], sensorData.imu.pose_covariance.r[1], sensorData.imu.pose_covariance.r[2],
143  sensorData.imu.pose_covariance.r[3], sensorData.imu.pose_covariance.r[4], sensorData.imu.pose_covariance.r[5],
144  sensorData.imu.pose_covariance.r[6], sensorData.imu.pose_covariance.r[7], sensorData.imu.pose_covariance.r[8]);
145  cv::Mat angCov = (cv::Mat_<double>(3,3)<<
146  sensorData.imu.angular_velocity_covariance.r[0], sensorData.imu.angular_velocity_covariance.r[1], sensorData.imu.angular_velocity_covariance.r[2],
147  sensorData.imu.angular_velocity_covariance.r[3], sensorData.imu.angular_velocity_covariance.r[4], sensorData.imu.angular_velocity_covariance.r[5],
148  sensorData.imu.angular_velocity_covariance.r[6], sensorData.imu.angular_velocity_covariance.r[7], sensorData.imu.angular_velocity_covariance.r[8]);
149  cv::Mat accCov = (cv::Mat_<double>(3,3)<<
150  sensorData.imu.linear_acceleration_covariance.r[0], sensorData.imu.linear_acceleration_covariance.r[1], sensorData.imu.linear_acceleration_covariance.r[2],
151  sensorData.imu.linear_acceleration_covariance.r[3], sensorData.imu.linear_acceleration_covariance.r[4], sensorData.imu.linear_acceleration_covariance.r[5],
152  sensorData.imu.linear_acceleration_covariance.r[6], sensorData.imu.linear_acceleration_covariance.r[7], sensorData.imu.linear_acceleration_covariance.r[8]);
153 
154  Eigen::Quaternionf quat = orientationT.getQuaternionf();
155 
156  return IMU(
157  cv::Vec4d(quat.x(), quat.y(), quat.z(), quat.w()),
158  orientationCov,
159  cv::Vec3d(gyrT[0], gyrT[1], gyrT[2]),
160  angCov,
161  cv::Vec3d(accT[0], accT[1], accT[2]),
162  accCov,
163  imuLocalTransform);
164 }
165 #endif
166 
167 class ZedIMUThread: public UThread
168 {
169 public:
170  ZedIMUThread(float rate, sl::Camera * zed, const Transform & imuLocalTransform, bool accurate)
171  {
172  UASSERT(rate > 0.0f);
173  UASSERT(zed != 0);
174  rate_ = rate;
175  zed_= zed;
176  accurate_ = accurate;
177  imuLocalTransform_ = imuLocalTransform;
178  }
179 private:
180  virtual void mainLoopBegin()
181  {
182  frameRateTimer_.start();
183  }
184  virtual void mainLoop()
185  {
186  double delay = 1000.0/double(rate_);
187  int sleepTime = delay - 1000.0f*frameRateTimer_.getElapsedTime();
188  if(sleepTime > 0)
189  {
190  if(accurate_)
191  {
192  if(sleepTime > 1)
193  {
194  uSleep(sleepTime-1);
195  }
196  // Add precision at the cost of a small overhead
197  delay/=1000.0;
198  while(frameRateTimer_.getElapsedTime() < delay-0.000001)
199  {
200  //
201  }
202  }
203  else
204  {
205  uSleep(sleepTime);
206  }
207  }
208  frameRateTimer_.start();
209 
210 #if ZED_SDK_MAJOR_VERSION < 3
211  sl::IMUData imudata;
212  bool res = zed_->getIMUData(imudata, sl::TIME_REFERENCE_IMAGE);
213  if(res == sl::SUCCESS && imudata.valid)
214  {
215  UEventsManager::post(new IMUEvent(zedIMUtoIMU(imudata, imuLocalTransform_), UTimer::now()));
216  }
217 #else
218  sl::SensorsData sensordata;
219  sl::ERROR_CODE res = zed_->getSensorsData(sensordata, sl::TIME_REFERENCE::IMAGE);
220  if(res == sl::ERROR_CODE::SUCCESS && sensordata.imu.is_available)
221  {
222  UEventsManager::post(new IMUEvent(zedIMUtoIMU(sensordata, imuLocalTransform_), UTimer::now()));
223  }
224 #endif
225  }
226  float rate_;
227  sl::Camera * zed_;
228  bool accurate_;
229  Transform imuLocalTransform_;
230  UTimer frameRateTimer_;
231 };
232 #endif
233 
234 bool CameraStereoZed::available()
235 {
236 #ifdef RTABMAP_ZED
237  return true;
238 #else
239  return false;
240 #endif
241 }
242 
243 CameraStereoZed::CameraStereoZed(
244  int deviceId,
245  int resolution,
246  int quality,
247  int sensingMode,
248  int confidenceThr,
249  bool computeOdometry,
250  float imageRate,
251  const Transform & localTransform,
252  bool selfCalibration,
253  bool odomForce3DoF,
254  int texturenessConfidenceThr) :
255  Camera(imageRate, localTransform)
256 #ifdef RTABMAP_ZED
257  ,
258  zed_(0),
259  src_(CameraVideo::kUsbDevice),
260  usbDevice_(deviceId),
261  svoFilePath_(""),
262  resolution_(resolution),
263  quality_(quality),
264  selfCalibration_(selfCalibration),
265  sensingMode_(sensingMode),
266  confidenceThr_(confidenceThr),
267  texturenessConfidenceThr_(texturenessConfidenceThr),
268  computeOdometry_(computeOdometry),
269  lost_(true),
270  force3DoF_(odomForce3DoF),
271  publishInterIMU_(false),
272  imuPublishingThread_(0)
273 #endif
274 {
275  UDEBUG("");
276 #ifdef RTABMAP_ZED
277 #if ZED_SDK_MAJOR_VERSION < 3
278  UASSERT(resolution_ >= sl::RESOLUTION_HD2K && resolution_ <sl::RESOLUTION_LAST);
279  UASSERT(quality_ >= sl::DEPTH_MODE_NONE && quality_ <sl::DEPTH_MODE_LAST);
280  UASSERT(sensingMode_ >= sl::SENSING_MODE_STANDARD && sensingMode_ <sl::SENSING_MODE_LAST);
281  UASSERT(confidenceThr_ >= 0 && confidenceThr_ <=100);
282 #else
283  sl::RESOLUTION res = static_cast<sl::RESOLUTION>(resolution_);
284  sl::DEPTH_MODE qual = static_cast<sl::DEPTH_MODE>(quality_);
285  sl::SENSING_MODE sens = static_cast<sl::SENSING_MODE>(sensingMode_);
286 
287  UASSERT(res >= sl::RESOLUTION::HD2K && res < sl::RESOLUTION::LAST);
288  UASSERT(qual >= sl::DEPTH_MODE::NONE && qual < sl::DEPTH_MODE::LAST);
289  UASSERT(sens >= sl::SENSING_MODE::STANDARD && sens < sl::SENSING_MODE::LAST);
290  UASSERT(confidenceThr_ >= 0 && confidenceThr_ <=100);
291  UASSERT(texturenessConfidenceThr_ >= 0 && texturenessConfidenceThr_ <=100);
292 #endif
293 #endif
294 }
295 
296 CameraStereoZed::CameraStereoZed(
297  const std::string & filePath,
298  int quality,
299  int sensingMode,
300  int confidenceThr,
301  bool computeOdometry,
302  float imageRate,
303  const Transform & localTransform,
304  bool selfCalibration,
305  bool odomForce3DoF,
306  int texturenessConfidenceThr) :
307  Camera(imageRate, localTransform)
308 #ifdef RTABMAP_ZED
309  ,
310  zed_(0),
311  src_(CameraVideo::kVideoFile),
312  usbDevice_(0),
313  svoFilePath_(filePath),
314  resolution_(2),
315  quality_(quality),
316  selfCalibration_(selfCalibration),
317  sensingMode_(sensingMode),
318  confidenceThr_(confidenceThr),
319  texturenessConfidenceThr_(texturenessConfidenceThr),
320  computeOdometry_(computeOdometry),
321  lost_(true),
322  force3DoF_(odomForce3DoF),
323  publishInterIMU_(false),
324  imuPublishingThread_(0)
325 #endif
326 {
327  UDEBUG("");
328 #ifdef RTABMAP_ZED
329 #if ZED_SDK_MAJOR_VERSION < 3
330  UASSERT(resolution_ >= sl::RESOLUTION_HD2K && resolution_ <sl::RESOLUTION_LAST);
331  UASSERT(quality_ >= sl::DEPTH_MODE_NONE && quality_ <sl::DEPTH_MODE_LAST);
332  UASSERT(sensingMode_ >= sl::SENSING_MODE_STANDARD && sensingMode_ <sl::SENSING_MODE_LAST);
333  UASSERT(confidenceThr_ >= 0 && confidenceThr_ <=100);
334 #else
335  sl::RESOLUTION res = static_cast<sl::RESOLUTION>(resolution_);
336  sl::DEPTH_MODE qual = static_cast<sl::DEPTH_MODE>(quality_);
337  sl::SENSING_MODE sens = static_cast<sl::SENSING_MODE>(sensingMode_);
338 
339  UASSERT(res >= sl::RESOLUTION::HD2K && res < sl::RESOLUTION::LAST);
340  UASSERT(qual >= sl::DEPTH_MODE::NONE && qual < sl::DEPTH_MODE::LAST);
341  UASSERT(sens >= sl::SENSING_MODE::STANDARD && sens < sl::SENSING_MODE::LAST);
342  UASSERT(confidenceThr_ >= 0 && confidenceThr_ <=100);
343  UASSERT(texturenessConfidenceThr_ >= 0 && texturenessConfidenceThr_ <=100);
344 #endif
345 #endif
346 }
347 
348 CameraStereoZed::~CameraStereoZed()
349 {
350 #ifdef RTABMAP_ZED
351  if(imuPublishingThread_)
352  {
353  imuPublishingThread_->join(true);
354  }
355  delete imuPublishingThread_;
356  delete zed_;
357 #endif
358 }
359 
360 void CameraStereoZed::publishInterIMU(bool enabled)
361 {
362 #ifdef RTABMAP_ZED
363  publishInterIMU_ = enabled;
364 #endif
365 }
366 
367 bool CameraStereoZed::init(const std::string & calibrationFolder, const std::string & cameraName)
368 {
369  UDEBUG("");
370 #ifdef RTABMAP_ZED
371  if(imuPublishingThread_)
372  {
373  imuPublishingThread_->join(true);
374  delete imuPublishingThread_;
375  imuPublishingThread_=0;
376  }
377  if(zed_)
378  {
379  delete zed_;
380  zed_ = 0;
381  }
382 
383  lost_ = true;
384 
385  sl::InitParameters param;
386  param.camera_resolution=static_cast<sl::RESOLUTION>(resolution_);
387  param.camera_fps=getImageRate();
388  param.depth_mode=(sl::DEPTH_MODE)quality_;
389 #if ZED_SDK_MAJOR_VERSION < 3
390  param.camera_linux_id=usbDevice_;
391  param.coordinate_units=sl::UNIT_METER;
392  param.coordinate_system=(sl::COORDINATE_SYSTEM)sl::COORDINATE_SYSTEM_IMAGE ;
393 #else
394  param.coordinate_units=sl::UNIT::METER;
395  param.coordinate_system=sl::COORDINATE_SYSTEM::IMAGE ;
396 #endif
397  param.sdk_verbose=true;
398  param.sdk_gpu_id=-1;
399  param.depth_minimum_distance=-1;
400  param.camera_disable_self_calib=!selfCalibration_;
401 
402  sl::ERROR_CODE r = sl::ERROR_CODE::SUCCESS;
403  if(src_ == CameraVideo::kVideoFile)
404  {
405  UINFO("svo file = %s", svoFilePath_.c_str());
406  zed_ = new sl::Camera(); // Use in SVO playback mode
407  #if ZED_SDK_MAJOR_VERSION < 3
408  param.svo_input_filename=svoFilePath_.c_str();
409 #else
410  param.input.setFromSVOFile(svoFilePath_.c_str());
411 #endif
412  r = zed_->open(param);
413  }
414  else
415  {
416 #if ZED_SDK_MAJOR_VERSION >= 3
417  param.input.setFromCameraID(usbDevice_);
418 #endif
419  UINFO("Resolution=%d imagerate=%f device=%d", resolution_, getImageRate(), usbDevice_);
420  zed_ = new sl::Camera(); // Use in Live Mode
421  r = zed_->open(param);
422  }
423 
424  if(r!=sl::ERROR_CODE::SUCCESS)
425  {
426  UERROR("Camera initialization failed: \"%s\"", toString(r).c_str());
427  delete zed_;
428  zed_ = 0;
429  return false;
430  }
431 
432 #if ZED_SDK_MAJOR_VERSION < 3
433  UINFO("Init ZED: Mode=%d Unit=%d CoordinateSystem=%d Verbose=false device=-1 minDist=-1 self-calibration=%s vflip=false",
434  quality_, sl::UNIT_METER, sl::COORDINATE_SYSTEM_IMAGE , selfCalibration_?"true":"false");
435 
436  if(quality_!=sl::DEPTH_MODE_NONE)
437  {
438  zed_->setConfidenceThreshold(confidenceThr_);
439  }
440 #else
441  UINFO("Init ZED: Mode=%d Unit=%d CoordinateSystem=%d Verbose=false device=-1 minDist=-1 self-calibration=%s vflip=false",
442  quality_, sl::UNIT::METER, sl::COORDINATE_SYSTEM::IMAGE , selfCalibration_?"true":"false");
443 #endif
444 
445 
446 
447 
448  UDEBUG("");
449 
450  if (computeOdometry_)
451  {
452 #if ZED_SDK_MAJOR_VERSION < 3
453  sl::TrackingParameters tparam;
454  tparam.enable_spatial_memory=false;
455  r = zed_->enableTracking(tparam);
456 #else
457  sl::PositionalTrackingParameters tparam;
458  tparam.enable_area_memory=false;
459  r = zed_->enablePositionalTracking(tparam);
460 #endif
461  if(r!=sl::ERROR_CODE::SUCCESS)
462  {
463  UERROR("Camera tracking initialization failed: \"%s\"", toString(r).c_str());
464  }
465  }
466 
467  sl::CameraInformation infos = zed_->getCameraInformation();
468  sl::CalibrationParameters *stereoParams = &(infos.calibration_parameters );
469  sl::Resolution res = stereoParams->left_cam.image_size;
470 
471  stereoModel_ = StereoCameraModel(
472  stereoParams->left_cam.fx,
473  stereoParams->left_cam.fy,
474  stereoParams->left_cam.cx,
475  stereoParams->left_cam.cy,
476  stereoParams->T[0],//baseline
477  this->getLocalTransform(),
478  cv::Size(res.width, res.height));
479 
480  UINFO("Calibration: fx=%f, fy=%f, cx=%f, cy=%f, baseline=%f, width=%d, height=%d, transform=%s",
481  stereoParams->left_cam.fx,
482  stereoParams->left_cam.fy,
483  stereoParams->left_cam.cx,
484  stereoParams->left_cam.cy,
485  stereoParams->T[0],//baseline
486  (int)res.width,
487  (int)res.height,
488  this->getLocalTransform().prettyPrint().c_str());
489 
490 #if ZED_SDK_MAJOR_VERSION < 3
491  if(infos.camera_model == sl::MODEL_ZED_M)
492 #else
493  if(infos.camera_model != sl::MODEL::ZED)
494 #endif
495  {
496  imuLocalTransform_ = this->getLocalTransform() * zedPoseToTransform(infos.camera_imu_transform).inverse();
497  UINFO("IMU local transform: %s (imu2cam=%s))",
498  imuLocalTransform_.prettyPrint().c_str(),
499  zedPoseToTransform(infos.camera_imu_transform).prettyPrint().c_str());
500 
501  if(publishInterIMU_)
502  {
503  imuPublishingThread_ = new ZedIMUThread(200, zed_, imuLocalTransform_, true);
504  imuPublishingThread_->start();
505  }
506  }
507 
508  return true;
509 #else
510  UERROR("CameraStereoZED: RTAB-Map is not built with ZED sdk support!");
511 #endif
512  return false;
513 }
514 
515 bool CameraStereoZed::isCalibrated() const
516 {
517 #ifdef RTABMAP_ZED
518  return stereoModel_.isValidForProjection();
519 #else
520  return false;
521 #endif
522 }
523 
524 std::string CameraStereoZed::getSerial() const
525 {
526 #ifdef RTABMAP_ZED
527  if(zed_)
528  {
529  return uFormat("%x", zed_->getCameraInformation ().serial_number);
530  }
531 #endif
532  return "";
533 }
534 
535 bool CameraStereoZed::odomProvided() const
536 {
537 #ifdef RTABMAP_ZED
538  return computeOdometry_;
539 #else
540  return false;
541 #endif
542 }
543 
544 bool CameraStereoZed::getPose(double stamp, Transform & pose, cv::Mat & covariance)
545 {
546 #ifdef RTABMAP_ZED
547 
548  if (computeOdometry_ && zed_)
549  {
550  sl::Pose p;
551 
552 #if ZED_SDK_MAJOR_VERSION < 3
553  if(!zed_->grab())
554  {
555  return false;
556  }
557  sl::TRACKING_STATE tracking_state = zed_->getPosition(p);
558  if (tracking_state == sl::TRACKING_STATE_OK)
559 #else
560  if(zed_->grab()!=sl::ERROR_CODE::SUCCESS)
561  {
562  return false;
563  }
564  sl::POSITIONAL_TRACKING_STATE tracking_state = zed_->getPosition(p);
565  if (tracking_state == sl::POSITIONAL_TRACKING_STATE::OK)
566 #endif
567  {
568  int trackingConfidence = p.pose_confidence;
569  // FIXME What does pose_confidence == -1 mean?
570  if (trackingConfidence>0)
571  {
572  pose = zedPoseToTransform(p);
573  if (!pose.isNull())
574  {
575  //transform from:
576  // x->right, y->down, z->forward
577  //to:
578  // x->forward, y->left, z->up
579  pose = this->getLocalTransform() * pose * this->getLocalTransform().inverse();
580  if(force3DoF_)
581  {
582  pose = pose.to3DoF();
583  }
584  if (lost_)
585  {
586  covariance = cv::Mat::eye(6, 6, CV_64FC1) * 9999.0f; // don't know transform with previous pose
587  lost_ = false;
588  UDEBUG("Init %s (var=%f)", pose.prettyPrint().c_str(), 9999.0f);
589  }
590  else
591  {
592  covariance = cv::Mat::eye(6, 6, CV_64FC1) * 1.0f / float(trackingConfidence);
593  UDEBUG("Run %s (var=%f)", pose.prettyPrint().c_str(), 1.0f / float(trackingConfidence));
594  }
595  return true;
596  }
597  else
598  {
599  covariance = cv::Mat::eye(6, 6, CV_64FC1) * 9999.0f; // lost
600  lost_ = true;
601  UWARN("ZED lost! (trackingConfidence=%d)", trackingConfidence);
602  }
603  }
604  else
605  {
606  covariance = cv::Mat::eye(6, 6, CV_64FC1) * 9999.0f; // lost
607  lost_ = true;
608  UWARN("ZED lost! (trackingConfidence=%d)", trackingConfidence);
609  }
610  }
611  else
612  {
613  UWARN("Tracking not ok: state=\"%s\"", toString(tracking_state).c_str());
614  }
615  }
616 #endif
617  return false;
618 }
619 
620 SensorData CameraStereoZed::captureImage(CameraInfo * info)
621 {
622  SensorData data;
623 #ifdef RTABMAP_ZED
624 #if ZED_SDK_MAJOR_VERSION < 3
625  sl::RuntimeParameters rparam((sl::SENSING_MODE)sensingMode_, quality_ > 0, quality_ > 0, sl::REFERENCE_FRAME_CAMERA);
626 #else
627  sl::RuntimeParameters rparam((sl::SENSING_MODE)sensingMode_, quality_ > 0, confidenceThr_, texturenessConfidenceThr_, sl::REFERENCE_FRAME::CAMERA);
628 #endif
629 
630  if(zed_)
631  {
632  UTimer timer;
633 #if ZED_SDK_MAJOR_VERSION < 3
634  bool res = zed_->grab(rparam);
635  while (src_ == CameraVideo::kUsbDevice && res!=sl::SUCCESS && timer.elapsed() < 2.0)
636  {
637  // maybe there is a latency with the USB, try again in 10 ms (for the next 2 seconds)
638  uSleep(10);
639  res = zed_->grab(rparam);
640  }
641 
642  if(res==sl::SUCCESS)
643 #else
644 
645  sl::ERROR_CODE res;
646  bool imuReceived = true;
647  do
648  {
649  res = zed_->grab(rparam);
650 
651  // If the sensor supports IMU, wait IMU to be available before sending data.
652  if(imuPublishingThread_ == 0 && !imuLocalTransform_.isNull())
653  {
654  sl::SensorsData imudatatmp;
655  res = zed_->getSensorsData(imudatatmp, sl::TIME_REFERENCE::IMAGE);
656  imuReceived = res == sl::ERROR_CODE::SUCCESS && imudatatmp.imu.is_available && imudatatmp.imu.timestamp.data_ns != 0;
657  }
658  }
659  while(src_ == CameraVideo::kUsbDevice && (res!=sl::ERROR_CODE::SUCCESS || !imuReceived) && timer.elapsed() < 2.0);
660 
661  if(res==sl::ERROR_CODE::SUCCESS)
662 #endif
663  {
664  // get left image
665  sl::Mat tmp;
666 #if ZED_SDK_MAJOR_VERSION < 3
667  zed_->retrieveImage(tmp,sl::VIEW_LEFT);
668 #else
669  zed_->retrieveImage(tmp,sl::VIEW::LEFT);
670 #endif
671  cv::Mat rgbaLeft = slMat2cvMat(tmp);
672 
673  cv::Mat left;
674  cv::cvtColor(rgbaLeft, left, cv::COLOR_BGRA2BGR);
675 
676  if(quality_ > 0)
677  {
678  // get depth image
679  cv::Mat depth;
680  sl::Mat tmp;
681 #if ZED_SDK_MAJOR_VERSION < 3
682  zed_->retrieveMeasure(tmp,sl::MEASURE_DEPTH);
683 #else
684  zed_->retrieveMeasure(tmp,sl::MEASURE::DEPTH);
685 #endif
686  slMat2cvMat(tmp).copyTo(depth);
687 
688  data = SensorData(left, depth, stereoModel_.left(), this->getNextSeqID(), UTimer::now());
689  }
690  else
691  {
692  // get right image
693 #if ZED_SDK_MAJOR_VERSION < 3
694  sl::Mat tmp;zed_->retrieveImage(tmp,sl::VIEW_RIGHT );
695 #else
696  sl::Mat tmp;zed_->retrieveImage(tmp,sl::VIEW::RIGHT );
697 #endif
698  cv::Mat rgbaRight = slMat2cvMat(tmp);
699  cv::Mat right;
700  cv::cvtColor(rgbaRight, right, cv::COLOR_BGRA2GRAY);
701 
702  data = SensorData(left, right, stereoModel_, this->getNextSeqID(), UTimer::now());
703  }
704 
705  if(imuPublishingThread_ == 0)
706  {
707 #if ZED_SDK_MAJOR_VERSION < 3
708  sl::IMUData imudata;
709  res = zed_->getIMUData(imudata, sl::TIME_REFERENCE_IMAGE);
710  if(res == sl::SUCCESS && imudata.valid)
711 #else
712  sl::SensorsData imudata;
713  res = zed_->getSensorsData(imudata, sl::TIME_REFERENCE::IMAGE);
714  if(res == sl::ERROR_CODE::SUCCESS && imudata.imu.is_available)
715 #endif
716  {
717  //ZED-Mini
718  data.setIMU(zedIMUtoIMU(imudata, imuLocalTransform_));
719  }
720  }
721 
722  if (computeOdometry_ && info)
723  {
724  sl::Pose pose;
725 #if ZED_SDK_MAJOR_VERSION < 3
726  sl::TRACKING_STATE tracking_state = zed_->getPosition(pose);
727  if (tracking_state == sl::TRACKING_STATE_OK)
728 #else
729  sl::POSITIONAL_TRACKING_STATE tracking_state = zed_->getPosition(pose);
730  if (tracking_state == sl::POSITIONAL_TRACKING_STATE::OK)
731 #endif
732  {
733  int trackingConfidence = pose.pose_confidence;
734  // FIXME What does pose_confidence == -1 mean?
735  if (trackingConfidence>0)
736  {
737  info->odomPose = zedPoseToTransform(pose);
738  if (!info->odomPose.isNull())
739  {
740  //transform from:
741  // x->right, y->down, z->forward
742  //to:
743  // x->forward, y->left, z->up
744  info->odomPose = this->getLocalTransform() * info->odomPose * this->getLocalTransform().inverse();
745  if(force3DoF_)
746  {
747  info->odomPose = info->odomPose.to3DoF();
748  }
749  if (lost_)
750  {
751  info->odomCovariance = cv::Mat::eye(6, 6, CV_64FC1) * 9999.0f; // don't know transform with previous pose
752  lost_ = false;
753  UDEBUG("Init %s (var=%f)", info->odomPose.prettyPrint().c_str(), 9999.0f);
754  }
755  else
756  {
757  info->odomCovariance = cv::Mat::eye(6, 6, CV_64FC1) * 1.0f / float(trackingConfidence);
758  UDEBUG("Run %s (var=%f)", info->odomPose.prettyPrint().c_str(), 1.0f / float(trackingConfidence));
759  }
760  }
761  else
762  {
763  info->odomCovariance = cv::Mat::eye(6, 6, CV_64FC1) * 9999.0f; // lost
764  lost_ = true;
765  UWARN("ZED lost! (trackingConfidence=%d)", trackingConfidence);
766  }
767  }
768  else
769  {
770  info->odomCovariance = cv::Mat::eye(6, 6, CV_64FC1) * 9999.0f; // lost
771  lost_ = true;
772  UWARN("ZED lost! (trackingConfidence=%d)", trackingConfidence);
773  }
774  }
775  else
776  {
777  UWARN("Tracking not ok: state=\"%s\"", toString(tracking_state).c_str());
778  }
779  }
780  }
781  else if(src_ == CameraVideo::kUsbDevice)
782  {
783  UERROR("CameraStereoZed: Failed to grab images after 2 seconds!");
784  }
785  else
786  {
787  UWARN("CameraStereoZed: end of stream is reached!");
788  }
789  }
790 #else
791  UERROR("CameraStereoZED: RTAB-Map is not built with ZED sdk support!");
792 #endif
793  return data;
794 }
795 
796 } // namespace rtabmap
rtabmap::CameraInfo::odomCovariance
cv::Mat odomCovariance
Definition: CameraInfo.h:70
UTimer
Definition: UTimer.h:46
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::CameraStereoZed::odomProvided
virtual bool odomProvided() const
Definition: CameraStereoZed.cpp:535
glm::quat
highp_quat quat
Quaternion of default single-precision floating-point numbers.
Definition: fwd.hpp:68
data
data
UConversion.h
Some conversion functions.
UASSERT
#define UASSERT(condition)
true
#define true
Definition: ConvertUTF.c:57
rtabmap::Transform::prettyPrint
std::string prettyPrint() const
Definition: Transform.cpp:316
rtabmap::Camera::getLocalTransform
const Transform & getLocalTransform() const
Definition: Camera.h:65
rtabmap::CameraStereoZed::getPose
virtual bool getPose(double stamp, Transform &pose, cv::Mat &covariance)
Definition: CameraStereoZed.cpp:544
rtabmap::CameraStereoZed::getSerial
virtual std::string getSerial() const
Definition: CameraStereoZed.cpp:524
rtabmap::Camera::getImageRate
float getImageRate() const
Definition: Camera.h:64
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::CameraStereoZed::isCalibrated
virtual bool isCalibrated() const
Definition: CameraStereoZed.cpp:515
UDEBUG
#define UDEBUG(...)
UERROR
#define UERROR(...)
UWARN
#define UWARN(...)
UTimer::now
static double now()
Definition: UTimer.cpp:80
rtabmap::SensorData::setIMU
void setIMU(const IMU &imu)
Definition: SensorData.h:289
rtabmap::Transform::to3DoF
Transform to3DoF() const
Definition: Transform.cpp:210
rtabmap::CameraStereoZed::CameraStereoZed
CameraStereoZed(int deviceId, int resolution=2, int quality=1, int sensingMode=0, int confidenceThr=100, bool computeOdometry=false, float imageRate=0.0f, const Transform &localTransform=Transform::getIdentity(), bool selfCalibration=true, bool odomForce3DoF=false, int texturenessConfidenceThr=90)
Definition: CameraStereoZed.cpp:243
rtabmap::CameraStereoZed::publishInterIMU
void publishInterIMU(bool enabled)
Definition: CameraStereoZed.cpp:360
rtabmap::CameraInfo::odomPose
Transform odomPose
Definition: CameraInfo.h:69
rtabmap::CameraStereoZed::init
virtual bool init(const std::string &calibrationFolder=".", const std::string &cameraName="")
Definition: CameraStereoZed.cpp:367
uFormat
std::string UTILITE_EXP uFormat(const char *fmt,...)
Definition: UConversion.cpp:365
res
res
rtabmap::Transform::inverse
Transform inverse() const
Definition: Transform.cpp:178
rtabmap::CameraStereoZed::captureImage
virtual SensorData captureImage(CameraInfo *info=0)
Definition: CameraStereoZed.cpp:620
glm::orientation
GLM_FUNC_DECL detail::tmat4x4< T, P > orientation(detail::tvec3< T, P > const &Normal, detail::tvec3< T, P > const &Up)
UINFO
#define UINFO(...)

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