37 #include <opencv2/imgproc/types_c.h>
47 _rectifyImages(
false),
50 _depthScaleFactor(1.0
f),
56 _scanLocalTransform(
Transform::getIdentity()),
58 _depthFromScan(
false),
59 _depthFromScanFillHoles(1),
60 _depthFromScanFillHolesFromBorder(
false),
61 _filenamesAreTimestamps(
false),
62 _hasConfigForEachFrame(
false),
63 _syncImageRateWithStamps(
true),
65 _groundTruthFormat(0),
66 _maxPoseTimeDiff(0.02),
72 Camera(imageRate, localTransform),
77 _rectifyImages(
false),
80 _depthScaleFactor(1.0
f),
86 _scanLocalTransform(
Transform::getIdentity()),
88 _depthFromScan(
false),
89 _depthFromScanFillHoles(1),
90 _depthFromScanFillHolesFromBorder(
false),
91 _filenamesAreTimestamps(
false),
92 _hasConfigForEachFrame(
false),
93 _syncImageRateWithStamps(
true),
95 _groundTruthFormat(0),
96 _maxPoseTimeDiff(0.02),
142 UWARN(
"Directory is empty \"%s\"",
_path.c_str());
148 UINFO(
"path=%s images=%d",
_path.c_str(), (
int)
this->imagesCount());
180 UERROR(
"Scan and image directories should be the same size \"%s\"(%d) vs \"%s\"(%d)",
203 UINFO(
"calibration folder=%s name=%s", calibrationFolder.c_str(), cameraName.c_str());
204 if(!calibrationFolder.empty() && !cameraName.empty())
206 if(!
_model.
load(calibrationFolder, cameraName))
208 UWARN(
"Missing calibration files for camera \"%s\" in \"%s\" folder, you should calibrate the camera!",
209 cameraName.c_str(), calibrationFolder.c_str());
213 UINFO(
"Camera parameters: fx=%f fy=%f cx=%f cy=%f",
219 cv::FileStorage fs(calibrationFolder+
"/"+cameraName+
".yaml", 0);
220 cv::FileNode poseNode = fs[
"local_transform"];
221 if(!poseNode.isNone())
223 UWARN(
"Using local transform from calibration file (%s) instead of the parameter one (%s).",
225 this->getLocalTransform().prettyPrint().c_str());
235 UERROR(
"Parameter \"rectifyImages\" is set, but no camera model is loaded or valid.");
248 #if CV_MAJOR_VERSION < 3 || (CV_MAJOR_VERSION == 3 && CV_MAJOR_VERSION < 2)
255 bool modelsWarned =
false;
256 bool localTWarned =
false;
260 cv::FileStorage fs(filePath, 0);
261 cv::FileNode poseNode = fs[
"cameraPoseARFrame"];
262 if(poseNode.isNone())
264 cv::FileNode
n = fs[
"local_transform"];
265 bool hasLocalTransform = !
n.isNone();
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.");
279 model.load(filePath);
281 if(!hasLocalTransform)
285 UWARN(
"Loaded calibration file doesn't have local_transform field, "
286 "the global local_transform parameter is used by default (%s).",
298 cv::FileNode timeNode = fs[
"time"];
299 cv::FileNode intrinsicsNode = fs[
"intrinsics"];
300 if(poseNode.isNone() || poseNode.size() != 16)
302 UERROR(
"Failed reading \"cameraPoseARFrame\" parameter, it should have 16 values (file=%s)", filePath.c_str());
306 else if(timeNode.isNone() || !timeNode.isReal())
308 UERROR(
"Failed reading \"time\" parameter (file=%s)", filePath.c_str());
312 else if(intrinsicsNode.isNone() || intrinsicsNode.size()!=9)
314 UERROR(
"Failed reading \"intrinsics\" parameter (file=%s)", filePath.c_str());
320 _stamps.push_back((
double)timeNode);
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.");
332 (
double)intrinsicsNode[0],
333 (
double)intrinsicsNode[4],
334 (
double)intrinsicsNode[2],
335 (
double)intrinsicsNode[5],
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]);
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).";
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",
367 opencv32warn.c_str());
389 for(std::list<std::string>::iterator
iter=numberList.begin();
iter!=numberList.end(); ++
iter)
393 std::string decimals;
395 if(
iter->length()>10)
397 decimals =
iter->substr(10,
iter->size()-10);
422 UERROR(
"Conversion filename to timestamp failed! (filename=%s)",
iter->c_str());
428 UERROR(
"The stamps count is not the same as the images (%d vs %d)! "
429 "Converting filenames to timestamps is activated.",
444 if(
str.empty() ||
str.at(0) ==
'#' ||
str.at(0) ==
'%')
449 std::list<std::string> strList =
uSplit(
str,
' ');
450 std::string stampStr = strList.front();
451 if(strList.size() == 2)
455 std::string millisecStr = strList.back();
456 while(millisecStr.size() < 6)
458 millisecStr =
"0" + millisecStr;
460 stampStr = stampStr+
'.'+millisecStr;
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).",
493 cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1) * (
i==0?9999.0:0.0001);
497 covariance.at<
double>(3,3) *= 0.01;
498 covariance.at<
double>(4,4) *= 0.01;
499 covariance.at<
double>(5,5) *= 0.01;
512 std::list<Transform> & outputPoses,
513 std::list<double> & inOutStamps,
514 const std::string & filePath,
516 double maxTimeDiff)
const
519 std::map<int, Transform> poses;
520 std::map<int, double> stamps;
523 UERROR(
"Cannot read pose file \"%s\".", filePath.c_str());
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());
535 UERROR(
"When using RGBD-SLAM, GPS, MALAGA, ST LUCIA and EuRoC MAV formats, images must have timestamps!");
543 std::map<double, int> stampsToIds;
544 for(std::map<int, double>::iterator
iter=stamps.begin();
iter!=stamps.end(); ++
iter)
546 stampsToIds.insert(std::make_pair(
iter->second,
iter->first));
550 if(inOutStamps.empty())
556 for(std::list<double>::iterator ster=inOutStamps.begin(); ster!=inOutStamps.end(); ++ster)
559 std::map<double, int>::iterator endIter = stampsToIds.lower_bound(*ster);
561 if(endIter != stampsToIds.end())
563 if(endIter->first == *ster)
565 pose = poses.at(endIter->second);
568 else if(endIter != stampsToIds.begin())
571 std::map<double, int>::iterator beginIter = endIter;
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)
580 UWARN(
"Cannot interpolate pose for stamp %f between %f and %f (> maximum time diff of %f sec)",
591 float t = (*ster - stampBeg) / (stampEnd-stampBeg);
592 Transform & ta = poses.at(beginIter->second);
593 Transform & tb = poses.at(endIter->second);
602 if(pose.
isNull() && !warned)
604 UDEBUG(
"Pose not found for stamp %f", *ster);
606 outputPoses.push_back(pose);
608 if(validPoses != (
int)inOutStamps.size())
610 UWARN(
"%d valid poses of %d stamps", validPoses, (
int)inOutStamps.size());
617 if(inOutStamps.size() == 0 && stamps.size() == poses.size())
621 else if(
format==8 && inOutStamps.size() == 0 && stamps.size()>0 && stamps.size() != poses.size())
623 UERROR(
"With Karlsruhe format, timestamps (%d) and poses (%d) should match!", (
int)stamps.size(), (
int)poses.size());
626 else if(!outputPoses.empty() && inOutStamps.empty() && stamps.empty())
628 UERROR(
"Timestamps are empty (poses=%d)! Forgot the set a timestamp file?", (
int)outputPoses.size());
632 UASSERT_MSG(outputPoses.size() == inOutStamps.size(),
uFormat(
"%d vs %d", (
int)outputPoses.size(), (
int)inOutStamps.size()).c_str());
670 return std::vector<std::string>();
682 else if(sleepTime < 0)
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.",
692 UWARN(
"CameraImages: Cannot read images as fast as their timestamps (target delay=%fs, capture time=%fs).",
712 cv::Mat depthFromScan;
728 std::string imageFilePath;
729 std::string scanFilePath;
747 if(scanFileNames.size())
791 if((
_dir && !imageFileName.empty()) || (!
_dir && !scanFileName.empty()))
793 imageFilePath =
_path + imageFileName;
830 if((
_dir && imageFileName.empty()) || (!
_dir && scanFileName.empty()))
835 imageFilePath =
_path + imageFileName;
873 if(!imageFilePath.empty())
877 #if CV_MAJOR_VERSION >2 || (CV_MAJOR_VERSION >=2 && CV_MINOR_VERSION >=4)
878 img = cv::imread(imageFilePath.c_str(), cv::IMREAD_UNCHANGED);
880 img = cv::imread(imageFilePath.c_str(), -1);
882 UDEBUG(
"width=%d, height=%d, channels=%d, elementSize=%d, total=%d",
887 if(
img.type() != CV_16UC1 &&
img.type() != CV_32FC1)
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());
902 #if CV_MAJOR_VERSION < 3
904 if(
img.depth() != CV_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);
915 UWARN(
"Conversion from 4 channels to 3 channels (file=%s)", imageFilePath.c_str());
917 cv::cvtColor(
img,
out, CV_BGRA2BGR);
922 cv::Mat debayeredImg;
925 cv::cvtColor(
img, debayeredImg, CV_BayerBG2BGR +
_bayerMode);
928 catch(
const cv::Exception &
e)
930 UWARN(
"Error debayering images: \"%s\". Please set bayer mode to -1 if images are not bayered!",
e.what());
941 if(!scanFilePath.empty())
946 UDEBUG(
"Loaded scan=%d points", (
int)scan.
size());
949 UDEBUG(
"Computing depth from scan...");
950 if(!
model.isValidForProjection())
952 UWARN(
"Depth from laser scan: Camera model should be valid.");
956 UWARN(
"Depth from laser scan: Loading already a depth image.");
973 UWARN(
"Directory is not set, camera must be initialized.");
976 if(
model.imageHeight() == 0 ||
model.imageWidth() == 0)
985 data.setGroundTruth(groundTruthPose);
990 info->odomPose = odometryPose;
991 info->odomCovariance = covariance.empty()?cv::Mat::eye(6,6,CV_64FC1):covariance;