CameraStereoZedOC.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 
29 #include <rtabmap/utilite/UTimer.h>
33 
34 #ifdef RTABMAP_ZEDOC
35 #define VIDEO_MOD_AVAILABLE
36 #define SENSORS_MOD_AVAILABLE
37 #include <zed-open-capture/videocapture.hpp>
38 #include <zed-open-capture/sensorcapture.hpp>
39 #include "SimpleIni.h"
40 
42 //
43 // Copyright (c) 2018, STEREOLABS.
44 //
45 // All rights reserved.
46 //
47 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
48 // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
49 // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
50 // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
51 // OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
52 // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
53 // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
54 // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
55 // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
56 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
57 // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
58 //
60 
61 inline std::vector<std::string> &split(const std::string &s, char delim, std::vector<std::string> &elems) {
62  std::stringstream ss(s);
63  std::string item;
64  while (getline(ss, item, delim)) {
65  elems.push_back(item);
66  }
67  return elems;
68 }
69 
70 inline std::vector<std::string> split(const std::string &s, char delim) {
71  std::vector<std::string> elems;
72  split(s, delim, elems);
73  return elems;
74 }
75 
76 class ConfManager {
77 public:
78 
79  ConfManager(std::string filename) {
80  filename_ = filename;
81  ini_.SetUnicode();
82  SI_Error rc = ini_.LoadFile(filename_.c_str());
83  is_opened_ = !(rc < 0);
84  }
85 
86  ~ConfManager() {
87  //if (is_opened_) ini_.SaveFile(filename_.c_str());
88  }
89 
90  float getValue(std::string key, float default_value = -1) {
91  if (is_opened_) {
92  std::vector<std::string> elems;
93  split(key, ':', elems);
94 
95  return atof(ini_.GetValue(elems.front().c_str(), elems.back().c_str(), std::to_string(default_value).c_str()));
96  } else
97  return -1.f;
98  }
99 
100  void setValue(std::string key, float value) {
101  if (is_opened_) {
102  std::vector<std::string> elems;
103  split(key, ':', elems);
104 
105  /*SI_Error rc = */ini_.SetValue(elems.front().c_str(), elems.back().c_str(), std::to_string(value).c_str());
106  }
107  }
108 
109  inline bool isOpened() {
110  return is_opened_;
111  }
112 
113 private:
114  std::string filename_;
115  bool is_opened_;
116  CSimpleIniA ini_;
117 };
118 
119 bool checkFile(std::string path) {
120  std::ifstream f(path.c_str());
121  return f.good();
122 }
123 
124 static inline std::string getRootHiddenDir() {
125 #ifdef WIN32
126 
127 #ifdef UNICODE
128  wchar_t szPath[MAX_PATH];
129 #else
130  TCHAR szPath[MAX_PATH];
131 #endif
132 
133  if (!SUCCEEDED(SHGetFolderPath(NULL, CSIDL_COMMON_APPDATA, NULL, 0, szPath)))
134  return "";
135 
136  char snfile_path[MAX_PATH];
137 
138 #ifndef UNICODE
139 
140  size_t newsize = strlen(szPath) + 1;
141  wchar_t * wcstring = new wchar_t[newsize];
142  // Convert char* string to a wchar_t* string.
143  size_t convertedChars = 0;
144  mbstowcs_s(&convertedChars, wcstring, newsize, szPath, _TRUNCATE);
145  wcstombs(snfile_path, wcstring, MAX_PATH);
146 #else
147  wcstombs(snfile_path, szPath, MAX_PATH);
148 #endif
149 
150  std::string filename(snfile_path);
151  filename += "\\Stereolabs\\";
152 
153 #else //LINUX
154  std::string homepath = getenv("HOME");
155  std::string filename = homepath + "/zed/";
156 #endif
157 
158  return filename;
159 }
160 
161 /*return the path to the Sl ZED hidden dir*/
162 static inline std::string getHiddenDir() {
163  std::string filename = getRootHiddenDir();
164 #ifdef WIN32
165  filename += "settings\\";
166 #else //LINUX
167  filename += "settings/";
168 #endif
169  return filename;
170 }
171 
172 bool downloadCalibrationFile(unsigned int serial_number, std::string &calibration_file) {
173 #ifndef _WIN32
174  std::string path = getHiddenDir();
175  char specific_name[128];
176  sprintf(specific_name, "SN%d.conf", serial_number);
177  calibration_file = path + specific_name;
178  if (!checkFile(calibration_file)) {
179  std::string cmd;
180  int res;
181 
182  // Create download folder
183  cmd = "mkdir -p " + path;
184  res = system(cmd.c_str());
185 
186  // Download the file
187  std::string url("'https://calib.stereolabs.com/?SN=");
188 
189  cmd = "wget " + url + std::to_string(serial_number) + "' -O " + calibration_file;
190  std::cout << cmd << std::endl;
191  res = system(cmd.c_str());
192 
193  if( res == EXIT_FAILURE )
194  {
195  std::cerr << "Error downloading the calibration file" << std::endl;
196  return false;
197  }
198 
199  if (!checkFile(calibration_file)) {
200  std::cerr << "Invalid calibration file" << std::endl;
201  return false;
202  }
203  }
204 #else
205  std::string path = getHiddenDir();
206  char specific_name[128];
207  sprintf(specific_name, "SN%d.conf", serial_number);
208  calibration_file = path + specific_name;
209  if (!checkFile(calibration_file)) {
210  TCHAR *settingFolder = new TCHAR[path.size() + 1];
211  settingFolder[path.size()] = 0;
212  std::copy(path.begin(), path.end(), settingFolder);
213  SHCreateDirectoryEx(NULL, settingFolder, NULL); //recursive creation
214 
215  std::string url("https://calib.stereolabs.com/?SN=");
216  url += std::to_string(serial_number);
217  TCHAR *address = new TCHAR[url.size() + 1];
218  address[url.size()] = 0;
219  std::copy(url.begin(), url.end(), address);
220  TCHAR *calibPath = new TCHAR[calibration_file.size() + 1];
221  calibPath[calibration_file.size()] = 0;
222  std::copy(calibration_file.begin(), calibration_file.end(), calibPath);
223 
224  HRESULT hr = URLDownloadToFile(NULL, address, calibPath, 0, NULL);
225  if (hr != 0) {
226  std::cout << "Fail to download calibration file" << std::endl;
227  return false;
228  }
229 
230  if (!checkFile(calibration_file)) {
231  std::cout << "Invalid calibration file" << std::endl;
232  return false;
233  }
234  }
235 #endif
236 
237  return true;
238 }
239 
240 bool initCalibration(std::string calibration_file, cv::Size image_size, rtabmap::StereoCameraModel & model, const rtabmap::Transform & localTransform) {
241 
242  if (!checkFile(calibration_file)) {
243  std::cout << "Calibration file missing." << std::endl;
244  return false;
245  }
246 
247  // Open camera configuration file
248  ConfManager camerareader(calibration_file.c_str());
249  if (!camerareader.isOpened())
250  return false;
251 
252  std::string resolution_str;
253  switch ((int) image_size.width) {
254  case 2208:
255  resolution_str = "2k";
256  break;
257  case 1920:
258  resolution_str = "fhd";
259  break;
260  case 1280:
261  resolution_str = "hd";
262  break;
263  case 672:
264  resolution_str = "vga";
265  break;
266  default:
267  resolution_str = "hd";
268  break;
269  }
270 
271  // Get translations
272  float T_[3];
273  T_[0] = camerareader.getValue("stereo:baseline", 0.0f);
274  T_[1] = camerareader.getValue("stereo:ty_" + resolution_str, 0.f);
275  if(T_[1] == 0.f)
276  {
277  T_[1] = camerareader.getValue("stereo:ty", 0.f);
278  }
279  T_[2] = camerareader.getValue("stereo:tz_" + resolution_str, 0.f);
280  if(T_[2] == 0.f)
281  {
282  T_[2] = camerareader.getValue("stereo:tz", 0.f);
283  }
284 
285  // Get left parameters
286  float left_cam_cx = camerareader.getValue("left_cam_" + resolution_str + ":cx", 0.0f);
287  float left_cam_cy = camerareader.getValue("left_cam_" + resolution_str + ":cy", 0.0f);
288  float left_cam_fx = camerareader.getValue("left_cam_" + resolution_str + ":fx", 0.0f);
289  float left_cam_fy = camerareader.getValue("left_cam_" + resolution_str + ":fy", 0.0f);
290  float left_cam_k1 = camerareader.getValue("left_cam_" + resolution_str + ":k1", 0.0f);
291  float left_cam_k2 = camerareader.getValue("left_cam_" + resolution_str + ":k2", 0.0f);
292  float left_cam_p1 = camerareader.getValue("left_cam_" + resolution_str + ":p1", 0.0f);
293  float left_cam_p2 = camerareader.getValue("left_cam_" + resolution_str + ":p2", 0.0f);
294  float left_cam_k3 = camerareader.getValue("left_cam_" + resolution_str + ":k3", 0.0f);
295 
296  // Get right parameters
297  float right_cam_cx = camerareader.getValue("right_cam_" + resolution_str + ":cx", 0.0f);
298  float right_cam_cy = camerareader.getValue("right_cam_" + resolution_str + ":cy", 0.0f);
299  float right_cam_fx = camerareader.getValue("right_cam_" + resolution_str + ":fx", 0.0f);
300  float right_cam_fy = camerareader.getValue("right_cam_" + resolution_str + ":fy", 0.0f);
301  float right_cam_k1 = camerareader.getValue("right_cam_" + resolution_str + ":k1", 0.0f);
302  float right_cam_k2 = camerareader.getValue("right_cam_" + resolution_str + ":k2", 0.0f);
303  float right_cam_p1 = camerareader.getValue("right_cam_" + resolution_str + ":p1", 0.0f);
304  float right_cam_p2 = camerareader.getValue("right_cam_" + resolution_str + ":p2", 0.0f);
305  float right_cam_k3 = camerareader.getValue("right_cam_" + resolution_str + ":k3", 0.0f);
306 
307  // (Linux only) Safety check A: Wrong "." or "," reading in file conf.
308 #ifndef _WIN32
309  if (right_cam_k1 == 0 && left_cam_k1 == 0 && left_cam_k2 == 0 && right_cam_k2 == 0) {
310  UERROR("ZED File invalid");
311 
312  std::string cmd = "rm " + calibration_file;
313  int res = system(cmd.c_str());
314  if( res == EXIT_FAILURE )
315  {
316  return false;
317  }
318  return false;
319  }
320 #endif
321 
322  // Get rotations
323  cv::Mat R_zed = (cv::Mat_<double>(1, 3) << camerareader.getValue("stereo:rx_" + resolution_str, 0.f), camerareader.getValue("stereo:cv_" + resolution_str, 0.f), camerareader.getValue("stereo:rz_" + resolution_str, 0.f));
324  //R_zed *= -1.f; // FIXME: we had to invert T below, do we need to do this with R? I don't see much difference looking at the disparity image
325  cv::Mat R;
326 
327  cv::Rodrigues(R_zed /*in*/, R /*out*/);
328 
329  cv::Mat distCoeffs_left, distCoeffs_right;
330 
331  // Left
332  cv::Mat cameraMatrix_left = (cv::Mat_<double>(3, 3) << left_cam_fx, 0, left_cam_cx, 0, left_cam_fy, left_cam_cy, 0, 0, 1);
333  distCoeffs_left = (cv::Mat_<double>(1, 5) << left_cam_k1, left_cam_k2, left_cam_p1, left_cam_p2, left_cam_k3);
334 
335  // Right
336  cv::Mat cameraMatrix_right = (cv::Mat_<double>(3, 3) << right_cam_fx, 0, right_cam_cx, 0, right_cam_fy, right_cam_cy, 0, 0, 1);
337  distCoeffs_right = (cv::Mat_<double>(1, 5) << right_cam_k1, right_cam_k2, right_cam_p1, right_cam_p2, right_cam_k3);
338 
339  // Stereo
340  cv::Mat T = (cv::Mat_<double>(3, 1) << T_[0], T_[1], T_[2]);
341  T /= -1000.f; // convert in meters, inverted to get positive baseline
342  //std::cout << " Camera Matrix L: \n" << cameraMatrix_left << std::endl << std::endl;
343  //std::cout << " Camera Matrix R: \n" << cameraMatrix_right << std::endl << std::endl;
344  //std::cout << " Camera Rotation: \n" << R << std::endl << std::endl;
345  //std::cout << " Camera Translation: \n" << T << std::endl << std::endl;
346 
347  cv::Mat R1, R2, P1, P2, Q;
348  cv::stereoRectify(cameraMatrix_left, distCoeffs_left, cameraMatrix_right, distCoeffs_right, image_size, R, T,
349  R1, R2, P1, P2, Q, cv::CALIB_ZERO_DISPARITY, 0, image_size);
350 
352  image_size, cameraMatrix_left, distCoeffs_left, R1, P1,
353  image_size, cameraMatrix_right, distCoeffs_right, R2, P2,
354  R, T, cv::Mat(), cv::Mat(), localTransform);
355  return true;
356 }
357 
358 #endif
359 
360 namespace rtabmap
361 {
362 
363 #ifdef RTABMAP_ZEDOC
364 
365 class ZedOCThread: public UThread
366 {
367 public:
368  ZedOCThread(sl_oc::sensors::SensorCapture* sensCap, const Transform & imuLocalTransform)
369  {
370  sensCap_= sensCap;
371  imuLocalTransform_ = imuLocalTransform;
372  }
373 
374  void getIMU(
375  const double & stamp,
376  IMU & imu,
377  int maxWaitTimeMs)
378  {
379  imu = IMU();
380  if(imuBuffer_.empty())
381  {
382  return;
383  }
384 
385  // Interpolate imu
386  cv::Vec3d acc;
387  cv::Vec3d gyr;
388 
389  int waitTry = 0;
390  imuMutex_.lock();
391  while(maxWaitTimeMs > 0 && imuBuffer_.rbegin()->first < stamp && waitTry < maxWaitTimeMs)
392  {
393  imuMutex_.unlock();
394  ++waitTry;
395  uSleep(1);
396  imuMutex_.lock();
397  }
398  bool set = false;
399  if(imuBuffer_.rbegin()->first < stamp)
400  {
401  if(maxWaitTimeMs>0)
402  {
403  UWARN("Could not find imu data to interpolate at image time %f after waiting %d ms (last is %f)...", stamp, maxWaitTimeMs, imuBuffer_.rbegin()->first);
404  }
405  }
406  else
407  {
408  std::map<double, std::pair<cv::Vec3f, cv::Vec3f> >::const_iterator iterB = imuBuffer_.lower_bound(stamp);
409  std::map<double, std::pair<cv::Vec3f, cv::Vec3f> >::const_iterator iterA = iterB;
410  if(iterA != imuBuffer_.begin())
411  {
412  iterA = --iterA;
413  }
414  if(iterB == imuBuffer_.end())
415  {
416  iterB = --iterB;
417  }
418  if(iterA == iterB && stamp == iterA->first)
419  {
420  acc[0] = iterA->second.first[0];
421  acc[1] = iterA->second.first[1];
422  acc[2] = iterA->second.first[2];
423  gyr[0] = iterA->second.second[0];
424  gyr[1] = iterA->second.second[1];
425  gyr[2] = iterA->second.second[2];
426  set = true;
427  }
428  else if(stamp >= iterA->first && stamp <= iterB->first)
429  {
430  float t = (stamp-iterA->first) / (iterB->first-iterA->first);
431  acc[0] = iterA->second.first[0] + t*(iterB->second.first[0] - iterA->second.first[0]);
432  acc[1] = iterA->second.first[1] + t*(iterB->second.first[1] - iterA->second.first[1]);
433  acc[2] = iterA->second.first[2] + t*(iterB->second.first[2] - iterA->second.first[2]);
434  gyr[0] = iterA->second.second[0] + t*(iterB->second.second[0] - iterA->second.second[0]);
435  gyr[1] = iterA->second.second[1] + t*(iterB->second.second[1] - iterA->second.second[1]);
436  gyr[2] = iterA->second.second[2] + t*(iterB->second.second[2] - iterA->second.second[2]);
437  set = true;
438  }
439  else
440  {
441  if(stamp < iterA->first)
442  {
443  UDEBUG("Could not find imu data to interpolate at image time %f (earliest is %f). Are sensors synchronized? (may take some time to be synchronized...)", stamp, iterA->first);
444  }
445  else
446  {
447  UDEBUG("Could not find imu data to interpolate at image time %f (between %f and %f). Are sensors synchronized? (may take some time to be synchronized...)", stamp, iterA->first, iterB->first);
448  }
449  }
450  }
451  imuMutex_.unlock();
452 
453  if(set)
454  {
455  imu = IMU(gyr, cv::Mat::eye(3, 3, CV_64FC1),
456  acc, cv::Mat::eye(3, 3, CV_64FC1),
457  imuLocalTransform_);
458  }
459 
460  return;
461  }
462 private:
463  virtual void mainLoop()
464  {
465  // ----> Get IMU data
466  const sl_oc::sensors::data::Imu imuData = sensCap_->getLastIMUData(2000);
467 
468  // Process data only if valid
469  if(imuData.valid == sl_oc::sensors::data::Imu::NEW_VAL ) // Uncomment to use only data syncronized with the video frames
470  {
471  UScopeMutex sm(imuMutex_);
472  static double deg2rad = 0.017453293;
473  std::pair<cv::Vec3d, cv::Vec3d> imu(
474  cv::Vec3d(imuData.aX, imuData.aY, imuData.aZ),
475  cv::Vec3d(imuData.gX*deg2rad, imuData.gY*deg2rad, imuData.gZ*deg2rad));
476 
477  double stamp = double(imuData.timestamp)/10e8;
478  if(!imuBuffer_.empty() && imuBuffer_.rbegin()->first > stamp)
479  {
480  UWARN("IMU data not received in order, reset buffer! (previous=%f new=%f)", imuBuffer_.rbegin()->first, stamp);
481  imuBuffer_.clear();
482  }
483  imuBuffer_.insert(imuBuffer_.end(), std::make_pair(stamp, imu));
484  if(imuBuffer_.size() > 1000)
485  {
486  imuBuffer_.erase(imuBuffer_.begin());
487  }
488  }
489 
490  }
491  sl_oc::sensors::SensorCapture* sensCap_;
492  Transform imuLocalTransform_;
493  UMutex imuMutex_;
494  std::map<double, std::pair<cv::Vec3f, cv::Vec3f> > imuBuffer_;
495 };
496 
497 #endif
498 
500 {
501 #ifdef RTABMAP_ZEDOC
502  return true;
503 #else
504  return false;
505 #endif
506 }
507 
509  int deviceId,
510  int resolution,
511  float imageRate,
512  const Transform & localTransform) :
513  Camera(imageRate, localTransform)
514 #ifdef RTABMAP_ZEDOC
515  ,
516  zed_(0),
517  sensors_(0),
518  imuThread_(0),
519  usbDevice_(deviceId),
520  resolution_(resolution),
521  lastStamp_(0)
522 #endif
523 {
524  UDEBUG("");
525 #ifdef RTABMAP_ZEDOC
526 
527  sl_oc::video::RESOLUTION res = static_cast<sl_oc::video::RESOLUTION>(resolution_);
528  UASSERT(res >= sl_oc::video::RESOLUTION::HD2K && res < sl_oc::video::RESOLUTION::LAST);
529 #endif
530 }
531 
533 {
534 #ifdef RTABMAP_ZEDOC
535  if(imuThread_)
536  {
537  imuThread_->join(true);
538  delete imuThread_;
539  }
540  delete zed_;
541  delete sensors_;
542 #endif
543 }
544 
545 bool CameraStereoZedOC::init(const std::string & calibrationFolder, const std::string & cameraName)
546 {
547  UDEBUG("");
548 #ifdef RTABMAP_ZEDOC
549  if(imuThread_)
550  {
551  imuThread_->join(true);
552  delete imuThread_;
553  imuThread_=0;
554  }
555  if(zed_)
556  {
557  delete zed_;
558  zed_ = 0;
559  }
560  if(sensors_)
561  {
562  delete sensors_;
563  sensors_ = 0;
564  }
565  lastStamp_ = 0;
566 
567  // ----> Set Video parameters
568  sl_oc::video::VideoParams params;
569  params.res = static_cast<sl_oc::video::RESOLUTION>(resolution_);
570 
571  params.fps = sl_oc::video::FPS::FPS_15;
572  if(this->getImageRate() > 60)
573  {
574  params.fps = sl_oc::video::FPS::FPS_100;
575  }
576  else if(this->getImageRate() > 30)
577  {
578  params.fps = sl_oc::video::FPS::FPS_60;
579  }
580  else if(this->getImageRate() > 15)
581  {
582  params.fps = sl_oc::video::FPS::FPS_30;
583  }
584 
586  {
587  params.verbose = sl_oc::VERBOSITY::INFO;
588  }
589  else if(ULogger::level() <= ULogger::kWarning)
590  {
591  params.verbose = sl_oc::VERBOSITY::WARNING;
592  }
593  // <---- Set Video parameters
594 
595  // ----> Create Video Capture
596  zed_ = new sl_oc::video::VideoCapture(params);
597  if( !zed_->initializeVideo(usbDevice_) )
598  {
599  UERROR("Cannot open camera video capture. Set log level <= info for more details.");
600 
601  delete zed_;
602  zed_ = 0;
603  return false;
604  }
605  int sn = zed_->getSerialNumber();
606  UINFO("Connected to camera sn: %d", sn);
607  // <---- Create Video Capture
608 
609  // ----> Retrieve calibration file from Stereolabs server
610  std::string calibration_file;
611  // ZED Calibration
612  unsigned int serial_number = sn;
613  // Download camera calibration file
614  if( !downloadCalibrationFile(serial_number, calibration_file) )
615  {
616  UERROR("Could not load calibration file from Stereolabs servers");
617  delete zed_;
618  zed_ = 0;
619  return false;
620  }
621  UINFO("Calibration file found. Loading...");
622 
623  // ----> Frame size
624  int w,h;
625  zed_->getFrameSize(w,h);
626  // <---- Frame size
627 
628  // ----> Initialize calibration
629  if(initCalibration(calibration_file, cv::Size(w/2,h), stereoModel_, this->getLocalTransform()))
630  {
632  {
633  std::cout << "Calibration left:" << std::endl << stereoModel_.left() << std::endl;
634  std::cout << "Calibration right:" << std::endl << stereoModel_.right() << std::endl;
635  }
636  stereoModel_.initRectificationMap();
637  }
638 
639  // ----> Create a Sensors Capture object
640  sensors_ = new sl_oc::sensors::SensorCapture((sl_oc::VERBOSITY)params.verbose);
641  if( !sensors_->initializeSensors(serial_number) ) // Note: we use the serial number acquired by the VideoCapture object
642  {
643  UERROR("Cannot open sensors capture. Set log level <= info for more details.");
644  delete sensors_;
645  sensors_ = 0;
646  }
647  else
648  {
649  UINFO("Sensors Capture connected to camera sn: %d", sensors_->getSerialNumber());
650  UINFO("Wait max 5 sec to see if the camera has imu...");
651  // Check is IMU data is available
652  UTimer timer;
653  while(timer.elapsed() < 5 &&
654  sensors_->getLastIMUData().valid != sl_oc::sensors::data::Imu::NEW_VAL)
655  {
656  // wait 5 sec to see if we can get an imu stream...
657  uSleep(100);
658  }
659  if(timer.elapsed() > 5)
660  {
661  UINFO("Camera doesn't have IMU sensor");
662  }
663  else
664  {
665  UINFO("Camera has IMU");
666 
667  // Start the sensor capture thread. Note: since sensor data can be retrieved at 400Hz and video data frequency is
668  // minor (max 100Hz), we use a separated thread for sensors.
669  // Transform based on ZED2: x->down, y->right, z->backward
670  Transform imuLocalTransform_ = this->getLocalTransform() * Transform(0,1,0,0, 1,0,0,0, 0,0,-1,0);
671  //std::cout << imuLocalTransform_ << std::endl;
672  imuThread_ = new ZedOCThread(sensors_, imuLocalTransform_);
673  // <---- Create Sensors Capture
674 
675  // ----> Enable video/sensors synchronization
676  if(!zed_->enableSensorSync(sensors_))
677  {
678  UWARN("Failed to enable image/imu synchronization");
679  }
680  // <---- Enable video/sensors synchronization
681 
682  imuThread_->start();
683  }
684  }
685 
686  return true;
687 #else
688  UERROR("CameraStereoZEDOC: RTAB-Map is not built with ZED Open Capture support!");
689 #endif
690  return false;
691 }
692 
694 {
695 #ifdef RTABMAP_ZEDOC
696  return stereoModel_.isValidForProjection();
697 #else
698  return false;
699 #endif
700 }
701 
702 std::string CameraStereoZedOC::getSerial() const
703 {
704 #ifdef RTABMAP_ZEDOC
705  if(zed_)
706  {
707  return uFormat("%x", zed_->getSerialNumber());
708  }
709 #endif
710  return "";
711 }
712 
714 {
716 #ifdef RTABMAP_ZEDOC
717  // Get a new frame from camera
718  if(zed_)
719  {
720  UTimer timer;
721 
722  bool imuReceived = imuThread_!=0;
723  bool warned = false;
724  do
725  {
726  const sl_oc::video::Frame frame = zed_->getLastFrame();
727 
728  if(frame.data!=nullptr && frame.timestamp!=lastStamp_)
729  {
730  lastStamp_ = frame.timestamp;
731 
732  double stamp = double(lastStamp_)/10e8;
733 
734  // If the sensor supports IMU, wait IMU to be available before sending data.
735  IMU imu;
736  if(imuThread_)
737  {
738  imuThread_->getIMU(stamp, imu, 10);
739  imuReceived = !imu.empty();
740  if(!imuReceived && !warned && timer.elapsed() > 1.0)
741  {
742  UWARN("Waiting for synchronized imu (this can take several seconds when camera has been just started)...");
743  warned = true;
744  }
745  }
746 
747  if(imuReceived)
748  {
749  cv::Mat frameBGR, left, right;
750 
751  // ----> Conversion from YUV 4:2:2 to BGR for visualization
752  cv::Mat frameYUV = cv::Mat( frame.height, frame.width, CV_8UC2, frame.data );
753  cv::cvtColor(frameYUV,frameBGR,cv::COLOR_YUV2BGR_YUYV);
754  // <---- Conversion from YUV 4:2:2 to BGR for visualization
755 
756  // ----> Extract left and right images from side-by-side
757  left = frameBGR(cv::Rect(0, 0, frameBGR.cols / 2, frameBGR.rows));
758  cv::cvtColor(frameBGR(cv::Rect(frameBGR.cols / 2, 0, frameBGR.cols / 2, frameBGR.rows)),right,cv::COLOR_BGR2GRAY);
759  // <---- Extract left and right images from side-by-side
760 
761  if(stereoModel_.isValidForRectification())
762  {
763  left = stereoModel_.left().rectifyImage(left);
764  right = stereoModel_.right().rectifyImage(right);
765  }
766 
767  data = SensorData(left, right, stereoModel_, this->getNextSeqID(), stamp);
768 
769  if(!imu.empty())
770  {
771  data.setIMU(imu);
772  }
773  }
774  }
775  else
776  {
777  UERROR("CameraStereoZEDOC: Cannot get frame from the camera since 100 msec!");
778  imuReceived = true;
779  }
780  }
781  while(!imuReceived && timer.elapsed() < 15.0);
782 
783  // ----> If the frame is valid we can convert, rectify and display it
784  if(!imuReceived)
785  {
786  UERROR("CameraStereoZEDOC: Cannot get synchronized IMU with camera for 15 sec!");
787  }
788  }
789 #else
790  UERROR("CameraStereoZEDOC: RTAB-Map is not built with ZED Open Capture support!");
791 #endif
792  return data;
793 }
794 
795 } // namespace rtabmap
rtabmap::SensorData
Definition: SensorData.h:51
w
RowVector3d w
SimpleIni.h
UINFO
#define UINFO(...)
rtabmap::StereoCameraModel
Definition: StereoCameraModel.h:35
rtabmap::IMU::empty
bool empty() const
Definition: IMU.h:67
set
timer
h
const double h
CSimpleIniTempl
Definition: SimpleIni.h:293
rtabmap::CameraStereoZedOC::captureImage
virtual SensorData captureImage(SensorCaptureInfo *info=0)
Definition: CameraStereoZedOC.cpp:713
rtabmap::SensorCapture::getLocalTransform
const Transform & getLocalTransform() const
Definition: SensorCapture.h:62
res
cout<< "Here is the matrix m:"<< endl<< m<< endl;Matrix< ptrdiff_t, 3, 1 > res
UTimer.h
rtabmap::CameraStereoZedOC::available
static bool available()
Definition: CameraStereoZedOC.cpp:499
R1
SO3 R1
rtabmap::SensorCaptureInfo
Definition: SensorCaptureInfo.h:36
rtabmap::IMU
Definition: IMU.h:19
data
int data[]
UScopeMutex
Definition: UMutex.h:157
UConversion.h
Some conversion functions.
UMutex
Definition: UMutex.h:54
filename
filename
rtabmap::CameraStereoZedOC::getSerial
virtual std::string getSerial() const
Definition: CameraStereoZedOC.cpp:702
ULogger::kWarning
@ kWarning
Definition: ULogger.h:252
ULogger::kInfo
@ kInfo
Definition: ULogger.h:252
UASSERT
#define UASSERT(condition)
rtabmap::CameraStereoZedOC::CameraStereoZedOC
CameraStereoZedOC(int deviceId, int resolution=3, float imageRate=0.0f, const Transform &localTransform=Transform::getIdentity())
Definition: CameraStereoZedOC.cpp:508
params
SmartProjectionParams params(gtsam::HESSIAN, gtsam::ZERO_ON_DEGENERACY)
Eigen::Triplet
rtabmap::CameraStereoZedOC::isCalibrated
virtual bool isCalibrated() const
Definition: CameraStereoZedOC.cpp:693
rtabmap::Camera
Definition: Camera.h:43
rtabmap::Camera::getImageRate
float getImageRate() const
Definition: Camera.h:49
CameraStereoZedOC.h
path
path
UWARN
#define UWARN(...)
f
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
uFormat
std::string UTILITE_EXPORT uFormat(const char *fmt,...)
Definition: UConversion.cpp:365
timer::elapsed
double elapsed() const
ULogger::level
static ULogger::Level level()
Definition: ULogger.h:340
SI_Error
SI_Error
Definition: SimpleIni.h:235
Eigen::Quaternion
rtabmap::Transform
Definition: Transform.h:41
R2
SO3 R2
UThread
Definition: UThread.h:86
rtabmap::CameraStereoZedOC::~CameraStereoZedOC
virtual ~CameraStereoZedOC()
Definition: CameraStereoZedOC.cpp:532
UThread.h
UDEBUG
#define UDEBUG(...)
uSleep
void uSleep(unsigned int ms)
Definition: Posix/UThreadC.h:23
UTimer
Definition: UTimer.h:46
split
void split(const G &g, const PredecessorMap< KEY > &tree, G &Ab1, G &Ab2)
url
url
cmd
string cmd
rtabmap::CameraStereoZedOC::init
virtual bool init(const std::string &calibrationFolder=".", const std::string &cameraName="")
Definition: CameraStereoZedOC.cpp:545
t
Point2 t(10, 10)
glm::to_string
GLM_FUNC_DECL std::string to_string(genType const &x)
rtabmap
Definition: CameraARCore.cpp:35
UEventsManager.h
UERROR
#define UERROR(...)
trace.model
model
Definition: trace.py:4
rtabmap::SensorCapture::getNextSeqID
int getNextSeqID()
Definition: SensorCapture.h:83
R
R


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