00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028 #include "rtabmap/core/CameraRGB.h"
00029 #include "rtabmap/core/Graph.h"
00030
00031 #include <rtabmap/utilite/UEventsManager.h>
00032 #include <rtabmap/utilite/UConversion.h>
00033 #include <rtabmap/utilite/UStl.h>
00034 #include <rtabmap/utilite/UConversion.h>
00035 #include <rtabmap/utilite/UFile.h>
00036 #include <rtabmap/utilite/UDirectory.h>
00037 #include <rtabmap/utilite/UTimer.h>
00038
00039 #include <opencv2/imgproc/imgproc.hpp>
00040 #include <opencv2/imgproc/types_c.h>
00041 #if CV_MAJOR_VERSION >= 3
00042 #include <opencv2/videoio/videoio_c.h>
00043 #endif
00044
00045 #include <rtabmap/core/util3d.h>
00046 #include <rtabmap/core/util3d_filtering.h>
00047 #include <rtabmap/core/util3d_surface.h>
00048
00049 #include <pcl/common/io.h>
00050
00051 #include <iostream>
00052 #include <fstream>
00053 #include <cmath>
00054
00055 namespace rtabmap
00056 {
00057
00059
00061 CameraImages::CameraImages() :
00062 _startAt(0),
00063 _refreshDir(false),
00064 _rectifyImages(false),
00065 _bayerMode(-1),
00066 _isDepth(false),
00067 _depthScaleFactor(1.0f),
00068 _count(0),
00069 _dir(0),
00070 _countScan(0),
00071 _scanDir(0),
00072 _scanLocalTransform(Transform::getIdentity()),
00073 _scanMaxPts(0),
00074 _scanDownsampleStep(1),
00075 _scanVoxelSize(0.0f),
00076 _scanNormalsK(0),
00077 _scanNormalsRadius(0),
00078 _scanForceGroundNormalsUp(false),
00079 _depthFromScan(false),
00080 _depthFromScanFillHoles(1),
00081 _depthFromScanFillHolesFromBorder(false),
00082 _filenamesAreTimestamps(false),
00083 _syncImageRateWithStamps(true),
00084 _odometryFormat(0),
00085 _groundTruthFormat(0),
00086 _maxPoseTimeDiff(0.02),
00087 _captureDelay(0.0)
00088 {}
00089 CameraImages::CameraImages(const std::string & path,
00090 float imageRate,
00091 const Transform & localTransform) :
00092 Camera(imageRate, localTransform),
00093 _path(path),
00094 _startAt(0),
00095 _refreshDir(false),
00096 _rectifyImages(false),
00097 _bayerMode(-1),
00098 _isDepth(false),
00099 _depthScaleFactor(1.0f),
00100 _count(0),
00101 _dir(0),
00102 _countScan(0),
00103 _scanDir(0),
00104 _scanLocalTransform(Transform::getIdentity()),
00105 _scanMaxPts(0),
00106 _scanDownsampleStep(1),
00107 _scanVoxelSize(0.0f),
00108 _scanNormalsK(0),
00109 _scanNormalsRadius(0),
00110 _scanForceGroundNormalsUp(false),
00111 _depthFromScan(false),
00112 _depthFromScanFillHoles(1),
00113 _depthFromScanFillHolesFromBorder(false),
00114 _filenamesAreTimestamps(false),
00115 _syncImageRateWithStamps(true),
00116 _odometryFormat(0),
00117 _groundTruthFormat(0),
00118 _maxPoseTimeDiff(0.02),
00119 _captureDelay(0.0)
00120 {
00121
00122 }
00123
00124 CameraImages::~CameraImages()
00125 {
00126 UDEBUG("");
00127 delete _dir;
00128 delete _scanDir;
00129 }
00130
00131 bool CameraImages::init(const std::string & calibrationFolder, const std::string & cameraName)
00132 {
00133 _lastFileName.clear();
00134 _lastScanFileName.clear();
00135 _count = 0;
00136 _countScan = 0;
00137 _captureDelay = 0.0;
00138
00139 UDEBUG("");
00140 if(_dir)
00141 {
00142 _dir->setPath(_path, "jpg ppm png bmp pnm tiff pgm");
00143 }
00144 else
00145 {
00146 _dir = new UDirectory(_path, "jpg ppm png bmp pnm tiff pgm");
00147 }
00148 if(_path[_path.size()-1] != '\\' && _path[_path.size()-1] != '/')
00149 {
00150 _path.append("/");
00151 }
00152 if(!_dir->isValid())
00153 {
00154 ULOGGER_ERROR("Directory path is not valid \"%s\"", _path.c_str());
00155 }
00156 else if(_dir->getFileNames().size() == 0)
00157 {
00158 UWARN("Directory is empty \"%s\"", _path.c_str());
00159 }
00160 else
00161 {
00162 UINFO("path=%s images=%d", _path.c_str(), (int)this->imagesCount());
00163 }
00164
00165
00166 if(_scanDir)
00167 {
00168 delete _scanDir;
00169 _scanDir = 0;
00170 }
00171 if(!_scanPath.empty())
00172 {
00173 UINFO("scan path=%s", _scanPath.c_str());
00174 _scanDir = new UDirectory(_scanPath, "pcd bin ply");
00175 if(_scanPath[_scanPath.size()-1] != '\\' && _scanPath[_scanPath.size()-1] != '/')
00176 {
00177 _scanPath.append("/");
00178 }
00179 if(!_scanDir->isValid())
00180 {
00181 UERROR("Scan directory path is not valid \"%s\"", _scanPath.c_str());
00182 delete _scanDir;
00183 _scanDir = 0;
00184 }
00185 else if(_scanDir->getFileNames().size() == 0)
00186 {
00187 UWARN("Scan directory is empty \"%s\"", _scanPath.c_str());
00188 delete _scanDir;
00189 _scanDir = 0;
00190 }
00191 else if(_scanDir->getFileNames().size() != _dir->getFileNames().size())
00192 {
00193 UERROR("Scan and image directories should be the same size \"%s\"(%d) vs \"%s\"(%d)",
00194 _scanPath.c_str(),
00195 (int)_scanDir->getFileNames().size(),
00196 _path.c_str(),
00197 (int)_dir->getFileNames().size());
00198 delete _scanDir;
00199 _scanDir = 0;
00200 }
00201 else
00202 {
00203 UINFO("path=%s scans=%d", _scanPath.c_str(), (int)this->imagesCount());
00204 }
00205 }
00206
00207
00208 UINFO("calibration folder=%s name=%s", calibrationFolder.c_str(), cameraName.c_str());
00209 if(!calibrationFolder.empty() && !cameraName.empty())
00210 {
00211 if(!_model.load(calibrationFolder, cameraName))
00212 {
00213 UWARN("Missing calibration files for camera \"%s\" in \"%s\" folder, you should calibrate the camera!",
00214 cameraName.c_str(), calibrationFolder.c_str());
00215 }
00216 else
00217 {
00218 UINFO("Camera parameters: fx=%f fy=%f cx=%f cy=%f",
00219 _model.fx(),
00220 _model.fy(),
00221 _model.cx(),
00222 _model.cy());
00223 }
00224 }
00225 _model.setName(cameraName);
00226
00227 _model.setLocalTransform(this->getLocalTransform());
00228 if(_rectifyImages && !_model.isValidForRectification())
00229 {
00230 UERROR("Parameter \"rectifyImages\" is set, but no camera model is loaded or valid.");
00231 return false;
00232 }
00233
00234 bool success = _dir->isValid();
00235 _stamps.clear();
00236 odometry_.clear();
00237 groundTruth_.clear();
00238 if(success)
00239 {
00240 if(_filenamesAreTimestamps)
00241 {
00242 const std::list<std::string> & filenames = _dir->getFileNames();
00243 for(std::list<std::string>::const_iterator iter=filenames.begin(); iter!=filenames.end(); ++iter)
00244 {
00245
00246
00247 std::list<std::string> list = uSplit(*iter, '.');
00248 if(list.size() == 3 || list.size() == 2)
00249 {
00250 list.pop_back();
00251 double stamp = 0.0;
00252 if(list.size() == 1)
00253 {
00254 std::list<std::string> numberList = uSplitNumChar(list.front());
00255 for(std::list<std::string>::iterator iter=numberList.begin(); iter!=numberList.end(); ++iter)
00256 {
00257 if(uIsNumber(*iter))
00258 {
00259 std::string decimals;
00260 std::string sec;
00261 if(iter->length()>10)
00262 {
00263 decimals = iter->substr(10, iter->size()-10);
00264 sec = iter->substr(0, 10);
00265 }
00266 else
00267 {
00268 sec = *iter;
00269 }
00270 stamp = uStr2Double(sec + "." + decimals);
00271 break;
00272 }
00273 }
00274 }
00275 else
00276 {
00277 std::string decimals = uSplitNumChar(list.back()).front();
00278 list.pop_back();
00279 std::string sec = uSplitNumChar(list.back()).back();
00280 stamp = uStr2Double(sec + "." + decimals);
00281 }
00282 if(stamp > 0.0)
00283 {
00284 _stamps.push_back(stamp);
00285 }
00286 else
00287 {
00288 UERROR("Conversion filename to timestamp failed! (filename=%s)", iter->c_str());
00289 }
00290 }
00291 }
00292 if(_stamps.size() != this->imagesCount())
00293 {
00294 UERROR("The stamps count is not the same as the images (%d vs %d)! "
00295 "Converting filenames to timestamps is activated.",
00296 (int)_stamps.size(), this->imagesCount());
00297 _stamps.clear();
00298 success = false;
00299 }
00300 }
00301 else if(_timestampsPath.size())
00302 {
00303 std::ifstream file;
00304 file.open(_timestampsPath.c_str(), std::ifstream::in);
00305 while(file.good())
00306 {
00307 std::string str;
00308 std::getline(file, str);
00309
00310 if(str.empty() || str.at(0) == '#' || str.at(0) == '%')
00311 {
00312 continue;
00313 }
00314
00315 std::list<std::string> strList = uSplit(str, ' ');
00316 std::string stampStr = strList.front();
00317 if(strList.size() == 2)
00318 {
00319
00320
00321 std::string millisecStr = strList.back();
00322 while(millisecStr.size() < 6)
00323 {
00324 millisecStr = "0" + millisecStr;
00325 }
00326 stampStr = stampStr+'.'+millisecStr;
00327 }
00328 _stamps.push_back(uStr2Double(stampStr));
00329 }
00330
00331 file.close();
00332
00333 if(_stamps.size() != this->imagesCount())
00334 {
00335 UERROR("The stamps count (%d) is not the same as the images (%d)! Please remove "
00336 "the timestamps file path if you don't want to use them (current file path=%s).",
00337 (int)_stamps.size(), this->imagesCount(), _timestampsPath.c_str());
00338 _stamps.clear();
00339 success = false;
00340 }
00341 }
00342
00343 if(success && _odometryPath.size())
00344 {
00345 success = readPoses(odometry_, _stamps, _odometryPath, _odometryFormat, _maxPoseTimeDiff);
00346 }
00347
00348 if(success && _groundTruthPath.size())
00349 {
00350 success = readPoses(groundTruth_, _stamps, _groundTruthPath, _groundTruthFormat, _maxPoseTimeDiff);
00351 }
00352 }
00353
00354 _captureTimer.restart();
00355
00356 return success;
00357 }
00358
00359 bool CameraImages::readPoses(std::list<Transform> & outputPoses, std::list<double> & inOutStamps, const std::string & filePath, int format, double maxTimeDiff) const
00360 {
00361 outputPoses.clear();
00362 std::map<int, Transform> poses;
00363 std::map<int, double> stamps;
00364 if(!graph::importPoses(filePath, format, poses, 0, &stamps))
00365 {
00366 UERROR("Cannot read pose file \"%s\".", filePath.c_str());
00367 return false;
00368 }
00369 else if((format != 1 && format != 5 && format != 6 && format != 7 && format != 9) && poses.size() != this->imagesCount())
00370 {
00371 UERROR("The pose count is not the same as the images (%d vs %d)! Please remove "
00372 "the pose file path if you don't want to use it (current file path=%s).",
00373 (int)poses.size(), this->imagesCount(), filePath.c_str());
00374 return false;
00375 }
00376 else if((format == 1 || format == 5 || format == 6 || format == 7 || format == 9) && inOutStamps.size() == 0)
00377 {
00378 UERROR("When using RGBD-SLAM, GPS, MALAGA, ST LUCIA and EuRoC MAV formats, images must have timestamps!");
00379 return false;
00380 }
00381 else if(format == 1 || format == 5 || format == 6 || format == 7 || format == 9)
00382 {
00383 UDEBUG("");
00384
00385 outputPoses.clear();
00386 std::map<double, int> stampsToIds;
00387 for(std::map<int, double>::iterator iter=stamps.begin(); iter!=stamps.end(); ++iter)
00388 {
00389 stampsToIds.insert(std::make_pair(iter->second, iter->first));
00390 }
00391 std::vector<double> values = uValues(stamps);
00392
00393 int validPoses = 0;
00394 for(std::list<double>::iterator ster=inOutStamps.begin(); ster!=inOutStamps.end(); ++ster)
00395 {
00396 Transform pose;
00397 std::map<double, int>::iterator endIter = stampsToIds.lower_bound(*ster);
00398 bool warned = false;
00399 if(endIter != stampsToIds.end())
00400 {
00401 if(endIter->first == *ster)
00402 {
00403 pose = poses.at(endIter->second);
00404 }
00405 else if(endIter != stampsToIds.begin())
00406 {
00407
00408 std::map<double, int>::iterator beginIter = endIter;
00409 --beginIter;
00410 double stampBeg = beginIter->first;
00411 double stampEnd = endIter->first;
00412 UASSERT(stampEnd > stampBeg && *ster>stampBeg && *ster < stampEnd);
00413 if(fabs(*ster-stampEnd) > maxTimeDiff || fabs(*ster-stampBeg) > maxTimeDiff)
00414 {
00415 if(!warned)
00416 {
00417 UWARN("Cannot interpolate pose for stamp %f between %f and %f (> maximum time diff of %f sec)",
00418 *ster,
00419 stampBeg,
00420 stampEnd,
00421 maxTimeDiff);
00422 }
00423 warned=true;
00424 }
00425 else
00426 {
00427 warned=false;
00428 float t = (*ster - stampBeg) / (stampEnd-stampBeg);
00429 Transform & ta = poses.at(beginIter->second);
00430 Transform & tb = poses.at(endIter->second);
00431 if(!ta.isNull() && !tb.isNull())
00432 {
00433 ++validPoses;
00434 pose = ta.interpolate(t, tb);
00435 }
00436 }
00437 }
00438 }
00439 if(pose.isNull() && !warned)
00440 {
00441 UDEBUG("Pose not found for stamp %f", *ster);
00442 }
00443 outputPoses.push_back(pose);
00444 }
00445 if(validPoses != (int)inOutStamps.size())
00446 {
00447 UWARN("%d valid poses of %d stamps", validPoses, (int)inOutStamps.size());
00448 }
00449 }
00450 else
00451 {
00452 UDEBUG("");
00453 outputPoses = uValuesList(poses);
00454 if(inOutStamps.size() == 0 && stamps.size() == poses.size())
00455 {
00456 inOutStamps = uValuesList(stamps);
00457 }
00458 else if(format==8 && inOutStamps.size() == 0 && stamps.size()>0 && stamps.size() != poses.size())
00459 {
00460 UERROR("With Karlsruhe format, timestamps (%d) and poses (%d) should match!", (int)stamps.size(), (int)poses.size());
00461 return false;
00462 }
00463 }
00464 UASSERT_MSG(outputPoses.size() == inOutStamps.size(), uFormat("%d vs %d", (int)outputPoses.size(), (int)inOutStamps.size()).c_str());
00465 return true;
00466 }
00467
00468 bool CameraImages::isCalibrated() const
00469 {
00470 return _model.isValidForProjection();
00471 }
00472
00473 std::string CameraImages::getSerial() const
00474 {
00475 return _model.name();
00476 }
00477
00478 unsigned int CameraImages::imagesCount() const
00479 {
00480 if(_dir)
00481 {
00482 return (unsigned int)_dir->getFileNames().size();
00483 }
00484 return 0;
00485 }
00486
00487 std::vector<std::string> CameraImages::filenames() const
00488 {
00489 if(_dir)
00490 {
00491 return uListToVector(_dir->getFileNames());
00492 }
00493 return std::vector<std::string>();
00494 }
00495
00496 SensorData CameraImages::captureImage(CameraInfo * info)
00497 {
00498 if(_syncImageRateWithStamps && _captureDelay>0.0)
00499 {
00500 int sleepTime = (1000*_captureDelay - 1000.0f*_captureTimer.getElapsedTime());
00501 if(sleepTime > 2)
00502 {
00503 uSleep(sleepTime-2);
00504 }
00505 else if(sleepTime < 0)
00506 {
00507 if(this->getImageRate() > 0.0f)
00508 {
00509 UWARN("CameraImages: Cannot read images as fast as their timestamps (target delay=%fs, capture time=%fs). Disable "
00510 "source image rate or disable synchronization of capture time with timestamps.",
00511 _captureDelay, _captureTimer.getElapsedTime());
00512 }
00513 else
00514 {
00515 UWARN("CameraImages: Cannot read images as fast as their timestamps (target delay=%fs, capture time=%fs).",
00516 _captureDelay, _captureTimer.getElapsedTime());
00517 }
00518 }
00519
00520
00521 while(_captureTimer.getElapsedTime() < _captureDelay-0.000001)
00522 {
00523
00524 }
00525 _captureTimer.start();
00526 }
00527 _captureDelay = 0.0;
00528
00529 cv::Mat img;
00530 LaserScan scan(cv::Mat(), _scanMaxPts, 0, LaserScan::kUnknown, _scanLocalTransform);
00531 double stamp = UTimer::now();
00532 Transform odometryPose;
00533 Transform groundTruthPose;
00534 cv::Mat depthFromScan;
00535 UDEBUG("");
00536 if(_dir->isValid())
00537 {
00538 if(_refreshDir)
00539 {
00540 _dir->update();
00541 if(_scanDir)
00542 {
00543 _scanDir->update();
00544 }
00545 }
00546 std::string imageFilePath;
00547 std::string scanFilePath;
00548 if(_startAt < 0)
00549 {
00550 const std::list<std::string> & fileNames = _dir->getFileNames();
00551 if(fileNames.size())
00552 {
00553 if(_lastFileName.empty() || uStrNumCmp(_lastFileName,*fileNames.rbegin()) < 0)
00554 {
00555 _lastFileName = *fileNames.rbegin();
00556 imageFilePath = _path + _lastFileName;
00557 }
00558 }
00559 if(_scanDir)
00560 {
00561 const std::list<std::string> & scanFileNames = _scanDir->getFileNames();
00562 if(scanFileNames.size())
00563 {
00564 if(_lastScanFileName.empty() || uStrNumCmp(_lastScanFileName,*scanFileNames.rbegin()) < 0)
00565 {
00566 _lastScanFileName = *scanFileNames.rbegin();
00567 scanFilePath = _scanPath + _lastScanFileName;
00568 }
00569 }
00570 }
00571
00572 if(_stamps.size())
00573 {
00574 stamp = _stamps.front();
00575 _stamps.pop_front();
00576 if(_stamps.size())
00577 {
00578 _captureDelay = _stamps.front() - stamp;
00579 }
00580 if(odometry_.size())
00581 {
00582 odometryPose = odometry_.front();
00583 odometry_.pop_front();
00584 }
00585 if(groundTruth_.size())
00586 {
00587 groundTruthPose = groundTruth_.front();
00588 groundTruth_.pop_front();
00589 }
00590 }
00591 }
00592 else
00593 {
00594 std::string fileName;
00595 fileName = _dir->getNextFileName();
00596 if(!fileName.empty())
00597 {
00598 imageFilePath = _path + fileName;
00599 if(_stamps.size())
00600 {
00601 stamp = _stamps.front();
00602 _stamps.pop_front();
00603 if(_stamps.size())
00604 {
00605 _captureDelay = _stamps.front() - stamp;
00606 }
00607 if(odometry_.size())
00608 {
00609 odometryPose = odometry_.front();
00610 odometry_.pop_front();
00611 }
00612 if(groundTruth_.size())
00613 {
00614 groundTruthPose = groundTruth_.front();
00615 groundTruth_.pop_front();
00616 }
00617 }
00618
00619 while(_count++ < _startAt && (fileName = _dir->getNextFileName()).size())
00620 {
00621 imageFilePath = _path + fileName;
00622 if(_stamps.size())
00623 {
00624 stamp = _stamps.front();
00625 _stamps.pop_front();
00626 if(_stamps.size())
00627 {
00628 _captureDelay = _stamps.front() - stamp;
00629 }
00630 if(odometry_.size())
00631 {
00632 odometryPose = odometry_.front();
00633 odometry_.pop_front();
00634 }
00635 if(groundTruth_.size())
00636 {
00637 groundTruthPose = groundTruth_.front();
00638 groundTruth_.pop_front();
00639 }
00640 }
00641 }
00642 }
00643 if(_scanDir)
00644 {
00645 fileName = _scanDir->getNextFileName();
00646 if(!fileName.empty())
00647 {
00648 scanFilePath = _scanPath + fileName;
00649 while(++_countScan < _startAt && (fileName = _scanDir->getNextFileName()).size())
00650 {
00651 scanFilePath = _scanPath + fileName;
00652 }
00653 }
00654 }
00655 }
00656
00657 if(!imageFilePath.empty())
00658 {
00659 ULOGGER_DEBUG("Loading image : %s", imageFilePath.c_str());
00660
00661 #if CV_MAJOR_VERSION >2 || (CV_MAJOR_VERSION >=2 && CV_MINOR_VERSION >=4)
00662 img = cv::imread(imageFilePath.c_str(), cv::IMREAD_UNCHANGED);
00663 #else
00664 img = cv::imread(imageFilePath.c_str(), -1);
00665 #endif
00666 UDEBUG("width=%d, height=%d, channels=%d, elementSize=%d, total=%d",
00667 img.cols, img.rows, img.channels(), img.elemSize(), img.total());
00668
00669 if(_isDepth)
00670 {
00671 if(img.type() != CV_16UC1 && img.type() != CV_32FC1)
00672 {
00673 UERROR("Depth is on and the loaded image has not a format supported (file = \"%s\", type=%d). "
00674 "Formats supported are 16 bits 1 channel (mm) and 32 bits 1 channel (m).",
00675 imageFilePath.c_str(), img.type());
00676 img = cv::Mat();
00677 }
00678
00679 if(_depthScaleFactor > 1.0f)
00680 {
00681 img /= _depthScaleFactor;
00682 }
00683 }
00684 else
00685 {
00686 #if CV_MAJOR_VERSION < 3
00687
00688 if(img.depth() != CV_8U)
00689 {
00690
00691 UWARN("Cannot read the image correctly, falling back to old OpenCV C interface...");
00692 IplImage * i = cvLoadImage(imageFilePath.c_str());
00693 img = cv::Mat(i, true);
00694 cvReleaseImage(&i);
00695 }
00696 #endif
00697 if(img.channels()>3)
00698 {
00699 UWARN("Conversion from 4 channels to 3 channels (file=%s)", imageFilePath.c_str());
00700 cv::Mat out;
00701 cv::cvtColor(img, out, CV_BGRA2BGR);
00702 img = out;
00703 }
00704 else if(_bayerMode >= 0 && _bayerMode <=3)
00705 {
00706 cv::Mat debayeredImg;
00707 try
00708 {
00709 cv::cvtColor(img, debayeredImg, CV_BayerBG2BGR + _bayerMode);
00710 img = debayeredImg;
00711 }
00712 catch(const cv::Exception & e)
00713 {
00714 UWARN("Error debayering images: \"%s\". Please set bayer mode to -1 if images are not bayered!", e.what());
00715 }
00716 }
00717
00718 }
00719
00720 if(!img.empty() && _model.isValidForRectification() && _rectifyImages)
00721 {
00722 img = _model.rectifyImage(img);
00723 }
00724 }
00725
00726 if(!scanFilePath.empty())
00727 {
00728
00729 scan = util3d::loadScan(scanFilePath);
00730 scan = LaserScan(scan.data(), _scanMaxPts, 0.0f, scan.format(), _scanLocalTransform);
00731 UDEBUG("Loaded scan=%d points", (int)scan.size());
00732 if(_depthFromScan && !img.empty())
00733 {
00734 UDEBUG("Computing depth from scan...");
00735 if(!_model.isValidForProjection())
00736 {
00737 UWARN("Depth from laser scan: Camera model should be valid.");
00738 }
00739 else if(_isDepth)
00740 {
00741 UWARN("Depth from laser scan: Loading already a depth image.");
00742 }
00743 else
00744 {
00745 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::laserScanToPointCloud(scan, scan.localTransform());
00746 depthFromScan = util3d::projectCloudToCamera(img.size(), _model.K(), cloud, _model.localTransform());
00747 if(_depthFromScanFillHoles!=0)
00748 {
00749 util3d::fillProjectedCloudHoles(depthFromScan, _depthFromScanFillHoles>0, _depthFromScanFillHolesFromBorder);
00750 }
00751 }
00752 }
00753
00754 scan = util3d::commonFiltering(scan, _scanDownsampleStep, 0, 0, _scanVoxelSize, _scanNormalsK, _scanNormalsRadius, _scanForceGroundNormalsUp);
00755 }
00756 }
00757 else
00758 {
00759 UWARN("Directory is not set, camera must be initialized.");
00760 }
00761
00762 if(_model.imageHeight() == 0 || _model.imageWidth() == 0)
00763 {
00764 _model.setImageSize(img.size());
00765 }
00766
00767 SensorData data(scan, _isDepth?cv::Mat():img, _isDepth?img:depthFromScan, _model, this->getNextSeqID(), stamp);
00768 data.setGroundTruth(groundTruthPose);
00769
00770 if(info && !odometryPose.isNull())
00771 {
00772 info->odomPose = odometryPose;
00773 info->odomCovariance = cv::Mat::eye(6,6,CV_64FC1);
00774 }
00775
00776 return data;
00777 }
00778
00779
00780
00782
00784 CameraVideo::CameraVideo(
00785 int usbDevice,
00786 bool rectifyImages,
00787 float imageRate,
00788 const Transform & localTransform) :
00789 Camera(imageRate, localTransform),
00790 _rectifyImages(rectifyImages),
00791 _src(kUsbDevice),
00792 _usbDevice(usbDevice)
00793 {
00794
00795 }
00796
00797 CameraVideo::CameraVideo(
00798 const std::string & filePath,
00799 bool rectifyImages,
00800 float imageRate,
00801 const Transform & localTransform) :
00802 Camera(imageRate, localTransform),
00803 _filePath(filePath),
00804 _rectifyImages(rectifyImages),
00805 _src(kVideoFile),
00806 _usbDevice(0)
00807 {
00808 }
00809
00810 CameraVideo::~CameraVideo()
00811 {
00812 _capture.release();
00813 }
00814
00815 bool CameraVideo::init(const std::string & calibrationFolder, const std::string & cameraName)
00816 {
00817 _guid = cameraName;
00818 if(_capture.isOpened())
00819 {
00820 _capture.release();
00821 }
00822
00823 if(_src == kUsbDevice)
00824 {
00825 ULOGGER_DEBUG("CameraVideo::init() Usb device initialization on device %d", _usbDevice);
00826 _capture.open(_usbDevice);
00827 }
00828 else if(_src == kVideoFile)
00829 {
00830 ULOGGER_DEBUG("Camera: filename=\"%s\"", _filePath.c_str());
00831 _capture.open(_filePath.c_str());
00832 }
00833 else
00834 {
00835 ULOGGER_ERROR("Camera: Unknown source...");
00836 }
00837 if(!_capture.isOpened())
00838 {
00839 ULOGGER_ERROR("Camera: Failed to create a capture object!");
00840 _capture.release();
00841 return false;
00842 }
00843 else
00844 {
00845 if (_guid.empty())
00846 {
00847 unsigned int guid = (unsigned int)_capture.get(CV_CAP_PROP_GUID);
00848 if (guid != 0 && guid != 0xffffffff)
00849 {
00850 _guid = uFormat("%08x", guid);
00851 }
00852 }
00853
00854
00855 if(!calibrationFolder.empty() && !_guid.empty())
00856 {
00857 if(!_model.load(calibrationFolder, _guid))
00858 {
00859 UWARN("Missing calibration files for camera \"%s\" in \"%s\" folder, you should calibrate the camera!",
00860 _guid.c_str(), calibrationFolder.c_str());
00861 }
00862 else
00863 {
00864 UINFO("Camera parameters: fx=%f fy=%f cx=%f cy=%f",
00865 _model.fx(),
00866 _model.fy(),
00867 _model.cx(),
00868 _model.cy());
00869 }
00870 }
00871 _model.setLocalTransform(this->getLocalTransform());
00872 if(_rectifyImages && !_model.isValidForRectification())
00873 {
00874 UERROR("Parameter \"rectifyImages\" is set, but no camera model is loaded or valid.");
00875 return false;
00876 }
00877 }
00878 return true;
00879 }
00880
00881 bool CameraVideo::isCalibrated() const
00882 {
00883 return _model.isValidForProjection();
00884 }
00885
00886 std::string CameraVideo::getSerial() const
00887 {
00888 return _guid;
00889 }
00890
00891 SensorData CameraVideo::captureImage(CameraInfo * info)
00892 {
00893 cv::Mat img;
00894 if(_capture.isOpened())
00895 {
00896 if(_capture.read(img))
00897 {
00898 if(_model.imageHeight() == 0 || _model.imageWidth() == 0)
00899 {
00900 _model.setImageSize(img.size());
00901 }
00902
00903 if(_model.isValidForRectification() && _rectifyImages)
00904 {
00905 img = _model.rectifyImage(img);
00906 }
00907 else
00908 {
00909
00910 img = img.clone();
00911 }
00912 }
00913 else if(_usbDevice)
00914 {
00915 UERROR("Camera has been disconnected!");
00916 }
00917 }
00918 else
00919 {
00920 ULOGGER_WARN("The camera must be initialized before requesting an image.");
00921 }
00922
00923 return SensorData(img, _model, this->getNextSeqID(), UTimer::now());
00924 }
00925
00926 }