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

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