i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 01:20:25 +08:00
Code Issues Packages Projects Releases Wiki Activity

Changed CameraModel::isValid() to CameraModel::isValidForProjection() for clarity (in contrast to CameraModel::isValidForRectification())

Browse Source
This commit is contained in:
matlabbe
2016-01-19 21:01:56 -05:00
parent d162fcf34e
commit eb21280f04
31 changed files with 157 additions and 156 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
corelib/src/CameraModel.cpp
View File

@@ -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())
{

14
corelib/src/CameraRGB.cpp
View File

@@ -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);
}

4
corelib/src/CameraRGBD.cpp
View File

@@ -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

10
corelib/src/CameraStereo.cpp
View File

@@ -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);

10
corelib/src/CameraThread.cpp
View File

@@ -105,14 +105,14 @@ void CameraThread::mainLoop()
std::vector<CameraModel> models = data.cameraModels();
for(unsigned int i=0; i<models.size(); ++i)
{
if(models[i].isValid())
if(models[i].isValidForProjection())
{
models[i] = models[i].scaled(1.0/double(_imageDecimation));
}
}
data.setCameraModels(models);
StereoCameraModel stereoModel = data.stereoCameraModel();
if(stereoModel.isValid())
if(stereoModel.isValidForProjection())
{
stereoModel.scale(1.0/double(_imageDecimation));
data.setStereoCameraModel(stereoModel);
@@ -127,7 +127,7 @@ void CameraThread::mainLoop()
cv::Mat tmpRgb;
cv::flip(data.imageRaw(), tmpRgb, 1);
data.setImageRaw(tmpRgb);
UASSERT_MSG(data.cameraModels().size() <= 1 && !data.stereoCameraModel().isValid(), "Only single RGBD cameras are supported for mirroring.");
UASSERT_MSG(data.cameraModels().size() <= 1 && !data.stereoCameraModel().isValidForProjection(), "Only single RGBD cameras are supported for mirroring.");
if(data.cameraModels().size() && data.cameraModels()[0].cx())
{
CameraModel tmpModel(
@@ -146,7 +146,7 @@ void CameraThread::mainLoop()
}
info.timeMirroring = timer.ticks();
}
if(_stereoToDepth && data.stereoCameraModel().isValid() && !data.rightRaw().empty())
if(_stereoToDepth && data.stereoCameraModel().isValidForProjection() && !data.rightRaw().empty())
{
UDEBUG("");
UTimer timer;
@@ -162,7 +162,7 @@ void CameraThread::mainLoop()
}
if(_scanFromDepth &&
data.cameraModels().size() &&
data.cameraModels().at(0).isValid() &&
data.cameraModels().at(0).isValidForProjection() &&
!data.depthRaw().empty())
{
UDEBUG("");

4
corelib/src/DBDriverSqlite3.cpp
View File

@@ -2726,7 +2726,7 @@ void DBDriverSqlite3::stepDepth(sqlite3_stmt * ppStmt, const SensorData & sensor
cy = sensorData.cameraModels()[0].cy();
localTransform = sensorData.cameraModels()[0].localTransform();
}
else if(sensorData.stereoCameraModel().isValid())
else if(sensorData.stereoCameraModel().isValidForProjection())
{
fx = sensorData.stereoCameraModel().left().fx();
fyOrBaseline = sensorData.stereoCameraModel().baseline();
@@ -2855,7 +2855,7 @@ void DBDriverSqlite3::stepSensorData(sqlite3_stmt * ppStmt,
memcpy(calibration.data()+i*(4+localTransform.size())+4, localTransform.data(), localTransform.size()*sizeof(float));
}
}
else if(sensorData.stereoCameraModel().isValid())
else if(sensorData.stereoCameraModel().isValidForProjection())
{
const Transform & localTransform = sensorData.stereoCameraModel().left().localTransform();
calibration.resize(5+localTransform.size());

2
corelib/src/Features2d.cpp
View File

@@ -566,7 +566,7 @@ std::vector<cv::Point3f> Feature2D::generateKeypoints3D(
const std::vector<cv::KeyPoint> & keypoints) const
{
std::vector<cv::Point3f> 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;

12
corelib/src/Memory.cpp
View File

@@ -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,

4
corelib/src/Odometry.cpp
View File

@@ -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();

4
corelib/src/OdometryF2F.cpp
View File

@@ -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;

4
corelib/src/OdometryMono.cpp
View File

@@ -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;

4
corelib/src/OdometryThread.cpp
View File

@@ -105,7 +105,7 @@ void OdometryThread::addData(const SensorData & data)
{
if(dynamic_cast<OdometryMono*>(_odometry) == 0 && dynamic_cast<OdometryLocalMap*>(_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;

18
corelib/src/RegistrationVis.cpp
View File

@@ -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<int> inliersV;
std::vector<int> matchesV;

22
corelib/src/StereoCameraModel.cpp
View File

@@ -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;

10
corelib/src/util3d.cpp
View File

@@ -524,7 +524,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromSensorData(
int subImageWidth = sensorData.depthRaw().cols/sensorData.cameraModels().size();
for(unsigned int i=0; i<sensorData.cameraModels().size(); ++i)
{
if(sensorData.cameraModels()[i].isValid())
if(sensorData.cameraModels()[i].isValidForProjection())
{
pcl::PointCloud<pcl::PointXYZ>::Ptr tmp = util3d::cloudFromDepth(
cv::Mat(sensorData.depthRaw(), cv::Rect(subImageWidth*i, 0, subImageWidth, sensorData.depthRaw().rows)),
@@ -579,7 +579,7 @@ pcl::PointCloud<pcl::PointXYZ>::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<pcl::PointXYZRGB>::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<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
if(!sensorData.depthRaw().empty() && sensorData.cameraModels().size())
@@ -648,7 +648,7 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudRGBFromSensorData(
int subImageWidth = sensorData.imageRaw().cols/sensorData.cameraModels().size();
for(unsigned int i=0; i<sensorData.cameraModels().size(); ++i)
{
if(sensorData.cameraModels()[i].isValid())
if(sensorData.cameraModels()[i].isValidForProjection())
{
if(subImageWidth % decimation != 0 || sensorData.depthRaw().rows % decimation != 0)
{
@@ -711,7 +711,7 @@ pcl::PointCloud<pcl::PointXYZRGB>::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("");

6
corelib/src/util3d_features.cpp
View File

@@ -53,7 +53,7 @@ std::vector<cv::Point3f> generateKeypoints3DDepth(
const cv::Mat & depth,
const CameraModel & cameraModel)
{
UASSERT(cameraModel.isValid());
UASSERT(cameraModel.isValidForProjection());
std::vector<CameraModel> models;
models.push_back(cameraModel);
return generateKeypoints3DDepth(keypoints, depth, models);
@@ -107,7 +107,7 @@ std::vector<cv::Point3f> generateKeypoints3DDisparity(
const StereoCameraModel & stereoCameraModel)
{
UASSERT(!disparity.empty() && (disparity.type() == CV_16SC1 || disparity.type() == CV_32F));
UASSERT(stereoCameraModel.isValid());
UASSERT(stereoCameraModel.isValidForProjection());
std::vector<cv::Point3f> keypoints3d;
keypoints3d.resize(keypoints.size());
for(unsigned int i=0; i!=keypoints.size(); ++i)
@@ -189,7 +189,7 @@ std::map<int, cv::Point3f> generateWords3DMono(
const std::map<int, cv::Point3f> & refGuess3D,
double * varianceOut)
{
UASSERT(cameraModel.isValid());
UASSERT(cameraModel.isValidForProjection());
std::map<int, cv::Point3f> words3D;
std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > pairs;
if(EpipolarGeometry::findPairs(refWords, nextWords, pairs) > 8)

2
corelib/src/util3d_motion_estimation.cpp
View File

@@ -56,7 +56,7 @@ Transform estimateMotion3DTo2D(
std::vector<int> * matchesOut,
std::vector<int> * inliersOut)
{
UASSERT(cameraModel.isValid());
UASSERT(cameraModel.isValidForProjection());
UASSERT(!guess.isNull());
Transform transform;
std::vector<int> matches, inliers;