Skip to content

Commit

Permalink
Fixed compilation error about stereoCameraModel() not found (#905) wi…
Browse files Browse the repository at this point in the history 
…th odometry approaches: Fovis, MSCKF, Okvis, VINS, Viso2.
  • Loading branch information
@matlabbe
matlabbe committed Sep 27, 2022
1 parent 5554613 commit 675dad5
Show file tree
Hide file tree
Showing 6 changed files with 50 additions and 51 deletions.
6 changes: 3 additions & 3 deletions CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -737,8 +737,8 @@ IF(WITH_ORB_SLAM AND NOT G2O_FOUND)
ENDIF(WITH_ORB_SLAM AND NOT G2O_FOUND)

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)
#LOAM, PCL>=1.10, latest g2o and CCCoreLib require c++14
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, but MSCKF_VIO requires c++11
include(CheckCXXCompilerFlag)
CHECK_CXX_COMPILER_FLAG("-std=c++14" COMPILER_SUPPORTS_CXX14)
IF(COMPILER_SUPPORTS_CXX14)
Expand All @@ -747,7 +747,7 @@ IF(NOT MSVC)
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.")
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 (
G2O_FOUND OR
Expand Down
44 changes: 20 additions & 24 deletions corelib/src/odometry/OdometryFovis.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}

Expand Down Expand Up @@ -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)
{
Expand All @@ -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
Expand All @@ -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;

Expand Down
14 changes: 7 additions & 7 deletions corelib/src/odometry/OdometryMSCKF.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)
{
Expand All @@ -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());
}

Expand Down Expand Up @@ -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)
{
Expand Down
12 changes: 6 additions & 6 deletions corelib/src/odometry/OdometryOkvis.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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),
Expand All @@ -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());
}
Expand Down
10 changes: 7 additions & 3 deletions corelib/src/odometry/OdometryVINS.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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())
{
Expand All @@ -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());
}

Expand Down Expand Up @@ -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);
}
Expand All @@ -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
Expand Down
15 changes: 7 additions & 8 deletions corelib/src/odometry/OdometryViso2.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -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
Expand Down

0 comments on commit 675dad5

Please sign in to comment.