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

Fixed compilation error about stereoCameraModel() not found (#905) with odometry approaches: Fovis, MSCKF, Okvis, VINS, Viso2.

Browse Source
This commit is contained in:
matlabbe
2022-09-27 18:56:05 +00:00
parent 55546132a0
commit 675dad5f76
6 changed files with 50 additions and 51 deletions
Download Patch File Download Diff File Expand all files Collapse all files

44
corelib/src/odometry/OdometryFovis.cpp
View File

@@ -123,18 +123,14 @@ Transform OdometryFovis::computeTransform(
return t;
}
if(!((data.cameraModels().size() == 1 &&
data.cameraModels()[0].isValidForReprojection()) ||
(data.stereoCameraModel().isValidForProjection() &&
data.stereoCameraModel().left().isValidForReprojection() &&
data.stereoCameraModel().right().isValidForReprojection())))
if(!((data.cameraModels().size() == 1 && data.cameraModels()[0].isValidForReprojection()) ||
(data.stereoCameraModels().size() == 1 && data.stereoCameraModels()[0].isValidForProjection())))
{
UERROR("Invalid camera model! Mono cameras=%d (reproj=%d), Stereo camera=%d (reproj=%d|%d)",
UERROR("Invalid camera model! Mono cameras=%d (reproj=%d), Stereo cameras=%d (reproj=%d)",
(int)data.cameraModels().size(),
data.cameraModels().size() && data.cameraModels()[0].isValidForReprojection()?1:0,
data.stereoCameraModel().isValidForProjection()?1:0,
data.stereoCameraModel().left().isValidForReprojection()?1:0,
data.stereoCameraModel().right().isValidForReprojection()?1:0);
(int)data.stereoCameraModels().size(),
data.stereoCameraModels().size() && data.stereoCameraModels()[0].isValidForProjection()?1:0);
return t;
}
@@ -254,18 +250,18 @@ Transform OdometryFovis::computeTransform(
depthImage_->setDepthImage((float*)depth.data);
depthSource = depthImage_;
}
else // stereo
else if(data.stereoCameraModels().size() == 1) // stereo
{
UDEBUG("");
// initialize left camera parameters
fovis::CameraIntrinsicsParameters left_parameters;
left_parameters.width = data.stereoCameraModel().left().imageWidth();
left_parameters.height = data.stereoCameraModel().left().imageHeight();
left_parameters.fx = data.stereoCameraModel().left().fx();
left_parameters.fy = data.stereoCameraModel().left().fy();
left_parameters.cx = data.stereoCameraModel().left().cx()==0.0?double(left_parameters.width) / 2.0:data.stereoCameraModel().left().cx();
left_parameters.cy = data.stereoCameraModel().left().cy()==0.0?double(left_parameters.height) / 2.0:data.stereoCameraModel().left().cy();
localTransform = data.stereoCameraModel().localTransform();
left_parameters.width = data.stereoCameraModels()[0].left().imageWidth();
left_parameters.height = data.stereoCameraModels()[0].left().imageHeight();
left_parameters.fx = data.stereoCameraModels()[0].left().fx();
left_parameters.fy = data.stereoCameraModels()[0].left().fy();
left_parameters.cx = data.stereoCameraModels()[0].left().cx()==0.0?double(left_parameters.width) / 2.0:data.stereoCameraModels()[0].left().cx();
left_parameters.cy = data.stereoCameraModels()[0].left().cy()==0.0?double(left_parameters.height) / 2.0:data.stereoCameraModels()[0].left().cy();
localTransform = data.stereoCameraModels()[0].localTransform();
if(rect_ == 0)
{
@@ -277,12 +273,12 @@ Transform OdometryFovis::computeTransform(
{
// initialize right camera parameters
fovis::CameraIntrinsicsParameters right_parameters;
right_parameters.width = data.stereoCameraModel().right().imageWidth();
right_parameters.height = data.stereoCameraModel().right().imageHeight();
right_parameters.fx = data.stereoCameraModel().right().fx();
right_parameters.fy = data.stereoCameraModel().right().fy();
right_parameters.cx = data.stereoCameraModel().right().cx()==0.0?double(right_parameters.width) / 2.0:data.stereoCameraModel().right().cx();
right_parameters.cy = data.stereoCameraModel().right().cy()==0.0?double(right_parameters.height) / 2.0:data.stereoCameraModel().right().cy();
right_parameters.width = data.stereoCameraModels()[0].right().imageWidth();
right_parameters.height = data.stereoCameraModels()[0].right().imageHeight();
right_parameters.fx = data.stereoCameraModels()[0].right().fx();
right_parameters.fy = data.stereoCameraModels()[0].right().fy();
right_parameters.cx = data.stereoCameraModels()[0].right().cx()==0.0?double(right_parameters.width) / 2.0:data.stereoCameraModels()[0].right().cx();
right_parameters.cy = data.stereoCameraModels()[0].right().cy()==0.0?double(right_parameters.height) / 2.0:data.stereoCameraModels()[0].right().cy();
// as we use rectified images, rotation is identity
// and translation is baseline only
@@ -293,7 +289,7 @@ Transform OdometryFovis::computeTransform(
stereo_parameters.right_to_left_rotation[1] = 0.0;
stereo_parameters.right_to_left_rotation[2] = 0.0;
stereo_parameters.right_to_left_rotation[3] = 0.0;
stereo_parameters.right_to_left_translation[0] = -data.stereoCameraModel().baseline();
stereo_parameters.right_to_left_translation[0] = -data.stereoCameraModels()[0].baseline();
stereo_parameters.right_to_left_translation[1] = 0.0;
stereo_parameters.right_to_left_translation[2] = 0.0;

14
corelib/src/odometry/OdometryMSCKF.cpp
View File

@@ -838,7 +838,7 @@ Transform OdometryMSCKF::computeTransform(
if(!data.imageRaw().empty() && !data.rightRaw().empty())
{
UDEBUG("Image update stamp=%f", data.stamp());
if(data.stereoCameraModel().isValidForProjection())
if(data.stereoCameraModels().size() == 1 && data.stereoCameraModels()[0].isValidForProjection())
{
if(msckf_ == 0)
{
@@ -852,13 +852,13 @@ Transform OdometryMSCKF::computeTransform(
imageProcessor_ = new ImageProcessorNoROS(
parameters_,
lastImu_.localTransform(),
data.stereoCameraModel(),
data.stereoCameraModels()[0],
this->imagesAlreadyRectified());
UINFO("Creating MsckfVioNoROS...");
msckf_ = new MsckfVioNoROS(
parameters_,
lastImu_.localTransform(),
data.stereoCameraModel(),
data.stereoCameraModels()[0],
this->imagesAlreadyRectified());
}
@@ -975,10 +975,10 @@ Transform OdometryMSCKF::computeTransform(
if(this->imagesAlreadyRectified())
{
info->newCorners.resize(measurements->features.size());
float fx = data.stereoCameraModel().left().fx();
float fy = data.stereoCameraModel().left().fy();
float cx = data.stereoCameraModel().left().cx();
float cy = data.stereoCameraModel().left().cy();
float fx = data.stereoCameraModels()[0].left().fx();
float fy = data.stereoCameraModels()[0].left().fy();
float cx = data.stereoCameraModels()[0].left().cx();
float cy = data.stereoCameraModels()[0].left().cy();
info->reg.inliersIDs.resize(measurements->features.size());
for(unsigned int i=0; i<measurements->features.size(); ++i)
{

12
corelib/src/odometry/OdometryOkvis.cpp
View File

@@ -221,21 +221,21 @@ Transform OdometryOkvis::computeTransform(
UDEBUG("Image update stamp=%f", data.stamp());
std::vector<cv::Mat> images;
std::vector<CameraModel> models;
if(data.stereoCameraModel().isValidForProjection())
if(data.stereoCameraModels().size() ==1 && data.stereoCameraModels()[0].isValidForProjection())
{
images.push_back(data.imageRaw());
images.push_back(data.rightRaw());
CameraModel mleft = data.stereoCameraModel().left();
CameraModel mleft = data.stereoCameraModels()[0].left();
// should be transform between IMU and camera
mleft.setLocalTransform(lastImu_.localTransform().inverse()*mleft.localTransform());
models.push_back(mleft);
CameraModel mright = data.stereoCameraModel().right();
CameraModel mright = data.stereoCameraModels()[0].right();
// To support not rectified images
if(!imagesAlreadyRectified())
{
cv::Mat R = data.stereoCameraModel().R();
cv::Mat T = data.stereoCameraModel().T();
cv::Mat R = data.stereoCameraModels()[0].R();
cv::Mat T = data.stereoCameraModels()[0].T();
UASSERT(R.cols==3 && R.rows == 3);
UASSERT(T.cols==1 && T.rows == 3);
Transform extrinsics(R.at<double>(0,0), R.at<double>(0,1), R.at<double>(0,2), T.at<double>(0,0),
@@ -246,7 +246,7 @@ Transform OdometryOkvis::computeTransform(
else
{
Transform extrinsics(1, 0, 0, 0,
0, 1, 0, data.stereoCameraModel().baseline(),
0, 1, 0, data.stereoCameraModels()[0].baseline(),
0, 0, 1, 0);
mright.setLocalTransform(extrinsics * mleft.localTransform());
}

10
corelib/src/odometry/OdometryVINS.cpp
View File

@@ -367,7 +367,7 @@ Transform OdometryVINS::computeTransform(
}
}
if(!data.imageRaw().empty() && !data.rightRaw().empty())
if(!data.imageRaw().empty() && !data.rightRaw().empty() && data.stereoCameraModels().size() == 1 && data.stereoCameraModels()[0].isValidForProjection())
{
if(USE_IMU==1 && lastImu_.localTransform().isNull())
{
@@ -379,7 +379,7 @@ Transform OdometryVINS::computeTransform(
// intialize
vinsEstimator_ = new VinsEstimator(
lastImu_.localTransform().isNull()?Transform::getIdentity():lastImu_.localTransform(),
data.stereoCameraModel(),
data.stereoCameraModels()[0],
this->imagesAlreadyRectified());
}
@@ -479,7 +479,7 @@ Transform OdometryVINS::computeTransform(
if(this->imagesAlreadyRectified())
{
cv::Point2f pt;
data.stereoCameraModel().left().reproject(pts_i(0), pts_i(1), pts_i(2), pt.x, pt.y);
data.stereoCameraModels()[0].left().reproject(pts_i(0), pts_i(1), pts_i(2), pt.x, pt.y);
info->reg.inliersIDs.push_back(info->newCorners.size());
info->newCorners.push_back(pt);
}
@@ -503,6 +503,10 @@ Transform OdometryVINS::computeTransform(
{
UERROR("VINS-Fusion requires stereo images!");
}
else
{
UERROR("VINS-Fusion requires stereo images (and only one stereo camera with valid calibration)!");
}
#else
UERROR("RTAB-Map is not built with VINS support! Select another visual odometry approach.");
#endif

15
corelib/src/odometry/OdometryViso2.cpp
View File

@@ -121,9 +121,8 @@ Transform OdometryViso2::computeTransform(
return t;
}
if(!(data.stereoCameraModel().isValidForProjection() &&
data.stereoCameraModel().left().isValidForReprojection() &&
data.stereoCameraModel().right().isValidForReprojection()))
if(!(data.stereoCameraModels().size() == 1 &&
data.stereoCameraModels()[0].isValidForProjection()))
{
UERROR("Invalid stereo camera model!");
return t;
@@ -161,10 +160,10 @@ Transform OdometryViso2::computeTransform(
if(viso2_ == 0)
{
VisualOdometryStereo::parameters params;
params.base = params.match.base = data.stereoCameraModel().baseline();
params.calib.cu = params.match.cu = data.stereoCameraModel().left().cx();
params.calib.cv = params.match.cv = data.stereoCameraModel().left().cy();
params.calib.f = params.match.f = data.stereoCameraModel().left().fx();
params.base = params.match.base = data.stereoCameraModels()[0].baseline();
params.calib.cu = params.match.cu = data.stereoCameraModels()[0].left().cx();
params.calib.cv = params.match.cv = data.stereoCameraModels()[0].left().cy();
params.calib.f = params.match.f = data.stereoCameraModels()[0].left().fx();
Parameters::parse(viso2Parameters_, Parameters::kOdomViso2RansacIters(), params.ransac_iters);
Parameters::parse(viso2Parameters_, Parameters::kOdomViso2InlierThreshold(), params.inlier_threshold);
@@ -255,7 +254,7 @@ Transform OdometryViso2::computeTransform(
}
}
const Transform & localTransform = data.stereoCameraModel().localTransform();
const Transform & localTransform = data.stereoCameraModels()[0].localTransform();
if(!t.isNull() && !t.isIdentity() && !localTransform.isIdentity() && !localTransform.isNull())
{
// from camera frame to base frame