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

rtabmap
Author(s): Mathieu Labbe
autogenerated on Thu Jul 25 2024 02:50:07