From eb21280f0486260b0ac94b4672200f85979f7c99 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Tue, 19 Jan 2016 21:01:56 -0500 Subject: [PATCH] Changed CameraModel::isValid() to CameraModel::isValidForProjection() for clarity (in contrast to CameraModel::isValidForRectification()) --- corelib/include/rtabmap/core/CameraModel.h | 12 +- corelib/include/rtabmap/core/SensorData.h | 2 +- .../include/rtabmap/core/StereoCameraModel.h | 2 +- corelib/src/CameraModel.cpp | 4 +- corelib/src/CameraRGB.cpp | 14 +-- corelib/src/CameraRGBD.cpp | 4 +- corelib/src/CameraStereo.cpp | 10 +- corelib/src/CameraThread.cpp | 10 +- corelib/src/DBDriverSqlite3.cpp | 4 +- corelib/src/Features2d.cpp | 2 +- corelib/src/Memory.cpp | 12 +- corelib/src/Odometry.cpp | 4 +- corelib/src/OdometryF2F.cpp | 4 +- corelib/src/OdometryMono.cpp | 4 +- corelib/src/OdometryThread.cpp | 4 +- corelib/src/RegistrationVis.cpp | 18 +-- corelib/src/StereoCameraModel.cpp | 22 ++-- corelib/src/util3d.cpp | 10 +- corelib/src/util3d_features.cpp | 6 +- corelib/src/util3d_motion_estimation.cpp | 2 +- examples/NoEventsExample/MapBuilder.h | 2 +- examples/RGBDMapping/MapBuilder.h | 2 +- .../include/rtabmap/gui/CalibrationDialog.h | 2 +- guilib/src/CalibrationDialog.cpp | 115 +++++++++--------- guilib/src/CameraViewer.cpp | 2 +- guilib/src/CreateSimpleCalibrationDialog.cpp | 6 +- guilib/src/DatabaseViewer.cpp | 6 +- guilib/src/MainWindow.cpp | 16 +-- guilib/src/OdometryViewer.cpp | 2 +- tools/CameraRGBD/main.cpp | 8 +- tools/StereoEval/main.cpp | 2 +- 31 files changed, 157 insertions(+), 156 deletions(-) diff --git a/corelib/include/rtabmap/core/CameraModel.h b/corelib/include/rtabmap/core/CameraModel.h index 7ed3aecd..5cf8b11d 100644 --- a/corelib/include/rtabmap/core/CameraModel.h +++ b/corelib/include/rtabmap/core/CameraModel.h @@ -74,7 +74,7 @@ public: void initRectificationMap(); - bool isValid() const {return (!K_.empty() || !P_.empty()) && fx()>0.0 && fy()>0.0;} + bool isValidForProjection() const {return fx()>0.0 && fy()>0.0;} bool isValidForRectification() const { return imageSize_.width>0 && @@ -94,10 +94,12 @@ public: double cy() const {return P_.empty()?K_.empty()?0.0:K_.at(1,2):P_.at(1,2);} double Tx() const {return P_.empty()?0.0:P_.at(0,3);} - const cv::Mat & K() const {return K_;} //intrinsic camera matrix - const cv::Mat & D() const {return D_;} //intrinsic distorsion matrix - const cv::Mat & R() const {return R_;} //rectification matrix - const cv::Mat & P() const {return P_;} //projection matrix + cv::Mat K_raw() const {return K_;} //intrinsic camera matrix (before rectification) + cv::Mat D_raw() const {return D_;} //intrinsic distorsion matrix (before rectification) + cv::Mat K() const {return !P_.empty()?P_.colRange(0,3):K_;} // if P exists, return rectified version + cv::Mat D() const {return P_.empty()&&!D_.empty()?D_:cv::Mat::zeros(1,4,CV_64FC1);} // if P exists, return rectified version + cv::Mat R() const {return R_;} //rectification matrix + cv::Mat P() const {return P_;} //projection matrix void setLocalTransform(const Transform & transform) {localTransform_ = transform;} const Transform & localTransform() const {return localTransform_;} diff --git a/corelib/include/rtabmap/core/SensorData.h b/corelib/include/rtabmap/core/SensorData.h index 104ba5b8..666b8ad0 100644 --- a/corelib/include/rtabmap/core/SensorData.h +++ b/corelib/include/rtabmap/core/SensorData.h @@ -139,7 +139,7 @@ public: _laserScanRaw.empty() && _laserScanCompressed.empty() && _cameraModels.size() == 0 && - !_stereoCameraModel.isValid() && + !_stereoCameraModel.isValidForProjection() && _userDataRaw.empty() && _userDataCompressed.empty() && _keypoints.size() == 0 && diff --git a/corelib/include/rtabmap/core/StereoCameraModel.h b/corelib/include/rtabmap/core/StereoCameraModel.h index 37089d6b..f26cb4d2 100644 --- a/corelib/include/rtabmap/core/StereoCameraModel.h +++ b/corelib/include/rtabmap/core/StereoCameraModel.h @@ -80,7 +80,7 @@ public: const Transform & localTransform = Transform::getIdentity()); virtual ~StereoCameraModel() {} - bool isValid() const {return left_.isValid() && right_.isValid() && baseline() > 0.0;} + bool isValidForProjection() const {return left_.isValidForProjection() && right_.isValidForProjection() && baseline() > 0.0;} bool isValidForRectification() const {return left_.isValidForRectification() && right_.isValidForRectification();} void initRectificationMap() {left_.initRectificationMap(); right_.initRectificationMap();} diff --git a/corelib/src/CameraModel.cpp b/corelib/src/CameraModel.cpp index c2c8f5b7..e7601d26 100644 --- a/corelib/src/CameraModel.cpp +++ b/corelib/src/CameraModel.cpp @@ -346,7 +346,7 @@ CameraModel CameraModel::scaled(double scale) const { CameraModel scaledModel = *this; UASSERT(scale > 0.0); - if(this->isValid()) + if(this->isValidForProjection()) { // has only effect on K and P cv::Mat K; @@ -397,6 +397,7 @@ double CameraModel::verticalFOV() const cv::Mat CameraModel::rectifyImage(const cv::Mat & raw, int interpolation) const { + UDEBUG(""); if(!mapX_.empty() && !mapY_.empty()) { cv::Mat rectified; @@ -413,6 +414,7 @@ cv::Mat CameraModel::rectifyImage(const cv::Mat & raw, int interpolation) const //inspired from https://github.com/code-iai/iai_kinect2/blob/master/depth_registration/src/depth_registration_cpu.cpp cv::Mat CameraModel::rectifyDepth(const cv::Mat & raw) const { + UDEBUG(""); UASSERT(raw.type() == CV_16UC1); if(!mapX_.empty() && !mapY_.empty()) { diff --git a/corelib/src/CameraRGB.cpp b/corelib/src/CameraRGB.cpp index 0666fc2d..12d5886a 100644 --- a/corelib/src/CameraRGB.cpp +++ b/corelib/src/CameraRGB.cpp @@ -216,7 +216,7 @@ bool CameraImages::init(const std::string & calibrationFolder, const std::string _model.setName(cameraName); _model.setLocalTransform(this->getLocalTransform()); - if(_rectifyImages && !_model.isValid()) + if(_rectifyImages && !_model.isValidForRectification()) { UERROR("Parameter \"rectifyImages\" is set, but no camera model is loaded or valid."); return false; @@ -401,7 +401,7 @@ bool CameraImages::init(const std::string & calibrationFolder, const std::string bool CameraImages::isCalibrated() const { - return _model.isValid(); + return _model.isValidForProjection(); } std::string CameraImages::getSerial() const @@ -609,7 +609,7 @@ SensorData CameraImages::captureImage() } - if(!img.empty() && _model.isValid() && _rectifyImages) + if(!img.empty() && _model.isValidForRectification() && _rectifyImages) { img = _model.rectifyImage(img); } @@ -623,7 +623,7 @@ SensorData CameraImages::captureImage() if(_depthFromScan && !img.empty()) { UDEBUG("Computing depth from scan..."); - if(!_model.isValid()) + if(!_model.isValidForProjection()) { UWARN("Depth from laser scan: Camera model should be valid."); } @@ -760,7 +760,7 @@ bool CameraVideo::init(const std::string & calibrationFolder, const std::string } } _model.setLocalTransform(this->getLocalTransform()); - if(_rectifyImages && !_model.isValid()) + if(_rectifyImages && !_model.isValidForRectification()) { UERROR("Parameter \"rectifyImages\" is set, but no camera model is loaded or valid."); return false; @@ -771,7 +771,7 @@ bool CameraVideo::init(const std::string & calibrationFolder, const std::string bool CameraVideo::isCalibrated() const { - return _model.isValid(); + return _model.isValidForProjection(); } std::string CameraVideo::getSerial() const @@ -786,7 +786,7 @@ SensorData CameraVideo::captureImage() { if(_capture.read(img)) { - if(_model.isValid() && (_src != kVideoFile || _rectifyImages)) + if(_model.isValidForRectification() && (_src != kVideoFile || _rectifyImages)) { img = _model.rectifyImage(img); } diff --git a/corelib/src/CameraRGBD.cpp b/corelib/src/CameraRGBD.cpp index 1743156c..8db8b78f 100644 --- a/corelib/src/CameraRGBD.cpp +++ b/corelib/src/CameraRGBD.cpp @@ -1373,7 +1373,7 @@ SensorData CameraFreenect2::captureImage() cv::flip(rgb, rgb, 1); cv::flip(depth, depth, 1); - if(stereoModel_.isValid()) + if(stereoModel_.isValidForRectification()) { //rectify rgb = stereoModel_.left().rectifyImage(rgb); @@ -1708,7 +1708,7 @@ bool CameraRGBDImages::init(const std::string & calibrationFolder, const std::st bool CameraRGBDImages::isCalibrated() const { - return this->cameraModel().isValid(); + return this->cameraModel().isValidForProjection(); } std::string CameraRGBDImages::getSerial() const diff --git a/corelib/src/CameraStereo.cpp b/corelib/src/CameraStereo.cpp index 1ed09f53..4ab1c39e 100644 --- a/corelib/src/CameraStereo.cpp +++ b/corelib/src/CameraStereo.cpp @@ -396,7 +396,7 @@ bool CameraStereoDC1394::init(const std::string & calibrationFolder, const std:: bool CameraStereoDC1394::isCalibrated() const { - return stereoModel_.isValid(); + return stereoModel_.isValidForProjection(); } std::string CameraStereoDC1394::getSerial() const @@ -431,7 +431,7 @@ SensorData CameraStereoDC1394::captureImage() right = stereoModel_.right().rectifyImage(right); } StereoCameraModel model; - if(stereoModel_.isValid()) + if(stereoModel_.isValidForProjection()) { model = StereoCameraModel( stereoModel_.left().fx(), //fx @@ -849,7 +849,7 @@ bool CameraStereoImages::init(const std::string & calibrationFolder, const std:: bool CameraStereoImages::isCalibrated() const { - return stereoModel_.isValid(); + return stereoModel_.isValidForProjection(); } std::string CameraStereoImages::getSerial() const @@ -970,7 +970,7 @@ bool CameraStereoVideo::init(const std::string & calibrationFolder, const std::s bool CameraStereoVideo::isCalibrated() const { - return stereoModel_.isValid(); + return stereoModel_.isValidForProjection(); } std::string CameraStereoVideo::getSerial() const @@ -999,7 +999,7 @@ SensorData CameraStereoVideo::captureImage() rightCvt = true; } - if(rectifyImages_ && stereoModel_.left().isValid() && stereoModel_.right().isValid()) + if(rectifyImages_ && stereoModel_.left().isValidForRectification() && stereoModel_.right().isValidForRectification()) { leftImage = stereoModel_.left().rectifyImage(leftImage); rightImage = stereoModel_.right().rectifyImage(rightImage); diff --git a/corelib/src/CameraThread.cpp b/corelib/src/CameraThread.cpp index ec1788c0..00b1c935 100644 --- a/corelib/src/CameraThread.cpp +++ b/corelib/src/CameraThread.cpp @@ -105,14 +105,14 @@ void CameraThread::mainLoop() std::vector models = data.cameraModels(); for(unsigned int i=0; i Feature2D::generateKeypoints3D( const std::vector & keypoints) const { std::vector keypoints3D; - if(!data.depthOrRightRaw().empty() && !data.imageRaw().empty() && data.stereoCameraModel().isValid()) + if(!data.depthOrRightRaw().empty() && !data.imageRaw().empty() && data.stereoCameraModel().isValidForProjection()) { //stereo cv::Mat imageMono; diff --git a/corelib/src/Memory.cpp b/corelib/src/Memory.cpp index 7248a270..ed4ba407 100644 --- a/corelib/src/Memory.cpp +++ b/corelib/src/Memory.cpp @@ -3019,7 +3019,7 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p if(!data.depthOrRightRaw().empty() && data.cameraModels().size() == 0 && - !data.stereoCameraModel().isValid()) + !data.stereoCameraModel().isValidForProjection()) { UERROR("Rectified images required! Calibrate your camera."); return 0; @@ -3110,8 +3110,8 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p { descriptors = cv::Mat(); } - else if((!data.depthRaw().empty() && data.cameraModels().size() && data.cameraModels()[0].isValid()) || - (!data.rightRaw().empty() && data.stereoCameraModel().isValid())) + else if((!data.depthRaw().empty() && data.cameraModels().size() && data.cameraModels()[0].isValidForProjection()) || + (!data.rightRaw().empty() && data.stereoCameraModel().isValidForProjection())) { keypoints3D = _feature2D->generateKeypoints3D(data, keypoints); t = timer.ticks(); @@ -3253,7 +3253,7 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p { cameraModels[i] = cameraModels[i].scaled(1.0/double(_imageDecimation)); } - if(stereoCameraModel.isValid()) + if(stereoCameraModel.isValidForProjection()) { stereoCameraModel.scale(1.0/double(_imageDecimation)); } @@ -3306,7 +3306,7 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p "", pose, data.groundTruth(), - stereoCameraModel.isValid()? + stereoCameraModel.isValidForProjection()? SensorData( ctLaserScan.getCompressedData(), maxLaserScanMaxPts, @@ -3337,7 +3337,7 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p "", pose, data.groundTruth(), - stereoCameraModel.isValid()? + stereoCameraModel.isValidForProjection()? SensorData( cv::Mat(), 0, diff --git a/corelib/src/Odometry.cpp b/corelib/src/Odometry.cpp index 0fafc3fd..a2846f7c 100644 --- a/corelib/src/Odometry.cpp +++ b/corelib/src/Odometry.cpp @@ -201,8 +201,8 @@ Transform Odometry::process(const SensorData & data, OdometryInfo * info) UASSERT(!data.imageRaw().empty()); - if(!data.stereoCameraModel().isValid() && - (data.cameraModels().size() == 0 || !data.cameraModels()[0].isValid())) + if(!data.stereoCameraModel().isValidForProjection() && + (data.cameraModels().size() == 0 || !data.cameraModels()[0].isValidForProjection())) { UERROR("Rectified images required! Calibrate your camera."); return Transform(); diff --git a/corelib/src/OdometryF2F.cpp b/corelib/src/OdometryF2F.cpp index 18c87bae..f29119e3 100644 --- a/corelib/src/OdometryF2F.cpp +++ b/corelib/src/OdometryF2F.cpp @@ -64,13 +64,13 @@ Transform OdometryF2F::computeTransform( { UTimer timer; Transform output; - if(!data.rightRaw().empty() && !data.stereoCameraModel().isValid()) + if(!data.rightRaw().empty() && !data.stereoCameraModel().isValidForProjection()) { UERROR("Calibrated stereo camera required"); return output; } if(!data.depthRaw().empty() && - (data.cameraModels().size() != 1 || !data.cameraModels()[0].isValid())) + (data.cameraModels().size() != 1 || !data.cameraModels()[0].isValidForProjection())) { UERROR("Calibrated camera required (multi-cameras not supported)."); return output; diff --git a/corelib/src/OdometryMono.cpp b/corelib/src/OdometryMono.cpp index f474465a..58de340a 100644 --- a/corelib/src/OdometryMono.cpp +++ b/corelib/src/OdometryMono.cpp @@ -189,14 +189,14 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo * return output; } - if(!(((data.cameraModels().size() == 1 && data.cameraModels()[0].isValid()) || data.stereoCameraModel().isValid()))) + if(!(((data.cameraModels().size() == 1 && data.cameraModels()[0].isValidForProjection()) || data.stereoCameraModel().isValidForProjection()))) { UERROR("Odometry cannot be done without calibration or on multi-camera!"); return output; } - const CameraModel & cameraModel = data.stereoCameraModel().isValid()?data.stereoCameraModel().left():data.cameraModels()[0]; + const CameraModel & cameraModel = data.stereoCameraModel().isValidForProjection()?data.stereoCameraModel().left():data.cameraModels()[0]; UTimer timer; diff --git a/corelib/src/OdometryThread.cpp b/corelib/src/OdometryThread.cpp index ce9e1668..3b8eb5db 100644 --- a/corelib/src/OdometryThread.cpp +++ b/corelib/src/OdometryThread.cpp @@ -105,7 +105,7 @@ void OdometryThread::addData(const SensorData & data) { if(dynamic_cast(_odometry) == 0 && dynamic_cast(_odometry) == 0) { - if(data.imageRaw().empty() || data.depthOrRightRaw().empty() || (data.cameraModels().size()==0 && !data.stereoCameraModel().isValid())) + if(data.imageRaw().empty() || data.depthOrRightRaw().empty() || (data.cameraModels().size()==0 && !data.stereoCameraModel().isValidForProjection())) { ULOGGER_ERROR("Missing some information (images empty or missing calibration)!?"); return; @@ -114,7 +114,7 @@ void OdometryThread::addData(const SensorData & data) else { // Mono and BOW can accept RGB only - if(data.imageRaw().empty() || (data.cameraModels().size()==0 && !data.stereoCameraModel().isValid())) + if(data.imageRaw().empty() || (data.cameraModels().size()==0 && !data.stereoCameraModel().isValidForProjection())) { ULOGGER_ERROR("Missing some information (image empty or missing calibration)!?"); return; diff --git a/corelib/src/RegistrationVis.cpp b/corelib/src/RegistrationVis.cpp index d05d59b7..4d6b93c9 100644 --- a/corelib/src/RegistrationVis.cpp +++ b/corelib/src/RegistrationVis.cpp @@ -532,17 +532,17 @@ Transform RegistrationVis::computeTransformationImpl( if(_estimationType == 2) // Epipolar Geometry { UDEBUG(""); - if(!signatureB->sensorData().stereoCameraModel().isValid() && + if(!signatureB->sensorData().stereoCameraModel().isValidForProjection() && (signatureB->sensorData().cameraModels().size() != 1 || - !signatureB->sensorData().cameraModels()[0].isValid())) + !signatureB->sensorData().cameraModels()[0].isValidForProjection())) { UERROR("Calibrated camera required (multi-cameras not supported)."); } else if((int)signatureA->getWords().size() >= _minInliers && (int)signatureB->getWords().size() >= _minInliers) { - UASSERT(signatureA->sensorData().stereoCameraModel().isValid() || (signatureA->sensorData().cameraModels().size() == 1 && signatureA->sensorData().cameraModels()[0].isValid())); - const CameraModel & cameraModel = signatureA->sensorData().stereoCameraModel().isValid()?signatureA->sensorData().stereoCameraModel().left():signatureA->sensorData().cameraModels()[0]; + UASSERT(signatureA->sensorData().stereoCameraModel().isValidForProjection() || (signatureA->sensorData().cameraModels().size() == 1 && signatureA->sensorData().cameraModels()[0].isValidForProjection())); + const CameraModel & cameraModel = signatureA->sensorData().stereoCameraModel().isValidForProjection()?signatureA->sensorData().stereoCameraModel().left():signatureA->sensorData().cameraModels()[0]; // we only need the camera transform, send guess words3 for scale estimation Transform cameraTransform; @@ -602,14 +602,14 @@ Transform RegistrationVis::computeTransformationImpl( else if(_estimationType == 1) // PnP { UDEBUG(""); - if(!signatureB->sensorData().stereoCameraModel().isValid() && + if(!signatureB->sensorData().stereoCameraModel().isValidForProjection() && (signatureB->sensorData().cameraModels().size() != 1 || - !signatureB->sensorData().cameraModels()[0].isValid())) + !signatureB->sensorData().cameraModels()[0].isValidForProjection())) { UERROR("Calibrated camera required (multi-cameras not supported). Id=%d Models=%d StereoModel=%d weight=%d", signatureB->id(), (int)signatureB->sensorData().cameraModels().size(), - signatureB->sensorData().stereoCameraModel().isValid()?1:0, + signatureB->sensorData().stereoCameraModel().isValidForProjection()?1:0, signatureB->getWeight()); } else @@ -619,8 +619,8 @@ Transform RegistrationVis::computeTransformationImpl( if((int)signatureA->getWords3().size() >= _minInliers && (int)signatureB->getWords().size() >= _minInliers) { - UASSERT(signatureB->sensorData().stereoCameraModel().isValid() || (signatureB->sensorData().cameraModels().size() == 1 && signatureB->sensorData().cameraModels()[0].isValid())); - const CameraModel & cameraModel = signatureB->sensorData().stereoCameraModel().isValid()?signatureB->sensorData().stereoCameraModel().left():signatureB->sensorData().cameraModels()[0]; + UASSERT(signatureB->sensorData().stereoCameraModel().isValidForProjection() || (signatureB->sensorData().cameraModels().size() == 1 && signatureB->sensorData().cameraModels()[0].isValidForProjection())); + const CameraModel & cameraModel = signatureB->sensorData().stereoCameraModel().isValidForProjection()?signatureB->sensorData().stereoCameraModel().left():signatureB->sensorData().cameraModels()[0]; std::vector inliersV; std::vector matchesV; diff --git a/corelib/src/StereoCameraModel.cpp b/corelib/src/StereoCameraModel.cpp index 3f17a02a..c8e54354 100644 --- a/corelib/src/StereoCameraModel.cpp +++ b/corelib/src/StereoCameraModel.cpp @@ -84,13 +84,13 @@ StereoCameraModel::StereoCameraModel( UASSERT(leftCameraModel.isValidForRectification() && rightCameraModel.isValidForRectification()); cv::Mat R1,R2,P1,P2,Q; - cv::stereoRectify(left_.K(), left_.D(), - right_.K(), right_.D(), + cv::stereoRectify(left_.K_raw(), left_.D_raw(), + right_.K_raw(), right_.D_raw(), left_.imageSize(), R_, T_, R1, R2, P1, P2, Q, cv::CALIB_ZERO_DISPARITY, 0, left_.imageSize()); - left_ = CameraModel(left_.name(), left_.imageSize(), left_.K(), left_.D(), R1, P1, left_.localTransform()); - right_ = CameraModel(right_.name(), right_.imageSize(), right_.K(), right_.D(), R2, P2, right_.localTransform()); + left_ = CameraModel(left_.name(), left_.imageSize(), left_.K_raw(), left_.D_raw(), R1, P1, left_.localTransform()); + right_ = CameraModel(right_.name(), right_.imageSize(), right_.K_raw(), right_.D_raw(), R2, P2, right_.localTransform()); } } @@ -114,13 +114,13 @@ StereoCameraModel::StereoCameraModel( extrinsics.translationMatrix().convertTo(T_, CV_64FC1); cv::Mat R1,R2,P1,P2,Q; - cv::stereoRectify(left_.K(), left_.D(), - right_.K(), right_.D(), + cv::stereoRectify(left_.K_raw(), left_.D_raw(), + right_.K_raw(), right_.D_raw(), left_.imageSize(), R_, T_, R1, R2, P1, P2, Q, cv::CALIB_ZERO_DISPARITY, 0, left_.imageSize()); - left_ = CameraModel(left_.name(), left_.imageSize(), left_.K(), left_.D(), R1, P1, left_.localTransform()); - right_ = CameraModel(right_.name(), right_.imageSize(), right_.K(), right_.D(), R2, P2, right_.localTransform()); + left_ = CameraModel(left_.name(), left_.imageSize(), left_.K_raw(), left_.D_raw(), R1, P1, left_.localTransform()); + right_ = CameraModel(right_.name(), right_.imageSize(), right_.K_raw(), right_.D_raw(), R2, P2, right_.localTransform()); } } @@ -345,7 +345,7 @@ void StereoCameraModel::scale(double scale) float StereoCameraModel::computeDepth(float disparity) const { //depth = baseline * f / (disparity + cx1-cx0); - UASSERT(this->isValid()); + UASSERT(this->isValidForProjection()); if(disparity == 0.0f) { return 0.0f; @@ -356,7 +356,7 @@ float StereoCameraModel::computeDepth(float disparity) const float StereoCameraModel::computeDisparity(float depth) const { // disparity = (baseline * fx / depth) - (cx1-cx0); - UASSERT(this->isValid()); + UASSERT(this->isValidForProjection()); if(depth == 0.0f) { return 0.0f; @@ -367,7 +367,7 @@ float StereoCameraModel::computeDisparity(float depth) const float StereoCameraModel::computeDisparity(unsigned short depth) const { // disparity = (baseline * fx / depth) - (cx1-cx0); - UASSERT(this->isValid()); + UASSERT(this->isValidForProjection()); if(depth == 0) { return 0.0f; diff --git a/corelib/src/util3d.cpp b/corelib/src/util3d.cpp index 9e7256bc..aaff9e4b 100644 --- a/corelib/src/util3d.cpp +++ b/corelib/src/util3d.cpp @@ -524,7 +524,7 @@ pcl::PointCloud::Ptr RTABMAP_EXP cloudFromSensorData( int subImageWidth = sensorData.depthRaw().cols/sensorData.cameraModels().size(); for(unsigned int i=0; i::Ptr tmp = util3d::cloudFromDepth( cv::Mat(sensorData.depthRaw(), cv::Rect(subImageWidth*i, 0, subImageWidth, sensorData.depthRaw().rows)), @@ -579,7 +579,7 @@ pcl::PointCloud::Ptr RTABMAP_EXP cloudFromSensorData( cloud = util3d::voxelize(cloud, voxelSize); } } - else if(!sensorData.imageRaw().empty() && !sensorData.rightRaw().empty() && sensorData.stereoCameraModel().isValid()) + else if(!sensorData.imageRaw().empty() && !sensorData.rightRaw().empty() && sensorData.stereoCameraModel().isValidForProjection()) { //stereo UASSERT(sensorData.rightRaw().type() == CV_8UC1); @@ -636,7 +636,7 @@ pcl::PointCloud::Ptr RTABMAP_EXP cloudRGBFromSensorData( { UASSERT(!sensorData.imageRaw().empty()); UASSERT((!sensorData.depthRaw().empty() && sensorData.cameraModels().size()) || - (!sensorData.rightRaw().empty() && sensorData.stereoCameraModel().isValid())); + (!sensorData.rightRaw().empty() && sensorData.stereoCameraModel().isValidForProjection())); pcl::PointCloud::Ptr cloud(new pcl::PointCloud); if(!sensorData.depthRaw().empty() && sensorData.cameraModels().size()) @@ -648,7 +648,7 @@ pcl::PointCloud::Ptr RTABMAP_EXP cloudRGBFromSensorData( int subImageWidth = sensorData.imageRaw().cols/sensorData.cameraModels().size(); for(unsigned int i=0; i::Ptr RTABMAP_EXP cloudRGBFromSensorData( cloud = util3d::voxelize(cloud, voxelSize); } } - else if(!sensorData.rightRaw().empty() && sensorData.stereoCameraModel().isValid()) + else if(!sensorData.rightRaw().empty() && sensorData.stereoCameraModel().isValidForProjection()) { //stereo UDEBUG(""); diff --git a/corelib/src/util3d_features.cpp b/corelib/src/util3d_features.cpp index bde1344b..d62b87d0 100644 --- a/corelib/src/util3d_features.cpp +++ b/corelib/src/util3d_features.cpp @@ -53,7 +53,7 @@ std::vector generateKeypoints3DDepth( const cv::Mat & depth, const CameraModel & cameraModel) { - UASSERT(cameraModel.isValid()); + UASSERT(cameraModel.isValidForProjection()); std::vector models; models.push_back(cameraModel); return generateKeypoints3DDepth(keypoints, depth, models); @@ -107,7 +107,7 @@ std::vector generateKeypoints3DDisparity( const StereoCameraModel & stereoCameraModel) { UASSERT(!disparity.empty() && (disparity.type() == CV_16SC1 || disparity.type() == CV_32F)); - UASSERT(stereoCameraModel.isValid()); + UASSERT(stereoCameraModel.isValidForProjection()); std::vector keypoints3d; keypoints3d.resize(keypoints.size()); for(unsigned int i=0; i!=keypoints.size(); ++i) @@ -189,7 +189,7 @@ std::map generateWords3DMono( const std::map & refGuess3D, double * varianceOut) { - UASSERT(cameraModel.isValid()); + UASSERT(cameraModel.isValidForProjection()); std::map words3D; std::list > > pairs; if(EpipolarGeometry::findPairs(refWords, nextWords, pairs) > 8) diff --git a/corelib/src/util3d_motion_estimation.cpp b/corelib/src/util3d_motion_estimation.cpp index ee090073..406e798c 100644 --- a/corelib/src/util3d_motion_estimation.cpp +++ b/corelib/src/util3d_motion_estimation.cpp @@ -56,7 +56,7 @@ Transform estimateMotion3DTo2D( std::vector * matchesOut, std::vector * inliersOut) { - UASSERT(cameraModel.isValid()); + UASSERT(cameraModel.isValidForProjection()); UASSERT(!guess.isNull()); Transform transform; std::vector matches, inliers; diff --git a/examples/NoEventsExample/MapBuilder.h b/examples/NoEventsExample/MapBuilder.h index 6c85ff85..a0d44923 100644 --- a/examples/NoEventsExample/MapBuilder.h +++ b/examples/NoEventsExample/MapBuilder.h @@ -109,7 +109,7 @@ public: if(data.depthOrRightRaw().cols == data.imageRaw().cols && data.depthOrRightRaw().rows == data.imageRaw().rows && !data.depthOrRightRaw().empty() && - (data.stereoCameraModel().isValid() || data.cameraModels().size())) + (data.stereoCameraModel().isValidForProjection() || data.cameraModels().size())) { pcl::PointCloud::Ptr cloud = util3d::cloudRGBFromSensorData( data, diff --git a/examples/RGBDMapping/MapBuilder.h b/examples/RGBDMapping/MapBuilder.h index 6166fe18..dfd72e7a 100644 --- a/examples/RGBDMapping/MapBuilder.h +++ b/examples/RGBDMapping/MapBuilder.h @@ -129,7 +129,7 @@ protected slots: if(odom.data().depthOrRightRaw().cols == odom.data().imageRaw().cols && odom.data().depthOrRightRaw().rows == odom.data().imageRaw().rows && !odom.data().depthOrRightRaw().empty() && - (odom.data().stereoCameraModel().isValid() || odom.data().cameraModels().size())) + (odom.data().stereoCameraModel().isValidForProjection() || odom.data().cameraModels().size())) { pcl::PointCloud::Ptr cloud = util3d::cloudRGBFromSensorData( odom.data(), diff --git a/guilib/include/rtabmap/gui/CalibrationDialog.h b/guilib/include/rtabmap/gui/CalibrationDialog.h index 6de9cd4f..0a24d520 100644 --- a/guilib/include/rtabmap/gui/CalibrationDialog.h +++ b/guilib/include/rtabmap/gui/CalibrationDialog.h @@ -51,7 +51,7 @@ public: CalibrationDialog(bool stereo = false, const QString & savingDirectory = ".", bool switchImages = false, QWidget * parent = 0); virtual ~CalibrationDialog(); - bool isCalibrated() const {return models_[0].isValid() && (stereo_?models_[1].isValid():true);} + bool isCalibrated() const {return models_[0].isValidForProjection() && (stereo_?models_[1].isValidForProjection():true);} const rtabmap::CameraModel & getLeftCameraModel() const {return models_[0];} const rtabmap::CameraModel & getRightCameraModel() const {return models_[1];} const rtabmap::StereoCameraModel & getStereoCameraModel() const {return stereoModel_;} diff --git a/guilib/src/CalibrationDialog.cpp b/guilib/src/CalibrationDialog.cpp index 5141d567..caa6cc2f 100644 --- a/guilib/src/CalibrationDialog.cpp +++ b/guilib/src/CalibrationDialog.cpp @@ -200,10 +200,9 @@ void CalibrationDialog::setSquareSize(double size) void CalibrationDialog::closeEvent(QCloseEvent* event) { - if(!savedCalibration_ && models_[0].isValid() && + if(!savedCalibration_ && models_[0].isValidForRectification() && (!stereo_ || - (stereoModel_.left().isValid() && - stereoModel_.right().isValid()&& + (stereoModel_.isValidForRectification() && (!ui_->label_baseline->isVisible() || stereoModel_.baseline() > 0.0)))) { QMessageBox::StandardButton b = QMessageBox::question(this, tr("Save calibration?"), @@ -737,7 +736,7 @@ void CalibrationDialog::calibrate() } } - if(stereo_ && models_[0].isValid() && models_[1].isValid()) + if(stereo_ && models_[0].isValidForRectification() && models_[1].isValidForRectification()) { UINFO("stereo calibration (samples=%d)...", (int)stereoImagePoints_[0].size()); cv::Size imageSize = imageSize_[0].width > imageSize_[1].width?imageSize_[0]:imageSize_[1]; @@ -758,8 +757,8 @@ void CalibrationDialog::calibrate() objectPoints, stereoImagePoints_[0], stereoImagePoints_[1], - models_[0].K(), models_[0].D(), - models_[1].K(), models_[1].D(), + models_[0].K_raw(), models_[0].D_raw(), + models_[1].K_raw(), models_[1].D_raw(), imageSize, R, T, E, F, cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, 100, 1e-5), cv::CALIB_FIX_INTRINSIC); @@ -768,72 +767,66 @@ void CalibrationDialog::calibrate() objectPoints, stereoImagePoints_[0], stereoImagePoints_[1], - models_[0].K(), models_[0].D(), - models_[1].K(), models_[1].D(), + models_[0].K_raw(), models_[0].D_raw(), + models_[1].K_raw(), models_[1].D_raw(), imageSize, R, T, E, F, cv::CALIB_FIX_INTRINSIC, cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, 100, 1e-5)); #endif UINFO("stereo calibration... done with RMS error=%f", rms); - double err = 0; - int npoints = 0; - std::vector lines[2]; - UINFO("Computing avg re-projection error..."); - for(unsigned int i = 0; i < stereoImagePoints_[0].size(); i++ ) + if(imageSize_[0] == imageSize_[1]) { - int npt = (int)stereoImagePoints_[0][i].size(); - cv::Mat imgpt[2]; - for(int k = 0; k < 2; k++ ) + //Stereo, compute stereo rectification + + cv::Mat R1, R2, P1, P2, Q; + cv::stereoRectify(models_[0].K_raw(), models_[0].D_raw(), + models_[1].K_raw(), models_[1].D_raw(), + imageSize, R, T, R1, R2, P1, P2, Q, + cv::CALIB_ZERO_DISPARITY, 0, imageSize); + + double err = 0; + int npoints = 0; + std::vector lines[2]; + UINFO("Computing avg re-projection error..."); + for(unsigned int i = 0; i < stereoImagePoints_[0].size(); i++ ) { - imgpt[k] = cv::Mat(stereoImagePoints_[k][i]); - cv::undistortPoints(imgpt[k], imgpt[k], models_[k].K(), models_[k].D(), cv::Mat(), models_[k].K()); - computeCorrespondEpilines(imgpt[k], k+1, F, lines[k]); + int npt = (int)stereoImagePoints_[0][i].size(); + + cv::Mat imgpt0 = cv::Mat(stereoImagePoints_[0][i]); + cv::Mat imgpt1 = cv::Mat(stereoImagePoints_[1][i]); + cv::undistortPoints(imgpt0, imgpt0, models_[0].K_raw(), models_[0].D_raw(), R1, P1); + cv::undistortPoints(imgpt1, imgpt1, models_[1].K_raw(), models_[1].D_raw(), R2, P2); + computeCorrespondEpilines(imgpt0, 1, F, lines[0]); + computeCorrespondEpilines(imgpt1, 2, F, lines[1]); + + for(int j = 0; j < npt; j++ ) + { + double errij = fabs(stereoImagePoints_[0][i][j].x*lines[1][j][0] + + stereoImagePoints_[0][i][j].y*lines[1][j][1] + lines[1][j][2]) + + fabs(stereoImagePoints_[1][i][j].x*lines[0][j][0] + + stereoImagePoints_[1][i][j].y*lines[0][j][1] + lines[0][j][2]); + err += errij; + } + npoints += npt; } - for(int j = 0; j < npt; j++ ) - { - double errij = fabs(stereoImagePoints_[0][i][j].x*lines[1][j][0] + - stereoImagePoints_[0][i][j].y*lines[1][j][1] + lines[1][j][2]) + - fabs(stereoImagePoints_[1][i][j].x*lines[0][j][0] + - stereoImagePoints_[1][i][j].y*lines[0][j][1] + lines[0][j][2]); - err += errij; - } - npoints += npt; - } - double totalAvgErr = err/(double)npoints; + double totalAvgErr = err/(double)npoints; + UINFO("stereo avg re projection error = %f", totalAvgErr); - UINFO("stereo avg re projection error = %f", totalAvgErr); - - cv::Mat R1, R2, P1, P2, Q; - cv::Rect validRoi[2]; - - cv::stereoRectify(models_[0].K(), models_[0].D(), - models_[1].K(), models_[1].D(), - imageSize, R, T, R1, R2, P1, P2, Q, - cv::CALIB_ZERO_DISPARITY, 0, imageSize, &validRoi[0], &validRoi[1]); - - UINFO("Valid ROI1 = %d,%d,%d,%d ROI2 = %d,%d,%d,%d newImageSize=%d/%d", - validRoi[0].x, validRoi[0].y, validRoi[0].width, validRoi[0].height, - validRoi[1].x, validRoi[1].y, validRoi[1].width, validRoi[1].height, - imageSize.width, imageSize.height); - - if(imageSize_[0].width == imageSize_[1].width) - { - //Stereo, keep new extrinsic projection matrix stereoModel_ = StereoCameraModel( - cameraName_.toStdString(), - imageSize_[0], models_[0].K(), models_[0].D(), R1, P1, - imageSize_[1], models_[1].K(), models_[1].D(), R2, P2, - R, T, E, F); + cameraName_.toStdString(), + imageSize_[0], models_[0].K_raw(), models_[0].D_raw(), R1, P1, + imageSize_[1], models_[1].K_raw(), models_[1].D_raw(), R2, P2, + R, T, E, F); } else { - //Kinect + //Kinect, ignore the stereo rectification stereoModel_ = StereoCameraModel( - cameraName_.toStdString(), - imageSize_[0], models_[0].K(), models_[0].D(), cv::Mat::eye(3,3,CV_64FC1), models_[0].P(), - imageSize_[1], models_[1].K(), models_[1].D(), cv::Mat::eye(3,3,CV_64FC1), models_[1].P(), - R, T, E, F); + cameraName_.toStdString(), + imageSize_[0], models_[0].K_raw(), models_[0].D_raw(), models_[0].R(), models_[0].P(), + imageSize_[1], models_[1].K_raw(), models_[1].D_raw(), models_[1].R(), models_[1].P(), + R, T, E, F); } std::stringstream strR1, strP1, strR2, strP2; @@ -854,6 +847,8 @@ void CalibrationDialog::calibrate() stereoModel_.isValidForRectification()) { stereoModel_.initRectificationMap(); + models_[0].initRectificationMap(); + models_[1].initRectificationMap(); ui_->radioButton_rectified->setEnabled(true); ui_->radioButton_stereoRectified->setEnabled(true); ui_->radioButton_stereoRectified->setChecked(true); @@ -877,7 +872,7 @@ bool CalibrationDialog::save() processingData_ = true; if(!stereo_) { - UASSERT(models_[0].isValid()); + UASSERT(models_[0].isValidForRectification()); QString cameraName = models_[0].name().c_str(); QString filePath = QFileDialog::getSaveFileName(this, tr("Export"), savingDirectory_+"/"+cameraName+".yaml", "*.yaml"); @@ -901,8 +896,8 @@ bool CalibrationDialog::save() } else { - UASSERT(stereoModel_.left().isValid() && - stereoModel_.right().isValid()&& + UASSERT(stereoModel_.left().isValidForRectification() && + stereoModel_.right().isValidForRectification() && (!ui_->label_baseline->isVisible() || stereoModel_.baseline() > 0.0)); QString cameraName = stereoModel_.name().c_str(); QString filePath = QFileDialog::getSaveFileName(this, tr("Export"), savingDirectory_ + "/" + cameraName, "*.yaml"); diff --git a/guilib/src/CameraViewer.cpp b/guilib/src/CameraViewer.cpp index fb589dd1..88ea6c42 100644 --- a/guilib/src/CameraViewer.cpp +++ b/guilib/src/CameraViewer.cpp @@ -139,7 +139,7 @@ void CameraViewer::showImage(const rtabmap::SensorData & data) } if(!data.depthOrRightRaw().empty() && - (data.stereoCameraModel().isValid() || (data.cameraModels().size() && data.cameraModels().at(0).isValid()))) + (data.stereoCameraModel().isValidForProjection() || (data.cameraModels().size() && data.cameraModels().at(0).isValidForProjection()))) { if(showCloudCheckbox_->isChecked()) { diff --git a/guilib/src/CreateSimpleCalibrationDialog.cpp b/guilib/src/CreateSimpleCalibrationDialog.cpp index 3f38e65c..18b5b236 100644 --- a/guilib/src/CreateSimpleCalibrationDialog.cpp +++ b/guilib/src/CreateSimpleCalibrationDialog.cpp @@ -171,7 +171,7 @@ void CreateSimpleCalibrationDialog::saveCalibration() ui_->doubleSpinBox_fy->value(), ui_->doubleSpinBox_cx->value(), ui_->doubleSpinBox_cy->value()); - UASSERT(modelLeft.isValid()); + UASSERT(modelLeft.isValidForProjection()); } else { @@ -216,8 +216,9 @@ void CreateSimpleCalibrationDialog::saveCalibration() ui_->doubleSpinBox_cy->value(), Transform::getIdentity(), ui_->doubleSpinBox_baseline->value()*-ui_->doubleSpinBox_fx->value()); - UASSERT(modelRight.isValid()); + UASSERT(modelRight.isValidForProjection()); stereoModel = StereoCameraModel(name.toStdString(), modelLeft, modelRight, Transform()); + UASSERT(stereoModel.isValidForProjection()); } else if(ui_->comboBox_advanced->currentIndex() == 1) { @@ -250,6 +251,7 @@ void CreateSimpleCalibrationDialog::saveCalibration() UASSERT(Transform::canParseString(ui_->lineEdit_RT->text().trimmed().toStdString())); stereoModel = StereoCameraModel(name.toStdString(), modelLeft, modelRight, Transform::fromString(ui_->lineEdit_RT->text().toStdString())); + UASSERT(stereoModel.isValidForRectification()); } std::string base = (dir+QDir::separator()+name).toStdString(); diff --git a/guilib/src/DatabaseViewer.cpp b/guilib/src/DatabaseViewer.cpp index bc3a8a96..98248330 100644 --- a/guilib/src/DatabaseViewer.cpp +++ b/guilib/src/DatabaseViewer.cpp @@ -866,7 +866,7 @@ void DatabaseViewer::extractImages() { UERROR("Cannot save calibration file, database name is empty!"); } - else if(data.stereoCameraModel().isValid()) + else if(data.stereoCameraModel().isValidForProjection()) { std::string cameraName = uSplit(databaseFileName_, '.').front(); StereoCameraModel model( @@ -913,7 +913,7 @@ void DatabaseViewer::extractImages() { UERROR("Only one camera calibration can be saved at this time (%d detected)", (int)data.cameraModels().size()); } - else if(data.cameraModels().size() == 1 && data.cameraModels().front().isValid()) + else if(data.cameraModels().size() == 1 && data.cameraModels().front().isValidForProjection()) { std::string cameraName = uSplit(databaseFileName_, '.').front(); CameraModel model(cameraName, @@ -2349,7 +2349,7 @@ void DatabaseViewer::updateStereo(const SensorData * data) !data->imageRaw().empty() && !data->depthOrRightRaw().empty() && data->depthOrRightRaw().type() == CV_8UC1 && - data->stereoCameraModel().isValid()) + data->stereoCameraModel().isValidForProjection()) { cv::Mat leftMono; if(data->imageRaw().channels() == 3) diff --git a/guilib/src/MainWindow.cpp b/guilib/src/MainWindow.cpp index f8b43b03..0013b650 100644 --- a/guilib/src/MainWindow.cpp +++ b/guilib/src/MainWindow.cpp @@ -807,7 +807,7 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom) if(odom.data().depthOrRightRaw().cols == odom.data().imageRaw().cols && odom.data().depthOrRightRaw().rows == odom.data().imageRaw().rows && !odom.data().depthOrRightRaw().empty() && - (odom.data().cameraModels().size() || odom.data().stereoCameraModel().isValid()) && + (odom.data().cameraModels().size() || odom.data().stereoCameraModel().isValidForProjection()) && _preferencesDialog->isCloudsShown(1)) { pcl::PointCloud::Ptr cloud; @@ -5097,11 +5097,11 @@ bool MainWindow::getExportedClouds( { const Signature & s = _cachedSignatures.value(jter->first); CameraModel model; - if(s.sensorData().stereoCameraModel().isValid()) + if(s.sensorData().stereoCameraModel().isValidForProjection()) { model = s.sensorData().stereoCameraModel().left(); } - else if(s.sensorData().cameraModels().size() == 1 && s.sensorData().cameraModels()[0].isValid()) + else if(s.sensorData().cameraModels().size() == 1 && s.sensorData().cameraModels()[0].isValidForProjection()) { model = s.sensorData().cameraModels()[0]; } @@ -5110,7 +5110,7 @@ bool MainWindow::getExportedClouds( { s.sensorData().uncompressDataConst(&image, 0, 0, 0); } - if(!jter->second.isNull() && model.isValid() && !image.empty()) + if(!jter->second.isNull() && model.isValidForProjection() && !image.empty()) { cameraPoses.insert(std::make_pair(jter->first, jter->second)); cameraModels.insert(std::make_pair(jter->first, model)); @@ -5171,7 +5171,7 @@ void MainWindow::exportImages() QDir dir; dir.mkdir(QString("%1/left").arg(path)); dir.mkdir(QString("%1/right").arg(path)); - if(data.stereoCameraModel().isValid()) + if(data.stereoCameraModel().isValidForProjection()) { std::string cameraName = "calibration"; StereoCameraModel model( @@ -5214,7 +5214,7 @@ void MainWindow::exportImages() { UERROR("Only one camera calibration can be saved at this time (%d detected)", (int)data.cameraModels().size()); } - else if(data.cameraModels().size() == 1 && data.cameraModels().front().isValid()) + else if(data.cameraModels().size() == 1 && data.cameraModels().front().isValidForProjection()) { std::string cameraName = "calibration"; CameraModel model(cameraName, @@ -5313,8 +5313,8 @@ void MainWindow::exportBundlerFormat() { UWARN("Missing image in cache for node %d", iter->first); } - else if((_cachedSignatures[iter->first].sensorData().cameraModels().size() == 1 && _cachedSignatures[iter->first].sensorData().cameraModels().at(0).isValid()) || - _cachedSignatures[iter->first].sensorData().stereoCameraModel().isValid()) + else if((_cachedSignatures[iter->first].sensorData().cameraModels().size() == 1 && _cachedSignatures[iter->first].sensorData().cameraModels().at(0).isValidForProjection()) || + _cachedSignatures[iter->first].sensorData().stereoCameraModel().isValidForProjection()) { poses.insert(*iter); } diff --git a/guilib/src/OdometryViewer.cpp b/guilib/src/OdometryViewer.cpp index 3f62d0c6..0ad0720b 100644 --- a/guilib/src/OdometryViewer.cpp +++ b/guilib/src/OdometryViewer.cpp @@ -192,7 +192,7 @@ void OdometryViewer::processData(const rtabmap::OdometryEvent & odom) if(!odom.data().imageRaw().empty() && !odom.data().depthOrRightRaw().empty() && - (odom.data().stereoCameraModel().isValid() || odom.data().cameraModels().size())) + (odom.data().stereoCameraModel().isValidForProjection() || odom.data().cameraModels().size())) { UDEBUG("New pose = %s, quality=%d", odom.pose().prettyPrint().c_str(), quality); diff --git a/tools/CameraRGBD/main.cpp b/tools/CameraRGBD/main.cpp index 3f5cfaa5..c7739057 100644 --- a/tools/CameraRGBD/main.cpp +++ b/tools/CameraRGBD/main.cpp @@ -254,7 +254,7 @@ int main(int argc, char * argv[]) data.imageRaw().cols, data.imageRaw().rows, data.depthOrRightRaw().cols, data.depthOrRightRaw().rows); } pcl::visualization::CloudViewer * viewer = 0; - if(!data.stereoCameraModel().isValid() && (data.cameraModels().size() == 0 || !data.cameraModels()[0].isValid())) + if(!data.stereoCameraModel().isValidForProjection() && (data.cameraModels().size() == 0 || !data.cameraModels()[0].isValidForProjection())) { UWARN("Camera not calibrated! The registered cloud cannot be shown."); } @@ -328,7 +328,7 @@ int main(int argc, char * argv[]) if(rgb.cols == depth.cols && rgb.rows == depth.rows && data.cameraModels().size() && - data.cameraModels()[0].isValid()) + data.cameraModels()[0].isValidForProjection()) { pcl::PointCloud::Ptr cloud = rtabmap::util3d::cloudFromDepthRGB( rgb, depth, @@ -342,7 +342,7 @@ int main(int argc, char * argv[]) } else if(!depth.empty() && data.cameraModels().size() && - data.cameraModels()[0].isValid()) + data.cameraModels()[0].isValidForProjection()) { pcl::PointCloud::Ptr cloud = rtabmap::util3d::cloudFromDepth( depth, @@ -369,7 +369,7 @@ int main(int argc, char * argv[]) cv::imshow("Left", rgb); // show frame cv::imshow("Right", right); - if(rgb.cols == right.cols && rgb.rows == right.rows && data.stereoCameraModel().isValid()) + if(rgb.cols == right.cols && rgb.rows == right.rows && data.stereoCameraModel().isValidForProjection()) { if(right.channels() == 3) { diff --git a/tools/StereoEval/main.cpp b/tools/StereoEval/main.cpp index 67605dc7..611c14aa 100644 --- a/tools/StereoEval/main.cpp +++ b/tools/StereoEval/main.cpp @@ -213,7 +213,7 @@ int main(int argc, char * argv[]) CameraModel(K[0].at(0,0), K[0].at(1,1), K[0].at(0,2), K[0].at(1,2)), CameraModel(K[1].at(0,0), K[1].at(1,1), K[1].at(0,2), K[1].at(1,2), Transform::getIdentity(), -baseline/K[1].at(0,0))); - UASSERT(model.isValid()); + UASSERT(model.isValidForProjection()); UINFO("Processing...");