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 OpenVINS odometry

Browse Source
This commit is contained in:
matlabbe
2022-09-27 13:48:57 -07:00
parent 675dad5f76
commit 74c6e9cfec
1 changed files with 42 additions and 38 deletions
Download Patch File Download Diff File Expand all files Collapse all files

80
corelib/src/odometry/OdometryOpenVINS.cpp
View File

@@ -99,7 +99,7 @@ Transform OdometryOpenVINS::computeTransform(
} }
// OpenVINS has to buffer image before computing transformation with IMU stamp > image stamp // OpenVINS has to buffer image before computing transformation with IMU stamp > image stamp
if(!data.imageRaw().empty() && !data.rightRaw().empty()) if(!data.imageRaw().empty() && !data.rightRaw().empty() && data.stereoCameraModels().size() == 1)
{ {
if(imuBuffer_.empty()) if(imuBuffer_.empty())
{ {
@@ -218,27 +218,27 @@ Transform OdometryOpenVINS::computeTransform(
// CAMERA ====================================================================== // CAMERA ======================================================================
bool fisheye = data.stereoCameraModel().left().isFisheye() && !this->imagesAlreadyRectified(); bool fisheye = data.stereoCameraModels()[0].left().isFisheye() && !this->imagesAlreadyRectified();
params.camera_fisheye.insert(std::make_pair(0, fisheye)); params.camera_fisheye.insert(std::make_pair(0, fisheye));
params.camera_fisheye.insert(std::make_pair(1, fisheye)); params.camera_fisheye.insert(std::make_pair(1, fisheye));
Eigen::VectorXd camLeft(8), camRight(8); Eigen::VectorXd camLeft(8), camRight(8);
if(this->imagesAlreadyRectified() || data.stereoCameraModel().left().D_raw().empty()) if(this->imagesAlreadyRectified() || data.stereoCameraModels()[0].left().D_raw().empty())
{ {
camLeft << data.stereoCameraModel().left().fx(), camLeft << data.stereoCameraModels()[0].left().fx(),
data.stereoCameraModel().left().fy(), data.stereoCameraModels()[0].left().fy(),
data.stereoCameraModel().left().cx(), data.stereoCameraModels()[0].left().cx(),
data.stereoCameraModel().left().cy(), 0, 0, 0, 0; data.stereoCameraModels()[0].left().cy(), 0, 0, 0, 0;
camRight << data.stereoCameraModel().right().fx(), camRight << data.stereoCameraModels()[0].right().fx(),
data.stereoCameraModel().right().fy(), data.stereoCameraModels()[0].right().fy(),
data.stereoCameraModel().right().cx(), data.stereoCameraModels()[0].right().cx(),
data.stereoCameraModel().right().cy(), 0, 0, 0, 0; data.stereoCameraModels()[0].right().cy(), 0, 0, 0, 0;
} }
else else
{ {
UASSERT(data.stereoCameraModel().left().D_raw().cols == data.stereoCameraModel().right().D_raw().cols); UASSERT(data.stereoCameraModels()[0].left().D_raw().cols == data.stereoCameraModels()[0].right().D_raw().cols);
UASSERT(data.stereoCameraModel().left().D_raw().cols >= 4); UASSERT(data.stereoCameraModels()[0].left().D_raw().cols >= 4);
UASSERT(data.stereoCameraModel().right().D_raw().cols >= 4); UASSERT(data.stereoCameraModels()[0].right().D_raw().cols >= 4);
//https://github.com/ethz-asl/kalibr/wiki/supported-models //https://github.com/ethz-asl/kalibr/wiki/supported-models
/// radial-tangential (radtan) /// radial-tangential (radtan)
@@ -247,41 +247,41 @@ Transform OdometryOpenVINS::computeTransform(
// (distortion_coeffs: [k1 k2 k3 k4]) rtabmap: (k1,k2,p1,p2,k3,k4) // (distortion_coeffs: [k1 k2 k3 k4]) rtabmap: (k1,k2,p1,p2,k3,k4)
camLeft << camLeft <<
data.stereoCameraModel().left().K_raw().at<double>(0,0), data.stereoCameraModels()[0].left().K_raw().at<double>(0,0),
data.stereoCameraModel().left().K_raw().at<double>(1,1), data.stereoCameraModels()[0].left().K_raw().at<double>(1,1),
data.stereoCameraModel().left().K_raw().at<double>(0,2), data.stereoCameraModels()[0].left().K_raw().at<double>(0,2),
data.stereoCameraModel().left().K_raw().at<double>(1,2), data.stereoCameraModels()[0].left().K_raw().at<double>(1,2),
data.stereoCameraModel().left().D_raw().at<double>(0,0), data.stereoCameraModels()[0].left().D_raw().at<double>(0,0),
data.stereoCameraModel().left().D_raw().at<double>(0,1), data.stereoCameraModels()[0].left().D_raw().at<double>(0,1),
data.stereoCameraModel().left().D_raw().at<double>(0,fisheye?4:2), data.stereoCameraModels()[0].left().D_raw().at<double>(0,fisheye?4:2),
data.stereoCameraModel().left().D_raw().at<double>(0,fisheye?5:3); data.stereoCameraModels()[0].left().D_raw().at<double>(0,fisheye?5:3);
camRight << camRight <<
data.stereoCameraModel().right().K_raw().at<double>(0,0), data.stereoCameraModels()[0].right().K_raw().at<double>(0,0),
data.stereoCameraModel().right().K_raw().at<double>(1,1), data.stereoCameraModels()[0].right().K_raw().at<double>(1,1),
data.stereoCameraModel().right().K_raw().at<double>(0,2), data.stereoCameraModels()[0].right().K_raw().at<double>(0,2),
data.stereoCameraModel().right().K_raw().at<double>(1,2), data.stereoCameraModels()[0].right().K_raw().at<double>(1,2),
data.stereoCameraModel().right().D_raw().at<double>(0,0), data.stereoCameraModels()[0].right().D_raw().at<double>(0,0),
data.stereoCameraModel().right().D_raw().at<double>(0,1), data.stereoCameraModels()[0].right().D_raw().at<double>(0,1),
data.stereoCameraModel().right().D_raw().at<double>(0,fisheye?4:2), data.stereoCameraModels()[0].right().D_raw().at<double>(0,fisheye?4:2),
data.stereoCameraModel().right().D_raw().at<double>(0,fisheye?5:3); data.stereoCameraModels()[0].right().D_raw().at<double>(0,fisheye?5:3);
} }
params.camera_intrinsics.insert(std::make_pair(0, camLeft)); params.camera_intrinsics.insert(std::make_pair(0, camLeft));
params.camera_intrinsics.insert(std::make_pair(1, camRight)); params.camera_intrinsics.insert(std::make_pair(1, camRight));
const IMU & imu = imuBuffer_.begin()->second; const IMU & imu = imuBuffer_.begin()->second;
imuLocalTransform_ = imu.localTransform(); imuLocalTransform_ = imu.localTransform();
Transform imuCam0 = imuLocalTransform_.inverse() * data.stereoCameraModel().localTransform(); Transform imuCam0 = imuLocalTransform_.inverse() * data.stereoCameraModels()[0].localTransform();
Transform cam0cam1; Transform cam0cam1;
if(this->imagesAlreadyRectified() || data.stereoCameraModel().stereoTransform().isNull()) if(this->imagesAlreadyRectified() || data.stereoCameraModels()[0].stereoTransform().isNull())
{ {
cam0cam1 = Transform( cam0cam1 = Transform(
1, 0, 0, data.stereoCameraModel().baseline(), 1, 0, 0, data.stereoCameraModels()[0].baseline(),
0, 1, 0, 0, 0, 1, 0, 0,
0, 0, 1, 0); 0, 0, 1, 0);
} }
else else
{ {
cam0cam1 = data.stereoCameraModel().stereoTransform().inverse(); cam0cam1 = data.stereoCameraModels()[0].stereoTransform().inverse();
} }
UASSERT(!cam0cam1.isNull()); UASSERT(!cam0cam1.isNull());
Transform imuCam1 = imuCam0 * cam0cam1; Transform imuCam1 = imuCam0 * cam0cam1;
@@ -296,8 +296,8 @@ Transform OdometryOpenVINS::computeTransform(
params.camera_extrinsics.insert(std::make_pair(0, cam_eigen0)); params.camera_extrinsics.insert(std::make_pair(0, cam_eigen0));
params.camera_extrinsics.insert(std::make_pair(1, cam_eigen1)); params.camera_extrinsics.insert(std::make_pair(1, cam_eigen1));
params.camera_wh.insert({0, std::make_pair(data.stereoCameraModel().left().imageWidth(),data.stereoCameraModel().left().imageHeight())}); params.camera_wh.insert({0, std::make_pair(data.stereoCameraModels()[0].left().imageWidth(),data.stereoCameraModels()[0].left().imageHeight())});
params.camera_wh.insert({1, std::make_pair(data.stereoCameraModel().right().imageWidth(),data.stereoCameraModel().right().imageHeight())}); params.camera_wh.insert({1, std::make_pair(data.stereoCameraModels()[0].right().imageWidth(),data.stereoCameraModels()[0].right().imageHeight())});
vioManager_ = new ov_msckf::VioManager(params); vioManager_ = new ov_msckf::VioManager(params);
} }
@@ -441,7 +441,7 @@ Transform OdometryOpenVINS::computeTransform(
// feature map // feature map
Transform fixT = this->getPose()*previousPoseInv; Transform fixT = this->getPose()*previousPoseInv;
Transform camLocalTransformInv = data.stereoCameraModel().localTransform().inverse()*this->getPose().inverse(); Transform camLocalTransformInv = data.stereoCameraModels()[0].localTransform().inverse()*this->getPose().inverse();
for (auto &it_per_id : vioManager_->get_features_SLAM()) for (auto &it_per_id : vioManager_->get_features_SLAM())
{ {
cv::Point3f pt3d; cv::Point3f pt3d;
@@ -455,7 +455,7 @@ Transform OdometryOpenVINS::computeTransform(
{ {
cv::Point2f pt; cv::Point2f pt;
pt3d = util3d::transformPoint(pt3d, camLocalTransformInv); pt3d = util3d::transformPoint(pt3d, camLocalTransformInv);
data.stereoCameraModel().left().reproject(pt3d.x, pt3d.y, pt3d.z, pt.x, pt.y); data.stereoCameraModels()[0].left().reproject(pt3d.x, pt3d.y, pt3d.z, 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);
} }
@@ -475,6 +475,10 @@ Transform OdometryOpenVINS::computeTransform(
{ {
UERROR("OpenVINS requires stereo images!"); UERROR("OpenVINS requires stereo images!");
} }
else
{
UERROR("OpenVINS requires stereo images (only one stereo camera and should be calibrated)!");
}
#else #else
UERROR("RTAB-Map is not built with OpenVINS support! Select another visual odometry approach."); UERROR("RTAB-Map is not built with OpenVINS support! Select another visual odometry approach.");