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

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