CameraImages.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/CameraImages.h>
29 #include <rtabmap/utilite/UConversion.h>
30 #include <rtabmap/utilite/UDirectory.h>
31 #include <rtabmap/utilite/UStl.h>
32 #include <rtabmap/utilite/UFile.h>
33 #include <rtabmap/utilite/UThreadC.h>
34 #include <rtabmap/core/util3d.h>
35 #include <rtabmap/core/util3d_filtering.h>
36 #include <rtabmap/core/Graph.h>
37 #include <opencv2/imgproc/types_c.h>
38 #include <fstream>
39 
40 namespace rtabmap
41 {
42 
43 CameraImages::CameraImages() :
44  _startAt(0),
45  _maxFrames(0),
46  _refreshDir(false),
47  _rectifyImages(false),
48  _bayerMode(-1),
49  _isDepth(false),
50  _depthScaleFactor(1.0f),
51  _count(0),
52  _framesPublished(0),
53  _dir(0),
54  _countScan(0),
55  _scanDir(0),
56  _scanLocalTransform(Transform::getIdentity()),
57  _scanMaxPts(0),
58  _depthFromScan(false),
59  _depthFromScanFillHoles(1),
60  _depthFromScanFillHolesFromBorder(false),
61  _filenamesAreTimestamps(false),
62  _hasConfigForEachFrame(false),
63  _syncImageRateWithStamps(true),
64  _odometryFormat(0),
65  _groundTruthFormat(0),
66  _maxPoseTimeDiff(0.02),
67  _captureDelay(0.0)
68  {}
69 CameraImages::CameraImages(const std::string & path,
70  float imageRate,
71  const Transform & localTransform) :
72  Camera(imageRate, localTransform),
73  _path(path),
74  _startAt(0),
75  _maxFrames(0),
76  _refreshDir(false),
77  _rectifyImages(false),
78  _bayerMode(-1),
79  _isDepth(false),
80  _depthScaleFactor(1.0f),
81  _count(0),
82  _framesPublished(0),
83  _dir(0),
84  _countScan(0),
85  _scanDir(0),
86  _scanLocalTransform(Transform::getIdentity()),
87  _scanMaxPts(0),
88  _depthFromScan(false),
89  _depthFromScanFillHoles(1),
90  _depthFromScanFillHolesFromBorder(false),
91  _filenamesAreTimestamps(false),
92  _hasConfigForEachFrame(false),
93  _syncImageRateWithStamps(true),
94  _odometryFormat(0),
95  _groundTruthFormat(0),
96  _maxPoseTimeDiff(0.02),
97  _captureDelay(0.0)
98 {
99 
100 }
101 
102 CameraImages::~CameraImages()
103 {
104  UDEBUG("");
105  delete _dir;
106  delete _scanDir;
107 }
108 
109 bool CameraImages::init(const std::string & calibrationFolder, const std::string & cameraName)
110 {
111  _lastFileName.clear();
112  _lastScanFileName.clear();
113  _count = 0;
114  _countScan = 0;
115  _captureDelay = 0.0;
116  _framesPublished=0;
117  _model = cameraModel();
118  _models.clear();
119  covariances_.clear();
120 
121  UDEBUG("");
122  if(_dir)
123  {
124  delete _dir;
125  _dir = 0;
126  }
127  if(!_path.empty())
128  {
129  _dir = new UDirectory(_path, "jpg ppm png bmp pnm tiff pgm");
130  if(_path[_path.size()-1] != '\\' && _path[_path.size()-1] != '/')
131  {
132  _path.append("/");
133  }
134  if(!_dir->isValid())
135  {
136  ULOGGER_ERROR("Directory path is not valid \"%s\"", _path.c_str());
137  delete _dir;
138  _dir = 0;
139  }
140  else if(_dir->getFileNames().size() == 0)
141  {
142  UWARN("Directory is empty \"%s\"", _path.c_str());
143  delete _dir;
144  _dir = 0;
145  }
146  else
147  {
148  UINFO("path=%s images=%d", _path.c_str(), (int)this->imagesCount());
149  }
150  }
151 
152  // check for scan directory
153  if(_scanDir)
154  {
155  delete _scanDir;
156  _scanDir = 0;
157  }
158  if(!_scanPath.empty())
159  {
160  UINFO("scan path=%s", _scanPath.c_str());
161  _scanDir = new UDirectory(_scanPath, "pcd bin ply"); // "bin" is for KITTI format
162  if(_scanPath[_scanPath.size()-1] != '\\' && _scanPath[_scanPath.size()-1] != '/')
163  {
164  _scanPath.append("/");
165  }
166  if(!_scanDir->isValid())
167  {
168  UERROR("Scan directory path is not valid \"%s\"", _scanPath.c_str());
169  delete _scanDir;
170  _scanDir = 0;
171  }
172  else if(_scanDir->getFileNames().size() == 0)
173  {
174  UWARN("Scan directory is empty \"%s\"", _scanPath.c_str());
175  delete _scanDir;
176  _scanDir = 0;
177  }
178  else if(_dir && _scanDir->getFileNames().size() != _dir->getFileNames().size())
179  {
180  UERROR("Scan and image directories should be the same size \"%s\"(%d) vs \"%s\"(%d)",
181  _scanPath.c_str(),
182  (int)_scanDir->getFileNames().size(),
183  _path.c_str(),
184  (int)_dir->getFileNames().size());
185  delete _scanDir;
186  _scanDir = 0;
187  }
188  else
189  {
190  UINFO("path=%s scans=%d", _scanPath.c_str(), (int)this->imagesCount());
191  }
192  }
193 
194  if(_dir==0 && _scanDir == 0)
195  {
196  ULOGGER_ERROR("Images path or scans path should be set!");
197  return false;
198  }
199 
200  if(_dir)
201  {
202  // look for calibration files
203  UINFO("calibration folder=%s name=%s", calibrationFolder.c_str(), cameraName.c_str());
204  if(!calibrationFolder.empty() && !cameraName.empty())
205  {
206  if(!_model.load(calibrationFolder, cameraName))
207  {
208  UWARN("Missing calibration files for camera \"%s\" in \"%s\" folder, you should calibrate the camera!",
209  cameraName.c_str(), calibrationFolder.c_str());
210  }
211  else
212  {
213  UINFO("Camera parameters: fx=%f fy=%f cx=%f cy=%f",
214  _model.fx(),
215  _model.fy(),
216  _model.cx(),
217  _model.cy());
218 
219  cv::FileStorage fs(calibrationFolder+"/"+cameraName+".yaml", 0);
220  cv::FileNode poseNode = fs["local_transform"];
221  if(!poseNode.isNone())
222  {
223  UWARN("Using local transform from calibration file (%s) instead of the parameter one (%s).",
224  _model.localTransform().prettyPrint().c_str(),
225  this->getLocalTransform().prettyPrint().c_str());
226  this->setLocalTransform(_model.localTransform());
227  }
228  }
229  }
230  _model.setName(cameraName);
231 
232  _model.setLocalTransform(this->getLocalTransform());
233  if(_rectifyImages && !_model.isValidForRectification())
234  {
235  UERROR("Parameter \"rectifyImages\" is set, but no camera model is loaded or valid.");
236  return false;
237  }
238  }
239 
240  bool success = _dir|| _scanDir;
241  _stamps.clear();
242  odometry_.clear();
243  groundTruth_.clear();
244  if(success)
245  {
246  if(_dir && _hasConfigForEachFrame)
247  {
248 #if CV_MAJOR_VERSION < 3 || (CV_MAJOR_VERSION == 3 && CV_MAJOR_VERSION < 2)
249  UDirectory dirJson(_path, "yaml xml");
250 #else
251  UDirectory dirJson(_path, "yaml xml json");
252 #endif
253  if(dirJson.getFileNames().size() == _dir->getFileNames().size())
254  {
255  bool modelsWarned = false;
256  bool localTWarned = false;
257  for(std::list<std::string>::const_iterator iter=dirJson.getFileNames().begin(); iter!=dirJson.getFileNames().end() && success; ++iter)
258  {
259  std::string filePath = _path+"/"+*iter;
260  cv::FileStorage fs(filePath, 0);
261  cv::FileNode poseNode = fs["cameraPoseARFrame"]; // Check if it is 3DScannerApp(iOS) format
262  if(poseNode.isNone())
263  {
264  cv::FileNode n = fs["local_transform"];
265  bool hasLocalTransform = !n.isNone();
266 
267  fs.release();
268  if(_model.isValidForProjection() && !modelsWarned)
269  {
270  UWARN("Camera model loaded for each frame is overridden by "
271  "general calibration file provided. Remove general calibration "
272  "file to use camera model of each frame. This warning will "
273  "be shown only one time.");
274  modelsWarned = true;
275  }
276  else
277  {
278  CameraModel model;
279  model.load(filePath);
280 
281  if(!hasLocalTransform)
282  {
283  if(!localTWarned)
284  {
285  UWARN("Loaded calibration file doesn't have local_transform field, "
286  "the global local_transform parameter is used by default (%s).",
287  this->getLocalTransform().prettyPrint().c_str());
288  localTWarned = true;
289  }
290  model.setLocalTransform(this->getLocalTransform());
291  }
292 
293  _models.push_back(model);
294  }
295  }
296  else
297  {
298  cv::FileNode timeNode = fs["time"];
299  cv::FileNode intrinsicsNode = fs["intrinsics"];
300  if(poseNode.isNone() || poseNode.size() != 16)
301  {
302  UERROR("Failed reading \"cameraPoseARFrame\" parameter, it should have 16 values (file=%s)", filePath.c_str());
303  success = false;
304  break;
305  }
306  else if(timeNode.isNone() || !timeNode.isReal())
307  {
308  UERROR("Failed reading \"time\" parameter (file=%s)", filePath.c_str());
309  success = false;
310  break;
311  }
312  else if(intrinsicsNode.isNone() || intrinsicsNode.size()!=9)
313  {
314  UERROR("Failed reading \"intrinsics\" parameter (file=%s)", filePath.c_str());
315  success = false;
316  break;
317  }
318  else
319  {
320  _stamps.push_back((double)timeNode);
321  if(_model.isValidForProjection() && !modelsWarned)
322  {
323  UWARN("Camera model loaded for each frame is overridden by "
324  "general calibration file provided. Remove general calibration "
325  "file to use camera model of each frame. This warning will "
326  "be shown only one time.");
327  modelsWarned = true;
328  }
329  else
330  {
331  _models.push_back(CameraModel(
332  (double)intrinsicsNode[0], //fx
333  (double)intrinsicsNode[4], //fy
334  (double)intrinsicsNode[2], //cx
335  (double)intrinsicsNode[5], //cy
336  CameraModel::opticalRotation()));
337  }
338  // we need to rotate from opengl world to rtabmap world
339  Transform pose(
340  (float)poseNode[0], (float)poseNode[1], (float)poseNode[2], (float)poseNode[3],
341  (float)poseNode[4], (float)poseNode[5], (float)poseNode[6], (float)poseNode[7],
342  (float)poseNode[8], (float)poseNode[9], (float)poseNode[10], (float)poseNode[11]);
343  pose = Transform::rtabmap_T_opengl() * pose * Transform::opengl_T_rtabmap();
344  odometry_.push_back(pose);
345  }
346  }
347  }
348  if(!success)
349  {
350  odometry_.clear();
351  _stamps.clear();
352  _models.clear();
353  }
354  }
355  else
356  {
357  std::string opencv32warn;
358 #if CV_MAJOR_VERSION < 3 || (CV_MAJOR_VERSION == 3 && CV_MAJOR_VERSION < 2)
359  opencv32warn = " RTAB-Map is currently built with OpenCV < 3.2, only xml and yaml files are supported (not json).";
360 #endif
361  UERROR("Parameter \"Config for each frame\" is true, but the "
362  "number of config files (%d) is not equal to number "
363  "of images (%d) in this directory \"%s\".%s",
364  (int)dirJson.getFileNames().size(),
365  (int)_dir->getFileNames().size(),
366  _path.c_str(),
367  opencv32warn.c_str());
368  success = false;
369  }
370  }
371 
372  if(_stamps.empty())
373  {
374  if(_filenamesAreTimestamps)
375  {
376  std::list<std::string> filenames = _dir?_dir->getFileNames():_scanDir->getFileNames();
377  for(std::list<std::string>::const_iterator iter=filenames.begin(); iter!=filenames.end(); ++iter)
378  {
379  // format is text_1223445645.12334_text.png or text_122344564512334_text.png
380  // If no decimals, 10 first number are the seconds
381  std::list<std::string> list = uSplit(*iter, '.');
382  if(list.size() == 3 || list.size() == 2)
383  {
384  list.pop_back(); // remove extension
385  double stamp = 0.0;
386  if(list.size() == 1)
387  {
388  std::list<std::string> numberList = uSplitNumChar(list.front());
389  for(std::list<std::string>::iterator iter=numberList.begin(); iter!=numberList.end(); ++iter)
390  {
391  if(uIsNumber(*iter))
392  {
393  std::string decimals;
394  std::string sec;
395  if(iter->length()>10)
396  {
397  decimals = iter->substr(10, iter->size()-10);
398  sec = iter->substr(0, 10);
399  }
400  else
401  {
402  sec = *iter;
403  }
404  stamp = uStr2Double(sec + "." + decimals);
405  break;
406  }
407  }
408  }
409  else
410  {
411  std::string decimals = uSplitNumChar(list.back()).front();
412  list.pop_back();
413  std::string sec = uSplitNumChar(list.back()).back();
414  stamp = uStr2Double(sec + "." + decimals);
415  }
416  if(stamp > 0.0)
417  {
418  _stamps.push_back(stamp);
419  }
420  else
421  {
422  UERROR("Conversion filename to timestamp failed! (filename=%s)", iter->c_str());
423  }
424  }
425  }
426  if(_stamps.size() != this->imagesCount())
427  {
428  UERROR("The stamps count is not the same as the images (%d vs %d)! "
429  "Converting filenames to timestamps is activated.",
430  (int)_stamps.size(), this->imagesCount());
431  _stamps.clear();
432  success = false;
433  }
434  }
435  else if(_timestampsPath.size())
436  {
437  std::ifstream file;
438  file.open(_timestampsPath.c_str(), std::ifstream::in);
439  while(file.good())
440  {
441  std::string str;
442  std::getline(file, str);
443 
444  if(str.empty() || str.at(0) == '#' || str.at(0) == '%')
445  {
446  continue;
447  }
448 
449  std::list<std::string> strList = uSplit(str, ' ');
450  std::string stampStr = strList.front();
451  if(strList.size() == 2)
452  {
453  // format "seconds millisec"
454  // the millisec str needs 0-padding if size < 6
455  std::string millisecStr = strList.back();
456  while(millisecStr.size() < 6)
457  {
458  millisecStr = "0" + millisecStr;
459  }
460  stampStr = stampStr+'.'+millisecStr;
461  }
462  _stamps.push_back(uStr2Double(stampStr));
463  }
464 
465  file.close();
466 
467  if(_stamps.size() != this->imagesCount())
468  {
469  UERROR("The stamps count (%d) is not the same as the images (%d)! Please remove "
470  "the timestamps file path if you don't want to use them (current file path=%s).",
471  (int)_stamps.size(), this->imagesCount(), _timestampsPath.c_str());
472  _stamps.clear();
473  success = false;
474  }
475  }
476  }
477 
478  if(success && _odometryPath.size() && odometry_.empty())
479  {
480  success = readPoses(odometry_, _stamps, _odometryPath, _odometryFormat, _maxPoseTimeDiff);
481  }
482 
483  if(success && _groundTruthPath.size())
484  {
485  success = readPoses(groundTruth_, _stamps, _groundTruthPath, _groundTruthFormat, _maxPoseTimeDiff);
486  }
487 
488  if(!odometry_.empty())
489  {
490  for(size_t i=0; i<odometry_.size(); ++i)
491  {
492  // linear cov = 0.0001
493  cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1) * (i==0?9999.0:0.0001);
494  if(i!=0)
495  {
496  // angular cov = 0.000001
497  covariance.at<double>(3,3) *= 0.01;
498  covariance.at<double>(4,4) *= 0.01;
499  covariance.at<double>(5,5) *= 0.01;
500  }
501  covariances_.push_back(covariance);
502  }
503  }
504  }
505 
506  _captureTimer.restart();
507 
508  return success;
509 }
510 
511 bool CameraImages::readPoses(
512  std::list<Transform> & outputPoses,
513  std::list<double> & inOutStamps,
514  const std::string & filePath,
515  int format,
516  double maxTimeDiff) const
517 {
518  outputPoses.clear();
519  std::map<int, Transform> poses;
520  std::map<int, double> stamps;
521  if(!graph::importPoses(filePath, format, poses, 0, &stamps))
522  {
523  UERROR("Cannot read pose file \"%s\".", filePath.c_str());
524  return false;
525  }
526  else if((format != 1 && format != 10 && format != 5 && format != 6 && format != 7 && format != 9) && poses.size() != this->imagesCount())
527  {
528  UERROR("The pose count is not the same as the images (%d vs %d)! Please remove "
529  "the pose file path if you don't want to use it (current file path=%s).",
530  (int)poses.size(), this->imagesCount(), filePath.c_str());
531  return false;
532  }
533  else if((format == 1 || format == 10 || format == 5 || format == 6 || format == 7 || format == 9) && (inOutStamps.empty() && stamps.size()!=poses.size()))
534  {
535  UERROR("When using RGBD-SLAM, GPS, MALAGA, ST LUCIA and EuRoC MAV formats, images must have timestamps!");
536  return false;
537  }
538  else if(format == 1 || format == 10 || format == 5 || format == 6 || format == 7 || format == 9)
539  {
540  UDEBUG("");
541  //Match ground truth values with images
542  outputPoses.clear();
543  std::map<double, int> stampsToIds;
544  for(std::map<int, double>::iterator iter=stamps.begin(); iter!=stamps.end(); ++iter)
545  {
546  stampsToIds.insert(std::make_pair(iter->second, iter->first));
547  }
548  std::vector<double> values = uValues(stamps);
549 
550  if(inOutStamps.empty())
551  {
552  inOutStamps = uValuesList(stamps);
553  }
554 
555  int validPoses = 0;
556  for(std::list<double>::iterator ster=inOutStamps.begin(); ster!=inOutStamps.end(); ++ster)
557  {
558  Transform pose; // null transform
559  std::map<double, int>::iterator endIter = stampsToIds.lower_bound(*ster);
560  bool warned = false;
561  if(endIter != stampsToIds.end())
562  {
563  if(endIter->first == *ster)
564  {
565  pose = poses.at(endIter->second);
566  ++validPoses;
567  }
568  else if(endIter != stampsToIds.begin())
569  {
570  //interpolate
571  std::map<double, int>::iterator beginIter = endIter;
572  --beginIter;
573  double stampBeg = beginIter->first;
574  double stampEnd = endIter->first;
575  UASSERT(stampEnd > stampBeg && *ster>stampBeg && *ster < stampEnd);
576  if(fabs(*ster-stampEnd) > maxTimeDiff || fabs(*ster-stampBeg) > maxTimeDiff)
577  {
578  if(!warned)
579  {
580  UWARN("Cannot interpolate pose for stamp %f between %f and %f (> maximum time diff of %f sec)",
581  *ster,
582  stampBeg,
583  stampEnd,
584  maxTimeDiff);
585  }
586  warned=true;
587  }
588  else
589  {
590  warned=false;
591  float t = (*ster - stampBeg) / (stampEnd-stampBeg);
592  Transform & ta = poses.at(beginIter->second);
593  Transform & tb = poses.at(endIter->second);
594  if(!ta.isNull() && !tb.isNull())
595  {
596  ++validPoses;
597  pose = ta.interpolate(t, tb);
598  }
599  }
600  }
601  }
602  if(pose.isNull() && !warned)
603  {
604  UDEBUG("Pose not found for stamp %f", *ster);
605  }
606  outputPoses.push_back(pose);
607  }
608  if(validPoses != (int)inOutStamps.size())
609  {
610  UWARN("%d valid poses of %d stamps", validPoses, (int)inOutStamps.size());
611  }
612  }
613  else
614  {
615  UDEBUG("");
616  outputPoses = uValuesList(poses);
617  if(inOutStamps.size() == 0 && stamps.size() == poses.size())
618  {
619  inOutStamps = uValuesList(stamps);
620  }
621  else if(format==8 && inOutStamps.size() == 0 && stamps.size()>0 && stamps.size() != poses.size())
622  {
623  UERROR("With Karlsruhe format, timestamps (%d) and poses (%d) should match!", (int)stamps.size(), (int)poses.size());
624  return false;
625  }
626  else if(!outputPoses.empty() && inOutStamps.empty() && stamps.empty())
627  {
628  UERROR("Timestamps are empty (poses=%d)! Forgot the set a timestamp file?", (int)outputPoses.size());
629  return false;
630  }
631  }
632  UASSERT_MSG(outputPoses.size() == inOutStamps.size(), uFormat("%d vs %d", (int)outputPoses.size(), (int)inOutStamps.size()).c_str());
633  return true;
634 }
635 
636 bool CameraImages::isCalibrated() const
637 {
638  return (_dir && (_model.isValidForProjection() || (_models.size() && _models.front().isValidForProjection()))) ||
639  _scanDir;
640 }
641 
642 std::string CameraImages::getSerial() const
643 {
644  return _model.name();
645 }
646 
647 unsigned int CameraImages::imagesCount() const
648 {
649  if(_dir)
650  {
651  return (unsigned int)_dir->getFileNames().size();
652  }
653  else if(_scanDir)
654  {
655  return (unsigned int)_scanDir->getFileNames().size();
656  }
657  return 0;
658 }
659 
660 std::vector<std::string> CameraImages::filenames() const
661 {
662  if(_dir)
663  {
664  return uListToVector(_dir->getFileNames());
665  }
666  else if(_scanDir)
667  {
668  return uListToVector(_scanDir->getFileNames());
669  }
670  return std::vector<std::string>();
671 }
672 
673 SensorData CameraImages::captureImage(SensorCaptureInfo * info)
674 {
675  if(_syncImageRateWithStamps && _captureDelay>0.0)
676  {
677  int sleepTime = (1000*_captureDelay - 1000.0f*_captureTimer.getElapsedTime());
678  if(sleepTime > 2)
679  {
680  uSleep(sleepTime-2);
681  }
682  else if(sleepTime < 0)
683  {
684  if(this->getImageRate() > 0.0f)
685  {
686  UWARN("CameraImages: Cannot read images as fast as their timestamps (target delay=%fs, capture time=%fs). Disable "
687  "source image rate or disable synchronization of capture time with timestamps.",
688  _captureDelay, _captureTimer.getElapsedTime());
689  }
690  else
691  {
692  UWARN("CameraImages: Cannot read images as fast as their timestamps (target delay=%fs, capture time=%fs).",
693  _captureDelay, _captureTimer.getElapsedTime());
694  }
695  }
696 
697  // Add precision at the cost of a small overhead
698  while(_captureTimer.getElapsedTime() < _captureDelay-0.000001)
699  {
700  //
701  }
702  _captureTimer.start();
703  }
704  _captureDelay = 0.0;
705 
706  cv::Mat img;
707  LaserScan scan(cv::Mat(), _scanMaxPts, 0, LaserScan::kUnknown, _scanLocalTransform);
708  double stamp = UTimer::now();
709  Transform odometryPose;
710  cv::Mat covariance;
711  Transform groundTruthPose;
712  cv::Mat depthFromScan;
713  CameraModel model = _model;
714  UDEBUG("");
715  if(_dir || _scanDir)
716  {
717  if(_refreshDir)
718  {
719  if(_dir)
720  {
721  _dir->update();
722  }
723  if(_scanDir)
724  {
725  _scanDir->update();
726  }
727  }
728  std::string imageFilePath;
729  std::string scanFilePath;
730  if(_startAt < 0)
731  {
732  if(_dir)
733  {
734  const std::list<std::string> & fileNames = _dir->getFileNames();
735  if(fileNames.size())
736  {
737  if(_lastFileName.empty() || uStrNumCmp(_lastFileName,*fileNames.rbegin()) < 0)
738  {
739  _lastFileName = *fileNames.rbegin();
740  imageFilePath = _path + _lastFileName;
741  }
742  }
743  }
744  if(_scanDir)
745  {
746  const std::list<std::string> & scanFileNames = _scanDir->getFileNames();
747  if(scanFileNames.size())
748  {
749  if(_lastScanFileName.empty() || uStrNumCmp(_lastScanFileName,*scanFileNames.rbegin()) < 0)
750  {
751  _lastScanFileName = *scanFileNames.rbegin();
752  scanFilePath = _scanPath + _lastScanFileName;
753  }
754  }
755  }
756 
757  if(_stamps.size())
758  {
759  stamp = _stamps.front();
760  _stamps.pop_front();
761  if(_stamps.size())
762  {
763  _captureDelay = _stamps.front() - stamp;
764  }
765  }
766  if(odometry_.size())
767  {
768  odometryPose = odometry_.front();
769  odometry_.pop_front();
770  if(covariances_.size())
771  {
772  covariance = covariances_.front();
773  covariances_.pop_front();
774  }
775  }
776  if(groundTruth_.size())
777  {
778  groundTruthPose = groundTruth_.front();
779  groundTruth_.pop_front();
780  }
781  if(_models.size() && !model.isValidForProjection())
782  {
783  model = _models.front();
784  _models.pop_front();
785  }
786  }
787  else
788  {
789  std::string imageFileName = _dir?_dir->getNextFileName():"";
790  std::string scanFileName = _scanDir?_scanDir->getNextFileName():"";
791  if((_dir && !imageFileName.empty()) || (!_dir && !scanFileName.empty()))
792  {
793  imageFilePath = _path + imageFileName;
794  scanFilePath = _scanPath + scanFileName;
795  if(_stamps.size())
796  {
797  stamp = _stamps.front();
798  _stamps.pop_front();
799  if(_stamps.size())
800  {
801  _captureDelay = _stamps.front() - stamp;
802  }
803  }
804  if(odometry_.size())
805  {
806  odometryPose = odometry_.front();
807  odometry_.pop_front();
808  if(covariances_.size())
809  {
810  covariance = covariances_.front();
811  covariances_.pop_front();
812  }
813  }
814  if(groundTruth_.size())
815  {
816  groundTruthPose = groundTruth_.front();
817  groundTruth_.pop_front();
818  }
819  if(_models.size() && !model.isValidForProjection())
820  {
821  model = _models.front();
822  _models.pop_front();
823  }
824 
825  while(_count++ < _startAt)
826  {
827  imageFileName = _dir?_dir->getNextFileName():"";
828  scanFileName = _scanDir?_scanDir->getNextFileName():"";
829 
830  if((_dir && imageFileName.empty()) || (!_dir && scanFileName.empty()))
831  {
832  break;
833  }
834 
835  imageFilePath = _path + imageFileName;
836  scanFilePath = _scanPath + scanFileName;
837  if(_stamps.size())
838  {
839  stamp = _stamps.front();
840  _stamps.pop_front();
841  if(_stamps.size())
842  {
843  _captureDelay = _stamps.front() - stamp;
844  }
845  }
846  if(odometry_.size())
847  {
848  odometryPose = odometry_.front();
849  odometry_.pop_front();
850  if(covariances_.size())
851  {
852  covariance = covariances_.front();
853  covariances_.pop_front();
854  }
855  }
856  if(groundTruth_.size())
857  {
858  groundTruthPose = groundTruth_.front();
859  groundTruth_.pop_front();
860  }
861  if(_models.size() && !model.isValidForProjection())
862  {
863  model = _models.front();
864  _models.pop_front();
865  }
866  }
867  }
868  }
869 
870  if(_maxFrames <=0 || ++_framesPublished <= _maxFrames)
871  {
872 
873  if(!imageFilePath.empty())
874  {
875  ULOGGER_DEBUG("Loading image : %s", imageFilePath.c_str());
876 
877 #if CV_MAJOR_VERSION >2 || (CV_MAJOR_VERSION >=2 && CV_MINOR_VERSION >=4)
878  img = cv::imread(imageFilePath.c_str(), cv::IMREAD_UNCHANGED);
879 #else
880  img = cv::imread(imageFilePath.c_str(), -1);
881 #endif
882  UDEBUG("width=%d, height=%d, channels=%d, elementSize=%d, total=%d",
883  img.cols, img.rows, img.channels(), img.elemSize(), img.total());
884 
885  if(_isDepth)
886  {
887  if(img.type() != CV_16UC1 && img.type() != CV_32FC1)
888  {
889  UERROR("Depth is on and the loaded image has not a format supported (file = \"%s\", type=%d). "
890  "Formats supported are 16 bits 1 channel (mm) and 32 bits 1 channel (m).",
891  imageFilePath.c_str(), img.type());
892  img = cv::Mat();
893  }
894 
895  if(_depthScaleFactor > 1.0f)
896  {
897  img /= _depthScaleFactor;
898  }
899  }
900  else
901  {
902 #if CV_MAJOR_VERSION < 3
903  // FIXME : it seems that some png are incorrectly loaded with opencv c++ interface, where c interface works...
904  if(img.depth() != CV_8U)
905  {
906  // The depth should be 8U
907  UWARN("Cannot read the image correctly, falling back to old OpenCV C interface...");
908  IplImage * i = cvLoadImage(imageFilePath.c_str());
909  img = cv::Mat(i, true);
910  cvReleaseImage(&i);
911  }
912 #endif
913  if(img.channels()>3)
914  {
915  UWARN("Conversion from 4 channels to 3 channels (file=%s)", imageFilePath.c_str());
916  cv::Mat out;
917  cv::cvtColor(img, out, CV_BGRA2BGR);
918  img = out;
919  }
920  else if(!img.empty() && _bayerMode >= 0 && _bayerMode <=3)
921  {
922  cv::Mat debayeredImg;
923  try
924  {
925  cv::cvtColor(img, debayeredImg, CV_BayerBG2BGR + _bayerMode);
926  img = debayeredImg;
927  }
928  catch(const cv::Exception & e)
929  {
930  UWARN("Error debayering images: \"%s\". Please set bayer mode to -1 if images are not bayered!", e.what());
931  }
932  }
933  }
934 
935  if(!img.empty() && model.isValidForRectification() && _rectifyImages)
936  {
937  img = model.rectifyImage(img);
938  }
939  }
940 
941  if(!scanFilePath.empty())
942  {
943  // load without filtering
944  scan = util3d::loadScan(scanFilePath);
945  scan = LaserScan(scan.data(), _scanMaxPts, 0.0f, scan.format(), _scanLocalTransform);
946  UDEBUG("Loaded scan=%d points", (int)scan.size());
947  if(_depthFromScan && !img.empty())
948  {
949  UDEBUG("Computing depth from scan...");
950  if(!model.isValidForProjection())
951  {
952  UWARN("Depth from laser scan: Camera model should be valid.");
953  }
954  else if(_isDepth)
955  {
956  UWARN("Depth from laser scan: Loading already a depth image.");
957  }
958  else
959  {
960  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::laserScanToPointCloud(scan, scan.localTransform());
961  depthFromScan = util3d::projectCloudToCamera(img.size(), model.K(), cloud, model.localTransform());
962  if(_depthFromScanFillHoles!=0)
963  {
964  util3d::fillProjectedCloudHoles(depthFromScan, _depthFromScanFillHoles>0, _depthFromScanFillHolesFromBorder);
965  }
966  }
967  }
968  }
969  }
970  }
971  else
972  {
973  UWARN("Directory is not set, camera must be initialized.");
974  }
975 
976  if(model.imageHeight() == 0 || model.imageWidth() == 0)
977  {
978  model.setImageSize(img.size());
979  }
980 
981  SensorData data;
982  if(!img.empty() || !scan.empty())
983  {
984  data = SensorData(scan, _isDepth?cv::Mat():img, _isDepth?img:depthFromScan, model, this->getNextSeqID(), stamp);
985  data.setGroundTruth(groundTruthPose);
986  }
987 
988  if(info && !odometryPose.isNull())
989  {
990  info->odomPose = odometryPose;
991  info->odomCovariance = covariance.empty()?cv::Mat::eye(6,6,CV_64FC1):covariance; // Note that with TORO and g2o file formats, we could get the covariance
992  }
993 
994  return data;
995 }
996 
997 } // namespace rtabmap
rtabmap::SensorData
Definition: SensorData.h:51
rtabmap::LaserScan::localTransform
Transform localTransform() const
Definition: LaserScan.h:127
rtabmap::CameraModel::fx
double fx() const
Definition: CameraModel.h:102
rtabmap::CameraModel::cx
double cx() const
Definition: CameraModel.h:104
rtabmap::CameraImages::CameraImages
CameraImages()
Definition: CameraImages.cpp:43
uIsNumber
bool uIsNumber(const std::string &str)
Definition: UStl.h:645
rtabmap::CameraModel::load
bool load(const std::string &filePath)
Definition: CameraModel.cpp:215
UINFO
#define UINFO(...)
format
std::string format(const std::string &str, const std::vector< std::string > &find, const std::vector< std::string > &replace)
rtabmap::CameraImages::_hasConfigForEachFrame
bool _hasConfigForEachFrame
Definition: CameraImages.h:159
rtabmap::CameraModel::setName
void setName(const std::string &name)
Definition: CameraModel.h:99
UDirectory::getFileNames
const std::list< std::string > & getFileNames() const
Definition: UDirectory.h:129
UTimer::now
static double now()
Definition: UTimer.cpp:80
rtabmap::CameraImages::_scanLocalTransform
Transform _scanLocalTransform
Definition: CameraImages.h:151
rtabmap::CameraImages::_models
std::list< CameraModel > _models
Definition: CameraImages.h:174
rtabmap::CameraImages::_groundTruthPath
std::string _groundTruthPath
Definition: CameraImages.h:165
rtabmap::CameraImages::_lastFileName
std::string _lastFileName
Definition: CameraImages.h:145
rtabmap::CameraImages::_model
CameraModel _model
Definition: CameraImages.h:173
list
rtabmap::CameraImages::_framesPublished
int _framesPublished
Definition: CameraImages.h:143
rtabmap::CameraModel::cy
double cy() const
Definition: CameraModel.h:105
UDirectory.h
this
this
rtabmap::CameraModel
Definition: CameraModel.h:38
rtabmap::Transform::opengl_T_rtabmap
static Transform opengl_T_rtabmap()
Definition: Transform.h:149
rtabmap::util3d::fillProjectedCloudHoles
void RTABMAP_CORE_EXPORT fillProjectedCloudHoles(cv::Mat &depthRegistered, bool verticalDirection, bool fillToBorder)
Definition: util3d.cpp:2927
rtabmap::LaserScan::data
const cv::Mat & data() const
Definition: LaserScan.h:116
rtabmap::CameraImages::_startAt
int _startAt
Definition: CameraImages.h:133
rtabmap::CameraImages::cameraModel
const CameraModel & cameraModel() const
Definition: CameraImages.h:59
CameraImages.h
rtabmap_netvlad.img
img
Definition: rtabmap_netvlad.py:78
rtabmap::CameraImages::_maxFrames
int _maxFrames
Definition: CameraImages.h:134
glm::sec
GLM_FUNC_DECL genType sec(genType const &angle)
rtabmap::SensorCapture::getLocalTransform
const Transform & getLocalTransform() const
Definition: SensorCapture.h:62
true
#define true
Definition: ConvertUTF.c:57
UTimer::start
void start()
Definition: UTimer.cpp:87
uStr2Double
double UTILITE_EXPORT uStr2Double(const std::string &str)
Definition: UConversion.cpp:147
rtabmap::LaserScan::kUnknown
@ kUnknown
Definition: LaserScan.h:40
util3d.h
rtabmap::LaserScan
Definition: LaserScan.h:37
uListToVector
std::vector< V > uListToVector(const std::list< V > &list)
Definition: UStl.h:473
rtabmap::CameraImages::_odometryFormat
int _odometryFormat
Definition: CameraImages.h:164
ULOGGER_DEBUG
#define ULOGGER_DEBUG(...)
Definition: ULogger.h:53
rtabmap::LaserScan::size
int size() const
Definition: LaserScan.h:131
rtabmap::Transform::isNull
bool isNull() const
Definition: Transform.cpp:107
rtabmap::CameraImages::_scanDir
UDirectory * _scanDir
Definition: CameraImages.h:148
fabs
Real fabs(const Real &a)
rtabmap::util3d::loadScan
LaserScan RTABMAP_CORE_EXPORT loadScan(const std::string &path)
Definition: util3d.cpp:3454
uSplitNumChar
std::list< std::string > uSplitNumChar(const std::string &str)
Definition: UStl.h:670
rtabmap::SensorCaptureInfo
Definition: SensorCaptureInfo.h:36
rtabmap::CameraImages::getSerial
virtual std::string getSerial() const
Definition: CameraImages.cpp:642
n
int n
rtabmap::CameraImages::_maxPoseTimeDiff
double _maxPoseTimeDiff
Definition: CameraImages.h:167
rtabmap::LaserScan::format
Format format() const
Definition: LaserScan.h:117
rtabmap::CameraImages::_stamps
std::list< double > _stamps
Definition: CameraImages.h:169
rtabmap::CameraModel::setLocalTransform
void setLocalTransform(const Transform &transform)
Definition: CameraModel.h:115
list::size
size_t size() const
UDirectory::update
void update()
Definition: UDirectory.cpp:104
rtabmap::CameraImages::_bayerMode
int _bayerMode
Definition: CameraImages.h:139
rtabmap::Transform::prettyPrint
std::string prettyPrint() const
Definition: Transform.cpp:326
uStrNumCmp
int uStrNumCmp(const std::string &a, const std::string &b)
Definition: UStl.h:717
data
int data[]
rtabmap::CameraImages::_captureTimer
UTimer _captureTimer
Definition: CameraImages.h:176
rtabmap::util3d::projectCloudToCamera
cv::Mat RTABMAP_CORE_EXPORT projectCloudToCamera(const cv::Size &imageSize, const cv::Mat &cameraMatrixK, const cv::Mat &laserScan, const rtabmap::Transform &cameraTransform)
Definition: util3d.cpp:2693
rtabmap::CameraImages::_scanMaxPts
int _scanMaxPts
Definition: CameraImages.h:152
UConversion.h
Some conversion functions.
util3d_filtering.h
rtabmap::Transform::rtabmap_T_opengl
static Transform rtabmap_T_opengl()
Definition: Transform.h:153
rtabmap::Transform::interpolate
Transform interpolate(float t, const Transform &other) const
Definition: Transform.cpp:303
rtabmap::CameraModel::isValidForProjection
bool isValidForProjection() const
Definition: CameraModel.h:87
rtabmap::CameraImages::_odometryPath
std::string _odometryPath
Definition: CameraImages.h:163
rtabmap::CameraImages::_depthFromScanFillHoles
int _depthFromScanFillHoles
Definition: CameraImages.h:155
rtabmap::CameraImages::imagesCount
unsigned int imagesCount() const
Definition: CameraImages.cpp:647
info
else if n * info
rtabmap::CameraModel::fy
double fy() const
Definition: CameraModel.h:103
UASSERT
#define UASSERT(condition)
UTimer::restart
double restart()
Definition: UTimer.h:94
rtabmap::CameraImages::_dir
UDirectory * _dir
Definition: CameraImages.h:144
rtabmap::CameraModel::opticalRotation
static Transform opticalRotation()
Definition: CameraModel.h:45
rtabmap::Camera
Definition: Camera.h:43
rtabmap::Camera::getImageRate
float getImageRate() const
Definition: Camera.h:49
out
std::ofstream out("Result.txt")
rtabmap::CameraModel::name
const std::string & name() const
Definition: CameraModel.h:100
rtabmap::CameraModel::isValidForRectification
bool isValidForRectification() const
Definition: CameraModel.h:89
path
path
UASSERT_MSG
#define UASSERT_MSG(condition, msg_str)
Definition: ULogger.h:67
str
UWARN
#define UWARN(...)
rtabmap::CameraImages::init
virtual bool init(const std::string &calibrationFolder=".", const std::string &cameraName="")
Definition: CameraImages.cpp:109
f
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
uFormat
std::string UTILITE_EXPORT uFormat(const char *fmt,...)
Definition: UConversion.cpp:365
rtabmap::CameraImages::_lastScanFileName
std::string _lastScanFileName
Definition: CameraImages.h:149
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
rtabmap::Transform
Definition: Transform.h:41
rtabmap::CameraImages::_timestampsPath
std::string _timestampsPath
Definition: CameraImages.h:160
rtabmap::CameraImages::odometry_
std::list< Transform > odometry_
Definition: CameraImages.h:170
rtabmap::CameraImages::_countScan
int _countScan
Definition: CameraImages.h:147
rtabmap::CameraImages::_filenamesAreTimestamps
bool _filenamesAreTimestamps
Definition: CameraImages.h:158
uSplit
std::list< std::string > uSplit(const std::string &str, char separator=' ')
Definition: UStl.h:564
rtabmap::CameraImages::_isDepth
bool _isDepth
Definition: CameraImages.h:140
rtabmap::CameraImages::_depthFromScanFillHolesFromBorder
bool _depthFromScanFillHolesFromBorder
Definition: CameraImages.h:156
Graph.h
UDirectory::getNextFileName
std::string getNextFileName()
Definition: UDirectory.cpp:221
values
leaf::MyValues values
rtabmap::graph::importPoses
bool RTABMAP_CORE_EXPORT importPoses(const std::string &filePath, int format, std::map< int, Transform > &poses, std::multimap< int, Link > *constraints=0, std::map< int, double > *stamps=0)
Definition: Graph.cpp:184
iter
iterator iter(handle obj)
rtabmap::CameraImages::filenames
std::vector< std::string > filenames() const
Definition: CameraImages.cpp:660
ULOGGER_ERROR
#define ULOGGER_ERROR(...)
Definition: ULogger.h:56
rtabmap::CameraImages::_rectifyImages
bool _rectifyImages
Definition: CameraImages.h:138
c_str
const char * c_str(Args &&...args)
UStl.h
Wrappers of STL for convenient functions.
rtabmap::CameraImages::_count
int _count
Definition: CameraImages.h:142
rtabmap::LaserScan::empty
bool empty() const
Definition: LaserScan.h:129
rtabmap::CameraImages::_syncImageRateWithStamps
bool _syncImageRateWithStamps
Definition: CameraImages.h:161
UDEBUG
#define UDEBUG(...)
rtabmap::CameraImages::_depthFromScan
bool _depthFromScan
Definition: CameraImages.h:154
uSleep
void uSleep(unsigned int ms)
Definition: Posix/UThreadC.h:23
rtabmap::CameraImages::_refreshDir
bool _refreshDir
Definition: CameraImages.h:137
rtabmap::CameraImages::~CameraImages
virtual ~CameraImages()
Definition: CameraImages.cpp:102
rtabmap::CameraImages::_scanPath
std::string _scanPath
Definition: CameraImages.h:150
rtabmap::util3d::laserScanToPointCloud
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_CORE_EXPORT laserScanToPointCloud(const LaserScan &laserScan, const Transform &transform=Transform())
Definition: util3d.cpp:2269
false
#define false
Definition: ConvertUTF.c:56
rtabmap::CameraImages::isCalibrated
virtual bool isCalibrated() const
Definition: CameraImages.cpp:636
UThreadC.h
uValues
std::vector< V > uValues(const std::multimap< K, V > &mm)
Definition: UStl.h:100
UTimer::getElapsedTime
double getElapsedTime()
Definition: UTimer.cpp:97
t
Point2 t(10, 10)
rtabmap::CameraImages::_depthScaleFactor
float _depthScaleFactor
Definition: CameraImages.h:141
rtabmap::SensorCapture::setLocalTransform
void setLocalTransform(const Transform &localTransform)
Definition: SensorCapture.h:66
UFile.h
rtabmap::CameraImages::covariances_
std::list< cv::Mat > covariances_
Definition: CameraImages.h:171
rtabmap
Definition: CameraARCore.cpp:35
UDirectory
Definition: UDirectory.h:34
rtabmap::CameraModel::localTransform
const Transform & localTransform() const
Definition: CameraModel.h:116
rtabmap::CameraImages::readPoses
bool readPoses(std::list< Transform > &outputPoses, std::list< double > &stamps, const std::string &filePath, int format, double maxTimeDiff) const
Definition: CameraImages.cpp:511
UERROR
#define UERROR(...)
trace.model
model
Definition: trace.py:4
file
file
rtabmap::CameraImages::_path
std::string _path
Definition: CameraImages.h:132
i
int i
rtabmap::CameraImages::_groundTruthFormat
int _groundTruthFormat
Definition: CameraImages.h:166
rtabmap::SensorCapture::getNextSeqID
int getNextSeqID()
Definition: SensorCapture.h:83
rtabmap::CameraImages::groundTruth_
std::list< Transform > groundTruth_
Definition: CameraImages.h:172
uValuesList
std::list< V > uValuesList(const std::multimap< K, V > &mm)
Definition: UStl.h:117
rtabmap::CameraImages::_captureDelay
double _captureDelay
Definition: CameraImages.h:177
UDirectory::isValid
bool isValid()
Definition: UDirectory.cpp:216
rtabmap::CameraImages::captureImage
virtual SensorData captureImage(SensorCaptureInfo *info=0)
Definition: CameraImages.cpp:673

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