From c880ab912cbf69cb0d99105ee8370207611e1342 Mon Sep 17 00:00:00 2001 From: Borong Yuan Date: Sun, 6 Aug 2023 15:45:19 +0800 Subject: [PATCH] fix openvins build --- .../rtabmap/core/odometry/OdometryOpenVINS.h | 3 +- corelib/src/odometry/OdometryOpenVINS.cpp | 144 ++++++++---------- 2 files changed, 67 insertions(+), 80 deletions(-) diff --git a/corelib/include/rtabmap/core/odometry/OdometryOpenVINS.h b/corelib/include/rtabmap/core/odometry/OdometryOpenVINS.h index ed0aa2de2c..6f5dbd2fc1 100644 --- a/corelib/include/rtabmap/core/odometry/OdometryOpenVINS.h +++ b/corelib/include/rtabmap/core/odometry/OdometryOpenVINS.h @@ -40,7 +40,6 @@ class RTABMAP_CORE_EXPORT OdometryOpenVINS : public Odometry { public: OdometryOpenVINS(const rtabmap::ParametersMap & parameters = rtabmap::ParametersMap()); - virtual ~OdometryOpenVINS(); virtual void reset(const Transform & initialPose = Transform::getIdentity()); virtual Odometry::Type getType() {return Odometry::kTypeOpenVINS;} @@ -52,7 +51,7 @@ class RTABMAP_CORE_EXPORT OdometryOpenVINS : public Odometry private: #ifdef RTABMAP_OPENVINS - ov_msckf::VioManager * vioManager_; + std::unique_ptr vioManager_; bool initGravity_; Transform previousPose_; Transform previousLocalTransform_; diff --git a/corelib/src/odometry/OdometryOpenVINS.cpp b/corelib/src/odometry/OdometryOpenVINS.cpp index fda60d38d5..25d7a96afe 100644 --- a/corelib/src/odometry/OdometryOpenVINS.cpp +++ b/corelib/src/odometry/OdometryOpenVINS.cpp @@ -30,20 +30,12 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap/core/util3d_transforms.h" #include "rtabmap/utilite/ULogger.h" #include "rtabmap/utilite/UTimer.h" -#include "rtabmap/utilite/UStl.h" -#include "rtabmap/utilite/UThread.h" -#include "rtabmap/utilite/UDirectory.h" #include #ifdef RTABMAP_OPENVINS #include "core/VioManager.h" -#include "core/VioManagerOptions.h" -#include "core/RosVisualizer.h" -#include "utils/dataset_reader.h" -#include "utils/parse_ros.h" -#include "utils/sensor_data.h" #include "state/State.h" -#include "types/Type.h" +#include "state/StateHelper.h" #endif namespace rtabmap { @@ -52,28 +44,19 @@ OdometryOpenVINS::OdometryOpenVINS(const ParametersMap & parameters) : Odometry(parameters) #ifdef RTABMAP_OPENVINS , - vioManager_(0), initGravity_(false), previousPose_(Transform::getIdentity()) #endif { } -OdometryOpenVINS::~OdometryOpenVINS() -{ -#ifdef RTABMAP_OPENVINS - delete vioManager_; -#endif -} - void OdometryOpenVINS::reset(const Transform & initialPose) { Odometry::reset(initialPose); #ifdef RTABMAP_OPENVINS if(!initGravity_) { - delete vioManager_; - vioManager_ = 0; + vioManager_.reset(); previousPose_.setIdentity(); previousLocalTransform_.setNull(); imuBuffer_.clear(); @@ -94,9 +77,7 @@ Transform OdometryOpenVINS::computeTransform( // Buffer imus; if(!data.imu().empty()) - { imuBuffer_.insert(std::make_pair(data.stamp(), data.imu())); - } // OpenVINS has to buffer image before computing transformation with IMU stamp > image stamp if(!data.imageRaw().empty() && !data.rightRaw().empty() && data.stereoCameraModels().size() == 1) @@ -106,7 +87,7 @@ Transform OdometryOpenVINS::computeTransform( UWARN("Waiting IMU for initialization..."); return t; } - if(vioManager_ == 0) + if(!vioManager_) { UINFO("OpenVINS Initialization"); @@ -127,16 +108,16 @@ Transform OdometryOpenVINS::computeTransform( //params.state_options.max_slam_in_update = INT_MAX; //params.state_options.max_msckf_in_update = INT_MAX; //params.state_options.max_aruco_features = 1024; - params.state_options.num_cameras = 2; + params.state_options.num_cameras = params.init_options.num_cameras = 2; //params.dt_slam_delay = 2; // Set what representation we should be using //params.state_options.feat_rep_msckf = LandmarkRepresentation::from_string("ANCHORED_MSCKF_INVERSE_DEPTH"); // default GLOBAL_3D //params.state_options.feat_rep_slam = LandmarkRepresentation::from_string("ANCHORED_MSCKF_INVERSE_DEPTH"); // default GLOBAL_3D //params.state_options.feat_rep_aruco = LandmarkRepresentation::from_string("ANCHORED_MSCKF_INVERSE_DEPTH"); // default GLOBAL_3D - if( params.state_options.feat_rep_msckf == LandmarkRepresentation::Representation::UNKNOWN || - params.state_options.feat_rep_slam == LandmarkRepresentation::Representation::UNKNOWN || - params.state_options.feat_rep_aruco == LandmarkRepresentation::Representation::UNKNOWN) + if( params.state_options.feat_rep_msckf == ov_type::LandmarkRepresentation::Representation::UNKNOWN || + params.state_options.feat_rep_slam == ov_type::LandmarkRepresentation::Representation::UNKNOWN || + params.state_options.feat_rep_aruco == ov_type::LandmarkRepresentation::Representation::UNKNOWN) { printf(RED "VioManager(): invalid feature representation specified:\n" RESET); printf(RED "\t- GLOBAL_3D\n" RESET); @@ -187,7 +168,7 @@ Transform OdometryOpenVINS::computeTransform( // TRACKERS ====================================================================== // Tracking flags - params.use_stereo = true; + params.use_stereo = params.init_options.use_stereo = true; //params.use_klt = true; params.use_aruco = false; //params.downsize_aruco = true; @@ -219,20 +200,40 @@ Transform OdometryOpenVINS::computeTransform( // CAMERA ====================================================================== 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(1, fisheye)); + if (fisheye) + { + params.camera_intrinsics.insert({0, std::make_shared( + data.stereoCameraModels()[0].left().imageWidth(), data.stereoCameraModels()[0].left().imageHeight())}); + params.camera_intrinsics.insert({1, std::make_shared( + data.stereoCameraModels()[0].right().imageWidth(), data.stereoCameraModels()[0].right().imageHeight())}); + params.init_options.camera_intrinsics.insert({0, std::make_shared( + data.stereoCameraModels()[0].left().imageWidth(), data.stereoCameraModels()[0].left().imageHeight())}); + params.init_options.camera_intrinsics.insert({1, std::make_shared( + data.stereoCameraModels()[0].right().imageWidth(), data.stereoCameraModels()[0].right().imageHeight())}); + } + else + { + params.camera_intrinsics.insert({0, std::make_shared( + data.stereoCameraModels()[0].left().imageWidth(), data.stereoCameraModels()[0].left().imageHeight())}); + params.camera_intrinsics.insert({1, std::make_shared( + data.stereoCameraModels()[0].right().imageWidth(), data.stereoCameraModels()[0].right().imageHeight())}); + params.init_options.camera_intrinsics.insert({0, std::make_shared( + data.stereoCameraModels()[0].left().imageWidth(), data.stereoCameraModels()[0].left().imageHeight())}); + params.init_options.camera_intrinsics.insert({1, std::make_shared( + data.stereoCameraModels()[0].right().imageWidth(), data.stereoCameraModels()[0].right().imageHeight())}); + } Eigen::VectorXd camLeft(8), camRight(8); - if(this->imagesAlreadyRectified() || data.stereoCameraModels()[0].left().D_raw().empty()) + if (this->imagesAlreadyRectified() || data.stereoCameraModels()[0].left().D_raw().empty()) { - camLeft << data.stereoCameraModels()[0].left().fx(), - data.stereoCameraModels()[0].left().fy(), - data.stereoCameraModels()[0].left().cx(), - data.stereoCameraModels()[0].left().cy(), 0, 0, 0, 0; + camLeft << data.stereoCameraModels()[0].left().fx(), + data.stereoCameraModels()[0].left().fy(), + data.stereoCameraModels()[0].left().cx(), + data.stereoCameraModels()[0].left().cy(), 0, 0, 0, 0; camRight << data.stereoCameraModels()[0].right().fx(), - data.stereoCameraModels()[0].right().fy(), - data.stereoCameraModels()[0].right().cx(), - data.stereoCameraModels()[0].right().cy(), 0, 0, 0, 0; + data.stereoCameraModels()[0].right().fy(), + data.stereoCameraModels()[0].right().cx(), + data.stereoCameraModels()[0].right().cy(), 0, 0, 0, 0; } else { @@ -246,27 +247,27 @@ Transform OdometryOpenVINS::computeTransform( /// equidistant (equi) // (distortion_coeffs: [k1 k2 k3 k4]) rtabmap: (k1,k2,p1,p2,k3,k4) - camLeft << - data.stereoCameraModels()[0].left().K_raw().at(0,0), - data.stereoCameraModels()[0].left().K_raw().at(1,1), - data.stereoCameraModels()[0].left().K_raw().at(0,2), - data.stereoCameraModels()[0].left().K_raw().at(1,2), - data.stereoCameraModels()[0].left().D_raw().at(0,0), - data.stereoCameraModels()[0].left().D_raw().at(0,1), - data.stereoCameraModels()[0].left().D_raw().at(0,fisheye?4:2), - data.stereoCameraModels()[0].left().D_raw().at(0,fisheye?5:3); - camRight << - data.stereoCameraModels()[0].right().K_raw().at(0,0), - data.stereoCameraModels()[0].right().K_raw().at(1,1), - data.stereoCameraModels()[0].right().K_raw().at(0,2), - data.stereoCameraModels()[0].right().K_raw().at(1,2), - data.stereoCameraModels()[0].right().D_raw().at(0,0), - data.stereoCameraModels()[0].right().D_raw().at(0,1), - data.stereoCameraModels()[0].right().D_raw().at(0,fisheye?4:2), - data.stereoCameraModels()[0].right().D_raw().at(0,fisheye?5:3); + camLeft << data.stereoCameraModels()[0].left().K_raw().at(0,0), + data.stereoCameraModels()[0].left().K_raw().at(1,1), + data.stereoCameraModels()[0].left().K_raw().at(0,2), + data.stereoCameraModels()[0].left().K_raw().at(1,2), + data.stereoCameraModels()[0].left().D_raw().at(0,0), + data.stereoCameraModels()[0].left().D_raw().at(0,1), + data.stereoCameraModels()[0].left().D_raw().at(0,fisheye?4:2), + data.stereoCameraModels()[0].left().D_raw().at(0,fisheye?5:3); + camRight << data.stereoCameraModels()[0].right().K_raw().at(0,0), + data.stereoCameraModels()[0].right().K_raw().at(1,1), + data.stereoCameraModels()[0].right().K_raw().at(0,2), + data.stereoCameraModels()[0].right().K_raw().at(1,2), + data.stereoCameraModels()[0].right().D_raw().at(0,0), + data.stereoCameraModels()[0].right().D_raw().at(0,1), + data.stereoCameraModels()[0].right().D_raw().at(0,fisheye?4:2), + data.stereoCameraModels()[0].right().D_raw().at(0,fisheye?5:3); } - params.camera_intrinsics.insert(std::make_pair(0, camLeft)); - params.camera_intrinsics.insert(std::make_pair(1, camRight)); + params.camera_intrinsics.at(0)->set_value(camLeft); + params.camera_intrinsics.at(1)->set_value(camRight); + params.init_options.camera_intrinsics.at(0)->set_value(camLeft); + params.init_options.camera_intrinsics.at(1)->set_value(camRight); const IMU & imu = imuBuffer_.begin()->second; imuLocalTransform_ = imu.localTransform(); @@ -288,46 +289,33 @@ Transform OdometryOpenVINS::computeTransform( Eigen::Matrix4d cam0_eigen = imuCam0.toEigen4d(); Eigen::Matrix4d cam1_eigen = imuCam1.toEigen4d(); Eigen::Matrix cam_eigen0; - cam_eigen0.block(0,0,4,1) = rot_2_quat(cam0_eigen.block(0,0,3,3).transpose()); + cam_eigen0.block(0,0,4,1) = ov_core::rot_2_quat(cam0_eigen.block(0,0,3,3).transpose()); cam_eigen0.block(4,0,3,1) = -cam0_eigen.block(0,0,3,3).transpose()*cam0_eigen.block(0,3,3,1); Eigen::Matrix cam_eigen1; - cam_eigen1.block(0,0,4,1) = rot_2_quat(cam1_eigen.block(0,0,3,3).transpose()); + cam_eigen1.block(0,0,4,1) = ov_core::rot_2_quat(cam1_eigen.block(0,0,3,3).transpose()); cam_eigen1.block(4,0,3,1) = -cam1_eigen.block(0,0,3,3).transpose()*cam1_eigen.block(0,3,3,1); params.camera_extrinsics.insert(std::make_pair(0, cam_eigen0)); params.camera_extrinsics.insert(std::make_pair(1, cam_eigen1)); + params.init_options.camera_extrinsics.insert(std::make_pair(0, cam_eigen0)); + params.init_options.camera_extrinsics.insert(std::make_pair(1, cam_eigen1)); - 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.stereoCameraModels()[0].right().imageWidth(),data.stereoCameraModels()[0].right().imageHeight())}); - - vioManager_ = new ov_msckf::VioManager(params); + vioManager_ = std::make_unique(params); } - cv::Mat left; - cv::Mat right; + cv::Mat left, right; if(data.imageRaw().type() == CV_8UC3) - { cv::cvtColor(data.imageRaw(), left, CV_BGR2GRAY); - } else if(data.imageRaw().type() == CV_8UC1) - { left = data.imageRaw().clone(); - } else - { UFATAL("Not supported color type!"); - } + if(data.rightRaw().type() == CV_8UC3) - { cv::cvtColor(data.rightRaw(), right, CV_BGR2GRAY); - } else if(data.rightRaw().type() == CV_8UC1) - { right = data.rightRaw().clone(); - } else - { UFATAL("Not supported color type!"); - } // Create the measurement ov_core::CameraData message; @@ -406,7 +394,7 @@ Transform OdometryOpenVINS::computeTransform( } else { - Eigen::Matrix covariance_posori = ov_msckf::StateHelper::get_marginal_covariance(vioManager_->get_state(),statevars); + Eigen::Matrix covariance_posori = ov_msckf::StateHelper::get_marginal_covariance(vioManager_->get_state(), statevars); for(int r=0; r<6; r++) { for(int c=0; c<6; c++) { ((double *)covariance.data)[6*r+c] = covariance_posori(r,c);