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

rtabmap
Author(s): Mathieu Labbe
autogenerated on Wed Jun 5 2019 22:41:30