i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 17:40:23 +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

6
CMakeLists.txt
View File

@@ -737,8 +737,8 @@ IF(WITH_ORB_SLAM AND NOT G2O_FOUND)
ENDIF(WITH_ORB_SLAM AND NOT G2O_FOUND) ENDIF(WITH_ORB_SLAM AND NOT G2O_FOUND)
IF(NOT MSVC) IF(NOT MSVC)
IF(loam_velodyne_FOUND OR floam_FOUND OR PCL_VERSION VERSION_GREATER "1.9.1" OR TORCH_FOUND OR G2O_FOUND OR CCCoreLib_FOUND OR Open3D_FOUND) IF((NOT WITH_MSCKF_VIO OR NOT msckf_vio_FOUND) AND (loam_velodyne_FOUND OR floam_FOUND OR PCL_VERSION VERSION_GREATER "1.9.1" OR TORCH_FOUND OR G2O_FOUND OR CCCoreLib_FOUND OR Open3D_FOUND))
#LOAM, PCL>=1.10, latest g2o and CCCoreLib require c++14 #LOAM, PCL>=1.10, latest g2o and CCCoreLib require c++14, but MSCKF_VIO requires c++11
include(CheckCXXCompilerFlag) include(CheckCXXCompilerFlag)
CHECK_CXX_COMPILER_FLAG("-std=c++14" COMPILER_SUPPORTS_CXX14) CHECK_CXX_COMPILER_FLAG("-std=c++14" COMPILER_SUPPORTS_CXX14)
IF(COMPILER_SUPPORTS_CXX14) IF(COMPILER_SUPPORTS_CXX14)
@@ -747,7 +747,7 @@ IF(NOT MSVC)
ELSE() ELSE()
message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++14 support. Please use a different C++ compiler if you want to use LOAM, latest PCL or g2o.") message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++14 support. Please use a different C++ compiler if you want to use LOAM, latest PCL or g2o.")
ENDIF() ENDIF()
ENDIF(loam_velodyne_FOUND OR floam_FOUND OR PCL_VERSION VERSION_GREATER "1.9.1" OR TORCH_FOUND OR G2O_FOUND OR CCCoreLib_FOUND OR Open3D_FOUND) ENDIF()
IF( (NOT (${CMAKE_CXX_STANDARD} STREQUAL "14")) AND ( IF( (NOT (${CMAKE_CXX_STANDARD} STREQUAL "14")) AND (
G2O_FOUND OR G2O_FOUND OR

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

@@ -123,18 +123,14 @@ Transform OdometryFovis::computeTransform(
return t; return t;
} }
if(!((data.cameraModels().size() == 1 && if(!((data.cameraModels().size() == 1 && data.cameraModels()[0].isValidForReprojection()) ||
data.cameraModels()[0].isValidForReprojection()) || (data.stereoCameraModels().size() == 1 && data.stereoCameraModels()[0].isValidForProjection())))
(data.stereoCameraModel().isValidForProjection() &&
data.stereoCameraModel().left().isValidForReprojection() &&
data.stereoCameraModel().right().isValidForReprojection())))
{ {
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(), (int)data.cameraModels().size(),
data.cameraModels().size() && data.cameraModels()[0].isValidForReprojection()?1:0, data.cameraModels().size() && data.cameraModels()[0].isValidForReprojection()?1:0,
data.stereoCameraModel().isValidForProjection()?1:0, (int)data.stereoCameraModels().size(),
data.stereoCameraModel().left().isValidForReprojection()?1:0, data.stereoCameraModels().size() && data.stereoCameraModels()[0].isValidForProjection()?1:0);
data.stereoCameraModel().right().isValidForReprojection()?1:0);
return t; return t;
} }
@@ -254,18 +250,18 @@ Transform OdometryFovis::computeTransform(
depthImage_->setDepthImage((float*)depth.data); depthImage_->setDepthImage((float*)depth.data);
depthSource = depthImage_; depthSource = depthImage_;
} }
else // stereo else if(data.stereoCameraModels().size() == 1) // stereo
{ {
UDEBUG(""); UDEBUG("");
// initialize left camera parameters // initialize left camera parameters
fovis::CameraIntrinsicsParameters left_parameters; fovis::CameraIntrinsicsParameters left_parameters;
left_parameters.width = data.stereoCameraModel().left().imageWidth(); left_parameters.width = data.stereoCameraModels()[0].left().imageWidth();
left_parameters.height = data.stereoCameraModel().left().imageHeight(); left_parameters.height = data.stereoCameraModels()[0].left().imageHeight();
left_parameters.fx = data.stereoCameraModel().left().fx(); left_parameters.fx = data.stereoCameraModels()[0].left().fx();
left_parameters.fy = data.stereoCameraModel().left().fy(); left_parameters.fy = data.stereoCameraModels()[0].left().fy();
left_parameters.cx = data.stereoCameraModel().left().cx()==0.0?double(left_parameters.width) / 2.0:data.stereoCameraModel().left().cx(); 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.stereoCameraModel().left().cy()==0.0?double(left_parameters.height) / 2.0:data.stereoCameraModel().left().cy(); left_parameters.cy = data.stereoCameraModels()[0].left().cy()==0.0?double(left_parameters.height) / 2.0:data.stereoCameraModels()[0].left().cy();
localTransform = data.stereoCameraModel().localTransform(); localTransform = data.stereoCameraModels()[0].localTransform();
if(rect_ == 0) if(rect_ == 0)
{ {
@@ -277,12 +273,12 @@ Transform OdometryFovis::computeTransform(
{ {
// initialize right camera parameters // initialize right camera parameters
fovis::CameraIntrinsicsParameters right_parameters; fovis::CameraIntrinsicsParameters right_parameters;
right_parameters.width = data.stereoCameraModel().right().imageWidth(); right_parameters.width = data.stereoCameraModels()[0].right().imageWidth();
right_parameters.height = data.stereoCameraModel().right().imageHeight(); right_parameters.height = data.stereoCameraModels()[0].right().imageHeight();
right_parameters.fx = data.stereoCameraModel().right().fx(); right_parameters.fx = data.stereoCameraModels()[0].right().fx();
right_parameters.fy = data.stereoCameraModel().right().fy(); right_parameters.fy = data.stereoCameraModels()[0].right().fy();
right_parameters.cx = data.stereoCameraModel().right().cx()==0.0?double(right_parameters.width) / 2.0:data.stereoCameraModel().right().cx(); 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.stereoCameraModel().right().cy()==0.0?double(right_parameters.height) / 2.0:data.stereoCameraModel().right().cy(); 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 // as we use rectified images, rotation is identity
// and translation is baseline only // 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[1] = 0.0;
stereo_parameters.right_to_left_rotation[2] = 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_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[1] = 0.0;
stereo_parameters.right_to_left_translation[2] = 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()) if(!data.imageRaw().empty() && !data.rightRaw().empty())
{ {
UDEBUG("Image update stamp=%f", data.stamp()); UDEBUG("Image update stamp=%f", data.stamp());
if(data.stereoCameraModel().isValidForProjection()) if(data.stereoCameraModels().size() == 1 && data.stereoCameraModels()[0].isValidForProjection())
{ {
if(msckf_ == 0) if(msckf_ == 0)
{ {
@@ -852,13 +852,13 @@ Transform OdometryMSCKF::computeTransform(
imageProcessor_ = new ImageProcessorNoROS( imageProcessor_ = new ImageProcessorNoROS(
parameters_, parameters_,
lastImu_.localTransform(), lastImu_.localTransform(),
data.stereoCameraModel(), data.stereoCameraModels()[0],
this->imagesAlreadyRectified()); this->imagesAlreadyRectified());
UINFO("Creating MsckfVioNoROS..."); UINFO("Creating MsckfVioNoROS...");
msckf_ = new MsckfVioNoROS( msckf_ = new MsckfVioNoROS(
parameters_, parameters_,
lastImu_.localTransform(), lastImu_.localTransform(),
data.stereoCameraModel(), data.stereoCameraModels()[0],
this->imagesAlreadyRectified()); this->imagesAlreadyRectified());
} }
@@ -975,10 +975,10 @@ Transform OdometryMSCKF::computeTransform(
if(this->imagesAlreadyRectified()) if(this->imagesAlreadyRectified())
{ {
info->newCorners.resize(measurements->features.size()); info->newCorners.resize(measurements->features.size());
float fx = data.stereoCameraModel().left().fx(); float fx = data.stereoCameraModels()[0].left().fx();
float fy = data.stereoCameraModel().left().fy(); float fy = data.stereoCameraModels()[0].left().fy();
float cx = data.stereoCameraModel().left().cx(); float cx = data.stereoCameraModels()[0].left().cx();
float cy = data.stereoCameraModel().left().cy(); float cy = data.stereoCameraModels()[0].left().cy();
info->reg.inliersIDs.resize(measurements->features.size()); info->reg.inliersIDs.resize(measurements->features.size());
for(unsigned int i=0; i<measurements->features.size(); ++i) 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()); UDEBUG("Image update stamp=%f", data.stamp());
std::vector<cv::Mat> images; std::vector<cv::Mat> images;
std::vector<CameraModel> models; 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.imageRaw());
images.push_back(data.rightRaw()); images.push_back(data.rightRaw());
CameraModel mleft = data.stereoCameraModel().left(); CameraModel mleft = data.stereoCameraModels()[0].left();
// should be transform between IMU and camera // should be transform between IMU and camera
mleft.setLocalTransform(lastImu_.localTransform().inverse()*mleft.localTransform()); mleft.setLocalTransform(lastImu_.localTransform().inverse()*mleft.localTransform());
models.push_back(mleft); models.push_back(mleft);
CameraModel mright = data.stereoCameraModel().right(); CameraModel mright = data.stereoCameraModels()[0].right();
// To support not rectified images // To support not rectified images
if(!imagesAlreadyRectified()) if(!imagesAlreadyRectified())
{ {
cv::Mat R = data.stereoCameraModel().R(); cv::Mat R = data.stereoCameraModels()[0].R();
cv::Mat T = data.stereoCameraModel().T(); cv::Mat T = data.stereoCameraModels()[0].T();
UASSERT(R.cols==3 && R.rows == 3); UASSERT(R.cols==3 && R.rows == 3);
UASSERT(T.cols==1 && T.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), 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 else
{ {
Transform extrinsics(1, 0, 0, 0, Transform extrinsics(1, 0, 0, 0,
0, 1, 0, data.stereoCameraModel().baseline(), 0, 1, 0, data.stereoCameraModels()[0].baseline(),
0, 0, 1, 0); 0, 0, 1, 0);
mright.setLocalTransform(extrinsics * mleft.localTransform()); 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()) if(USE_IMU==1 && lastImu_.localTransform().isNull())
{ {
@@ -379,7 +379,7 @@ Transform OdometryVINS::computeTransform(
// intialize // intialize
vinsEstimator_ = new VinsEstimator( vinsEstimator_ = new VinsEstimator(
lastImu_.localTransform().isNull()?Transform::getIdentity():lastImu_.localTransform(), lastImu_.localTransform().isNull()?Transform::getIdentity():lastImu_.localTransform(),
data.stereoCameraModel(), data.stereoCameraModels()[0],
this->imagesAlreadyRectified()); this->imagesAlreadyRectified());
} }
@@ -479,7 +479,7 @@ Transform OdometryVINS::computeTransform(
if(this->imagesAlreadyRectified()) if(this->imagesAlreadyRectified())
{ {
cv::Point2f pt; 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->reg.inliersIDs.push_back(info->newCorners.size());
info->newCorners.push_back(pt); info->newCorners.push_back(pt);
} }
@@ -503,6 +503,10 @@ Transform OdometryVINS::computeTransform(
{ {
UERROR("VINS-Fusion requires stereo images!"); UERROR("VINS-Fusion requires stereo images!");
} }
else
{
UERROR("VINS-Fusion requires stereo images (and only one stereo camera with valid calibration)!");
}
#else #else
UERROR("RTAB-Map is not built with VINS support! Select another visual odometry approach."); UERROR("RTAB-Map is not built with VINS support! Select another visual odometry approach.");
#endif #endif

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

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