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