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