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.getNanoseconds())/10e8);
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_ == 1 || resolution_ == 2) // HD2K, HD1080
290  {
291  resolution_ -= 1; // HD2K=0, HD1080=1
292  }
293  if(resolution_ == 3) // HD1200
294  {
295  resolution_ = 1; // HD1080=1
296  }
297  if(resolution_ == 4 || resolution_ == -1)
298  {
299  resolution_ = 2; // HD720=2
300  }
301  else if(resolution_ == 5 || resolution_ == 6) // SVGA, VGA
302  {
303  resolution_ = 3; // VGA=3
304  }
305 #else // ZED=4
306  if(resolution_ == -1)
307  {
308  resolution_ = int(sl::RESOLUTION::AUTO); // AUTO
309  }
310 #endif
311 
312 #if ZED_SDK_MAJOR_VERSION < 3
313  UASSERT(resolution_ >= sl::RESOLUTION_HD2K && resolution_ <sl::RESOLUTION_LAST);
314  UASSERT(quality_ >= sl::DEPTH_MODE_NONE && quality_ <sl::DEPTH_MODE_LAST);
315  UASSERT(sensingMode_ >= sl::SENSING_MODE_STANDARD && sensingMode_ <sl::SENSING_MODE_LAST);
316  UASSERT(confidenceThr_ >= 0 && confidenceThr_ <=100);
317 #else
318  sl::RESOLUTION res = static_cast<sl::RESOLUTION>(resolution_);
319  sl::DEPTH_MODE qual = static_cast<sl::DEPTH_MODE>(quality_);
320 
321  UASSERT(qual >= sl::DEPTH_MODE::NONE && qual < sl::DEPTH_MODE::LAST);
322 #if ZED_SDK_MAJOR_VERSION < 4
323  UASSERT(res >= sl::RESOLUTION::HD2K && res < sl::RESOLUTION::LAST);
324  sl::SENSING_MODE sens = static_cast<sl::SENSING_MODE>(sensingMode_);
325  UASSERT(sens >= sl::SENSING_MODE::STANDARD && sens < sl::SENSING_MODE::LAST);
326 #else
327  UASSERT(res >= sl::RESOLUTION::HD4K && res < sl::RESOLUTION::LAST);
328  UASSERT(sensingMode_ >= 0 && sensingMode_ < 2);
329 #endif
330  UASSERT(confidenceThr_ >= 0 && confidenceThr_ <=100);
331  UASSERT(texturenessConfidenceThr_ >= 0 && texturenessConfidenceThr_ <=100);
332 #endif
333 #endif
334 }
335 
336 CameraStereoZed::CameraStereoZed(
337  const std::string & filePath,
338  int quality,
339  int sensingMode,
340  int confidenceThr,
341  bool computeOdometry,
342  float imageRate,
343  const Transform & localTransform,
344  bool selfCalibration,
345  bool odomForce3DoF,
346  int texturenessConfidenceThr) :
347  Camera(imageRate, localTransform)
348 #ifdef RTABMAP_ZED
349  ,
350  zed_(0),
351  src_(CameraVideo::kVideoFile),
352  usbDevice_(0),
353  svoFilePath_(filePath),
354 #if ZED_SDK_MAJOR_VERSION < 3
355  resolution_(sl::RESOLUTION_HD720),
356 #elif ZED_SDK_MAJOR_VERSION < 4
357  resolution_(sl::RESOLUTION::HD720),
358 #else
359  resolution_(int(sl::RESOLUTION::AUTO)),
360 #endif
361  quality_(quality),
362  selfCalibration_(selfCalibration),
363  sensingMode_(sensingMode),
364  confidenceThr_(confidenceThr),
365  texturenessConfidenceThr_(texturenessConfidenceThr),
366  computeOdometry_(computeOdometry),
367  lost_(true),
368  force3DoF_(odomForce3DoF),
369  imuPublishingThread_(0)
370 #endif
371 {
372  UDEBUG("");
373 #ifdef RTABMAP_ZED
374 #if ZED_SDK_MAJOR_VERSION < 3
375  UASSERT(resolution_ >= sl::RESOLUTION_HD2K && resolution_ <sl::RESOLUTION_LAST);
376  UASSERT(quality_ >= sl::DEPTH_MODE_NONE && quality_ <sl::DEPTH_MODE_LAST);
377  UASSERT(sensingMode_ >= sl::SENSING_MODE_STANDARD && sensingMode_ <sl::SENSING_MODE_LAST);
378  UASSERT(confidenceThr_ >= 0 && confidenceThr_ <=100);
379 #else
380  sl::RESOLUTION res = static_cast<sl::RESOLUTION>(resolution_);
381  sl::DEPTH_MODE qual = static_cast<sl::DEPTH_MODE>(quality_);
382 
383  UASSERT(res >= sl::RESOLUTION::HD2K && res < sl::RESOLUTION::LAST);
384  UASSERT(qual >= sl::DEPTH_MODE::NONE && qual < sl::DEPTH_MODE::LAST);
385 #if ZED_SDK_MAJOR_VERSION < 4
386  sl::SENSING_MODE sens = static_cast<sl::SENSING_MODE>(sensingMode_);
387  UASSERT(sens >= sl::SENSING_MODE::STANDARD && sens < sl::SENSING_MODE::LAST);
388 #else
389  UASSERT(sensingMode_ >= 0 && sensingMode_ < 2);
390 #endif
391  UASSERT(confidenceThr_ >= 0 && confidenceThr_ <=100);
392  UASSERT(texturenessConfidenceThr_ >= 0 && texturenessConfidenceThr_ <=100);
393 #endif
394 #endif
395 }
396 
397 CameraStereoZed::~CameraStereoZed()
398 {
399 #ifdef RTABMAP_ZED
400  if(imuPublishingThread_)
401  {
402  imuPublishingThread_->join(true);
403  }
404  delete imuPublishingThread_;
405  delete zed_;
406 #endif
407 }
408 
409 bool CameraStereoZed::init(const std::string & calibrationFolder, const std::string & cameraName)
410 {
411  UDEBUG("");
412 #ifdef RTABMAP_ZED
413  if(imuPublishingThread_)
414  {
415  imuPublishingThread_->join(true);
416  delete imuPublishingThread_;
417  imuPublishingThread_=0;
418  }
419  if(zed_)
420  {
421  delete zed_;
422  zed_ = 0;
423  }
424 
425  lost_ = true;
426 
427  sl::InitParameters param;
428  param.camera_resolution=static_cast<sl::RESOLUTION>(resolution_);
429  param.camera_fps=getImageRate();
430  param.depth_mode=(sl::DEPTH_MODE)quality_;
431 #if ZED_SDK_MAJOR_VERSION < 3
432  param.camera_linux_id=usbDevice_;
433  param.coordinate_units=sl::UNIT_METER;
434  param.coordinate_system=(sl::COORDINATE_SYSTEM)sl::COORDINATE_SYSTEM_IMAGE ;
435 #else
436  param.coordinate_units=sl::UNIT::METER;
437  param.coordinate_system=sl::COORDINATE_SYSTEM::IMAGE ;
438 #endif
439  param.sdk_verbose=true;
440  param.sdk_gpu_id=-1;
441  param.depth_minimum_distance=-1;
442  param.camera_disable_self_calib=!selfCalibration_;
443 
444  sl::ERROR_CODE r = sl::ERROR_CODE::SUCCESS;
445  if(src_ == CameraVideo::kVideoFile)
446  {
447  UINFO("svo file = %s", svoFilePath_.c_str());
448  zed_ = new sl::Camera(); // Use in SVO playback mode
449  #if ZED_SDK_MAJOR_VERSION < 3
450  param.svo_input_filename=svoFilePath_.c_str();
451 #else
452  param.input.setFromSVOFile(svoFilePath_.c_str());
453 #endif
454  r = zed_->open(param);
455  }
456  else
457  {
458 #if ZED_SDK_MAJOR_VERSION >= 3
459  param.input.setFromCameraID(usbDevice_);
460 #endif
461  UINFO("Resolution=%d imagerate=%f device=%d", resolution_, getImageRate(), usbDevice_);
462  zed_ = new sl::Camera(); // Use in Live Mode
463  r = zed_->open(param);
464  }
465 
466  if(r!=sl::ERROR_CODE::SUCCESS)
467  {
468  UERROR("Camera initialization failed: \"%s\"", toString(r).c_str());
469  delete zed_;
470  zed_ = 0;
471  return false;
472  }
473 
474 #if ZED_SDK_MAJOR_VERSION < 3
475  UINFO("Init ZED: Mode=%d Unit=%d CoordinateSystem=%d Verbose=false device=-1 minDist=-1 self-calibration=%s vflip=false",
476  quality_, sl::UNIT_METER, sl::COORDINATE_SYSTEM_IMAGE , selfCalibration_?"true":"false");
477 
478  if(quality_!=sl::DEPTH_MODE_NONE)
479  {
480  zed_->setConfidenceThreshold(confidenceThr_);
481  }
482 #else
483  UINFO("Init ZED: Mode=%d Unit=%d CoordinateSystem=%d Verbose=false device=-1 minDist=-1 self-calibration=%s vflip=false",
484  quality_, sl::UNIT::METER, sl::COORDINATE_SYSTEM::IMAGE , selfCalibration_?"true":"false");
485 #endif
486 
487 
488 
489 
490  UDEBUG("");
491 
492  if (computeOdometry_)
493  {
494 #if ZED_SDK_MAJOR_VERSION < 3
495  sl::TrackingParameters tparam;
496  tparam.enable_spatial_memory=false;
497  r = zed_->enableTracking(tparam);
498 #else
499  sl::PositionalTrackingParameters tparam;
500  tparam.enable_area_memory=false;
501  r = zed_->enablePositionalTracking(tparam);
502 #endif
503  if(r!=sl::ERROR_CODE::SUCCESS)
504  {
505  UERROR("Camera tracking initialization failed: \"%s\"", toString(r).c_str());
506  }
507  }
508 
509  sl::CameraInformation infos = zed_->getCameraInformation();
510 #if ZED_SDK_MAJOR_VERSION < 4
511  sl::CalibrationParameters *stereoParams = &(infos.calibration_parameters );
512 #else
513  sl::CalibrationParameters *stereoParams = &(infos.camera_configuration.calibration_parameters );
514 #endif
515  sl::Resolution res = stereoParams->left_cam.image_size;
516 
517 #if ZED_SDK_MAJOR_VERSION < 4
518  stereoModel_ = StereoCameraModel(
519  stereoParams->left_cam.fx,
520  stereoParams->left_cam.fy,
521  stereoParams->left_cam.cx,
522  stereoParams->left_cam.cy,
523  stereoParams->T[0],//baseline
524  this->getLocalTransform(),
525  cv::Size(res.width, res.height));
526 #else
527  stereoModel_ = StereoCameraModel(
528  stereoParams->left_cam.fx,
529  stereoParams->left_cam.fy,
530  stereoParams->left_cam.cx,
531  stereoParams->left_cam.cy,
532  stereoParams->getCameraBaseline(),
533  this->getLocalTransform(),
534  cv::Size(res.width, res.height));
535 #endif
536 
537 #if ZED_SDK_MAJOR_VERSION < 4
538  UINFO("Calibration: fx=%f, fy=%f, cx=%f, cy=%f, baseline=%f, width=%d, height=%d, transform=%s",
539  stereoParams->left_cam.fx,
540  stereoParams->left_cam.fy,
541  stereoParams->left_cam.cx,
542  stereoParams->left_cam.cy,
543  stereoParams->T[0],//baseline
544  (int)res.width,
545  (int)res.height,
546  this->getLocalTransform().prettyPrint().c_str());
547 #else
548  UINFO("Calibration: fx=%f, fy=%f, cx=%f, cy=%f, baseline=%f, width=%d, height=%d, transform=%s",
549  stereoParams->left_cam.fx,
550  stereoParams->left_cam.fy,
551  stereoParams->left_cam.cx,
552  stereoParams->left_cam.cy,
553  stereoParams->getCameraBaseline(),
554  (int)res.width,
555  (int)res.height,
556  this->getLocalTransform().prettyPrint().c_str());
557 #endif
558 
559 #if ZED_SDK_MAJOR_VERSION < 3
560  if(infos.camera_model == sl::MODEL_ZED_M)
561 #else
562  if(infos.camera_model != sl::MODEL::ZED)
563 #endif
564  {
565 #if ZED_SDK_MAJOR_VERSION < 4
566  imuLocalTransform_ = this->getLocalTransform() * zedPoseToTransform(infos.camera_imu_transform).inverse();
567 #else
568  imuLocalTransform_ = this->getLocalTransform() * zedPoseToTransform(infos.sensors_configuration.camera_imu_transform).inverse();
569 #endif
570 
571 #if ZED_SDK_MAJOR_VERSION < 4
572  UINFO("IMU local transform: %s (imu2cam=%s))",
573  imuLocalTransform_.prettyPrint().c_str(),
574  zedPoseToTransform(infos.camera_imu_transform).prettyPrint().c_str());
575 #else
576  UINFO("IMU local transform: %s (imu2cam=%s))",
577  imuLocalTransform_.prettyPrint().c_str(),
578  zedPoseToTransform(infos.sensors_configuration.camera_imu_transform).prettyPrint().c_str());
579 #endif
580  if(isInterIMUPublishing())
581  {
582  imuPublishingThread_ = new ZedIMUThread(200, zed_, this, imuLocalTransform_, true);
583  imuPublishingThread_->start();
584  }
585  }
586 
587  return true;
588 #else
589  UERROR("CameraStereoZED: RTAB-Map is not built with ZED sdk support!");
590 #endif
591  return false;
592 }
593 
594 bool CameraStereoZed::isCalibrated() const
595 {
596 #ifdef RTABMAP_ZED
597  return stereoModel_.isValidForProjection();
598 #else
599  return false;
600 #endif
601 }
602 
603 std::string CameraStereoZed::getSerial() const
604 {
605 #ifdef RTABMAP_ZED
606  if(zed_)
607  {
608  return uFormat("%x", zed_->getCameraInformation ().serial_number);
609  }
610 #endif
611  return "";
612 }
613 
614 bool CameraStereoZed::odomProvided() const
615 {
616 #ifdef RTABMAP_ZED
617  return computeOdometry_;
618 #else
619  return false;
620 #endif
621 }
622 
623 bool CameraStereoZed::getPose(double stamp, Transform & pose, cv::Mat & covariance, double maxWaitTime)
624 {
625 #ifdef RTABMAP_ZED
626 
627  if (computeOdometry_ && zed_)
628  {
629  sl::Pose p;
630 
631 #if ZED_SDK_MAJOR_VERSION < 3
632  if(!zed_->grab())
633  {
634  return false;
635  }
636  sl::TRACKING_STATE tracking_state = zed_->getPosition(p);
637  if (tracking_state == sl::TRACKING_STATE_OK)
638 #else
639  if(zed_->grab()!=sl::ERROR_CODE::SUCCESS)
640  {
641  return false;
642  }
643  sl::POSITIONAL_TRACKING_STATE tracking_state = zed_->getPosition(p);
644  if (tracking_state == sl::POSITIONAL_TRACKING_STATE::OK)
645 #endif
646  {
647  int trackingConfidence = p.pose_confidence;
648  // FIXME What does pose_confidence == -1 mean?
649  if (trackingConfidence>0)
650  {
651  pose = zedPoseToTransform(p);
652  if (!pose.isNull())
653  {
654  //transform from:
655  // x->right, y->down, z->forward
656  //to:
657  // x->forward, y->left, z->up
658  pose = this->getLocalTransform() * pose * this->getLocalTransform().inverse();
659  if(force3DoF_)
660  {
661  pose = pose.to3DoF();
662  }
663  if (lost_)
664  {
665  covariance = cv::Mat::eye(6, 6, CV_64FC1) * 9999.0f; // don't know transform with previous pose
666  lost_ = false;
667  UDEBUG("Init %s (var=%f)", pose.prettyPrint().c_str(), 9999.0f);
668  }
669  else
670  {
671  covariance = cv::Mat::eye(6, 6, CV_64FC1) * 1.0f / float(trackingConfidence);
672  UDEBUG("Run %s (var=%f)", pose.prettyPrint().c_str(), 1.0f / float(trackingConfidence));
673  }
674  return true;
675  }
676  else
677  {
678  covariance = cv::Mat::eye(6, 6, CV_64FC1) * 9999.0f; // lost
679  lost_ = true;
680  UWARN("ZED lost! (trackingConfidence=%d)", trackingConfidence);
681  }
682  }
683  else
684  {
685  covariance = cv::Mat::eye(6, 6, CV_64FC1) * 9999.0f; // lost
686  lost_ = true;
687  UWARN("ZED lost! (trackingConfidence=%d)", trackingConfidence);
688  }
689  }
690  else
691  {
692  UWARN("Tracking not ok: state=\"%s\"", toString(tracking_state).c_str());
693  }
694  }
695 #endif
696  return false;
697 }
698 
699 SensorData CameraStereoZed::captureImage(SensorCaptureInfo * info)
700 {
701  SensorData data;
702 #ifdef RTABMAP_ZED
703 #if ZED_SDK_MAJOR_VERSION < 3
704  sl::RuntimeParameters rparam((sl::SENSING_MODE)sensingMode_, quality_ > 0, quality_ > 0, sl::REFERENCE_FRAME_CAMERA);
705 #elif ZED_SDK_MAJOR_VERSION < 4
706  sl::RuntimeParameters rparam((sl::SENSING_MODE)sensingMode_, quality_ > 0, confidenceThr_, texturenessConfidenceThr_, sl::REFERENCE_FRAME::CAMERA);
707 #else
708  sl::RuntimeParameters rparam(quality_ > 0, sensingMode_ == 1, confidenceThr_, texturenessConfidenceThr_, sl::REFERENCE_FRAME::CAMERA);
709 #endif
710 
711  if(zed_)
712  {
713  UTimer timer;
714 #if ZED_SDK_MAJOR_VERSION < 3
715  bool res = zed_->grab(rparam);
716  while (src_ == CameraVideo::kUsbDevice && res!=sl::SUCCESS && timer.elapsed() < 2.0)
717  {
718  // maybe there is a latency with the USB, try again in 10 ms (for the next 2 seconds)
719  uSleep(10);
720  res = zed_->grab(rparam);
721  }
722 
723  if(res==sl::SUCCESS)
724 #else
725 
726  sl::ERROR_CODE res;
727  sl::Timestamp timestamp;
728  bool imuReceived = true;
729  do
730  {
731  res = zed_->grab(rparam);
732  timestamp = zed_->getTimestamp(sl::TIME_REFERENCE::IMAGE);
733 
734  // If the sensor supports IMU, wait IMU to be available before sending data.
735  if(imuPublishingThread_ == 0 && !imuLocalTransform_.isNull())
736  {
737  sl::SensorsData imudatatmp;
738  res = zed_->getSensorsData(imudatatmp, sl::TIME_REFERENCE::IMAGE);
739  imuReceived = res == sl::ERROR_CODE::SUCCESS && imudatatmp.imu.is_available && imudatatmp.imu.timestamp.getNanoseconds() != 0;
740  }
741  }
742  while(src_ == CameraVideo::kUsbDevice && (res!=sl::ERROR_CODE::SUCCESS || !imuReceived) && timer.elapsed() < 2.0);
743 
744  if(res==sl::ERROR_CODE::SUCCESS)
745 #endif
746  {
747  // get left image
748  sl::Mat tmp;
749 #if ZED_SDK_MAJOR_VERSION < 3
750  zed_->retrieveImage(tmp,sl::VIEW_LEFT);
751 #else
752  zed_->retrieveImage(tmp,sl::VIEW::LEFT);
753 #endif
754  cv::Mat rgbaLeft = slMat2cvMat(tmp);
755 
756  cv::Mat left;
757  cv::cvtColor(rgbaLeft, left, cv::COLOR_BGRA2BGR);
758 
759  if(quality_ > 0)
760  {
761  // get depth image
762  cv::Mat depth;
763  sl::Mat tmp;
764 #if ZED_SDK_MAJOR_VERSION < 3
765  zed_->retrieveMeasure(tmp,sl::MEASURE_DEPTH);
766 #else
767  zed_->retrieveMeasure(tmp,sl::MEASURE::DEPTH);
768 #endif
769  slMat2cvMat(tmp).copyTo(depth);
770 #if ZED_SDK_MAJOR_VERSION < 3
771  data = SensorData(left, depth, stereoModel_.left(), this->getNextSeqID(), UTimer::now());
772 #else
773  data = SensorData(left, depth, stereoModel_.left(), this->getNextSeqID(), double(timestamp.getNanoseconds())/10e8);
774 #endif
775  }
776  else
777  {
778  // get right image
779 #if ZED_SDK_MAJOR_VERSION < 3
780  sl::Mat tmp;zed_->retrieveImage(tmp,sl::VIEW_RIGHT );
781 #else
782  sl::Mat tmp;zed_->retrieveImage(tmp,sl::VIEW::RIGHT );
783 #endif
784  cv::Mat rgbaRight = slMat2cvMat(tmp);
785  cv::Mat right;
786  cv::cvtColor(rgbaRight, right, cv::COLOR_BGRA2GRAY);
787 #if ZED_SDK_MAJOR_VERSION < 3
788  data = SensorData(left, right, stereoModel_, this->getNextSeqID(), UTimer::now());
789 #else
790  data = SensorData(left, right, stereoModel_, this->getNextSeqID(), double(timestamp.getNanoseconds())/10e8);
791 #endif
792  }
793 
794  if(imuPublishingThread_ == 0)
795  {
796 #if ZED_SDK_MAJOR_VERSION < 3
797  sl::IMUData imudata;
798  res = zed_->getIMUData(imudata, sl::TIME_REFERENCE_IMAGE);
799  if(res == sl::SUCCESS && imudata.valid)
800 #else
801  sl::SensorsData imudata;
802  res = zed_->getSensorsData(imudata, sl::TIME_REFERENCE::IMAGE);
803  if(res == sl::ERROR_CODE::SUCCESS && imudata.imu.is_available)
804 #endif
805  {
806  //ZED-Mini
807  data.setIMU(zedIMUtoIMU(imudata, imuLocalTransform_));
808  }
809  }
810 
811  if (computeOdometry_ && info)
812  {
813  sl::Pose pose;
814 #if ZED_SDK_MAJOR_VERSION < 3
815  sl::TRACKING_STATE tracking_state = zed_->getPosition(pose);
816  if (tracking_state == sl::TRACKING_STATE_OK)
817 #else
818  sl::POSITIONAL_TRACKING_STATE tracking_state = zed_->getPosition(pose);
819  if (tracking_state == sl::POSITIONAL_TRACKING_STATE::OK)
820 #endif
821  {
822  int trackingConfidence = pose.pose_confidence;
823  // FIXME What does pose_confidence == -1 mean?
824  info->odomPose = zedPoseToTransform(pose);
825  if (!info->odomPose.isNull())
826  {
827 #if ZED_SDK_MAJOR_VERSION >=3
828  if(pose.timestamp != timestamp)
829  {
830  UWARN("Pose retrieve doesn't have same stamp (%ld) than grabbed image (%ld)", pose.timestamp, timestamp);
831  }
832 #endif
833 
834  //transform from:
835  // x->right, y->down, z->forward
836  //to:
837  // x->forward, y->left, z->up
838  info->odomPose = this->getLocalTransform() * info->odomPose * this->getLocalTransform().inverse();
839  if(force3DoF_)
840  {
841  info->odomPose = info->odomPose.to3DoF();
842  }
843  if (lost_)
844  {
845  info->odomCovariance = cv::Mat::eye(6, 6, CV_64FC1) * 9999.0f; // don't know transform with previous pose
846  lost_ = false;
847  UDEBUG("Init %s (var=%f)", info->odomPose.prettyPrint().c_str(), 9999.0f);
848  }
849  else if(trackingConfidence==0)
850  {
851  info->odomCovariance = cv::Mat::eye(6, 6, CV_64FC1) * 9999.0f; // lost
852  lost_ = true;
853  UWARN("ZED lost! (trackingConfidence=%d)", trackingConfidence);
854  }
855  else
856  {
857  info->odomCovariance = cv::Mat::eye(6, 6, CV_64FC1) * 1.0f / float(trackingConfidence);
858  UDEBUG("Run %s (var=%f)", info->odomPose.prettyPrint().c_str(), 1.0f / float(trackingConfidence));
859  }
860  }
861  else
862  {
863  info->odomCovariance = cv::Mat::eye(6, 6, CV_64FC1) * 9999.0f; // lost
864  lost_ = true;
865  UWARN("ZED lost! (trackingConfidence=%d)", trackingConfidence);
866  }
867  }
868  else
869  {
870  UWARN("Tracking not ok: state=\"%s\"", toString(tracking_state).c_str());
871  }
872  }
873  }
874  else if(src_ == CameraVideo::kUsbDevice)
875  {
876  UERROR("CameraStereoZed: Failed to grab images after 2 seconds!");
877  }
878  else
879  {
880  UWARN("CameraStereoZed: end of stream is reached!");
881  }
882  }
883 #else
884  UERROR("CameraStereoZED: RTAB-Map is not built with ZED sdk support!");
885 #endif
886  return data;
887 }
888 
889 void CameraStereoZed::postInterIMUPublic(const IMU & imu, double stamp)
890 {
891  postInterIMU(imu, stamp);
892 }
893 
894 } // namespace rtabmap
rtabmap::SensorData
Definition: SensorData.h:51
int
int
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::CameraStereoZed::CameraStereoZed
CameraStereoZed(int deviceId, int resolution=-1, 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
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
CameraStereoZed.h
rtabmap::CameraStereoZed::getPose
virtual bool getPose(double stamp, Transform &pose, cv::Mat &covariance, double maxWaitTime=0.0)
Definition: CameraStereoZed.cpp:623
if
if(ret >=0) *info
rtabmap::CameraStereoZed::captureImage
virtual SensorData captureImage(SensorCaptureInfo *info=0)
Definition: CameraStereoZed.cpp:699
rtabmap::SensorCapture::getLocalTransform
const Transform & getLocalTransform() const
Definition: SensorCapture.h:62
rtabmap::CameraStereoZed::~CameraStereoZed
virtual ~CameraStereoZed()
Definition: CameraStereoZed.cpp:397
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:614
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
sl
Definition: CameraStereoZed.h:34
rtabmap::CameraStereoZed::isCalibrated
virtual bool isCalibrated() const
Definition: CameraStereoZed.cpp:594
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:603
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:889
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(...)
else
else
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:409
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 Feb 13 2025 03:44:51