Skip to content

Commit

Permalink
fix openvins build
Browse files Browse the repository at this point in the history
  • Loading branch information
@borongyuan
borongyuan committed Aug 6, 2023
1 parent e2f0371 commit 7800282
Show file tree
Hide file tree
Showing 2 changed files with 67 additions and 80 deletions.
3 changes: 1 addition & 2 deletions corelib/include/rtabmap/core/odometry/OdometryOpenVINS.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;}
Expand All @@ -52,7 +51,7 @@ class RTABMAP_CORE_EXPORT OdometryOpenVINS : public Odometry

private:
#ifdef RTABMAP_OPENVINS
ov_msckf::VioManager * vioManager_;
std::unique_ptr<ov_msckf::VioManager> vioManager_;
bool initGravity_;
Transform previousPose_;
Transform previousLocalTransform_;
Expand Down
144 changes: 66 additions & 78 deletions corelib/src/odometry/OdometryOpenVINS.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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 <opencv2/imgproc/types_c.h>

#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 {
Expand All @@ -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();
Expand All @@ -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)
Expand All @@ -106,7 +87,7 @@ Transform OdometryOpenVINS::computeTransform(
UWARN("Waiting IMU for initialization...");
return t;
}
if(vioManager_ == 0)
if(!vioManager_)
{
UINFO("OpenVINS Initialization");

Expand All @@ -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);
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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<ov_core::CamEqui>(
data.stereoCameraModels()[0].left().imageWidth(), data.stereoCameraModels()[0].left().imageHeight())});
params.camera_intrinsics.insert({1, std::make_shared<ov_core::CamEqui>(
data.stereoCameraModels()[0].right().imageWidth(), data.stereoCameraModels()[0].right().imageHeight())});
params.init_options.camera_intrinsics.insert({0, std::make_shared<ov_core::CamEqui>(
data.stereoCameraModels()[0].left().imageWidth(), data.stereoCameraModels()[0].left().imageHeight())});
params.init_options.camera_intrinsics.insert({1, std::make_shared<ov_core::CamEqui>(
data.stereoCameraModels()[0].right().imageWidth(), data.stereoCameraModels()[0].right().imageHeight())});
}
else
{
params.camera_intrinsics.insert({0, std::make_shared<ov_core::CamRadtan>(
data.stereoCameraModels()[0].left().imageWidth(), data.stereoCameraModels()[0].left().imageHeight())});
params.camera_intrinsics.insert({1, std::make_shared<ov_core::CamRadtan>(
data.stereoCameraModels()[0].right().imageWidth(), data.stereoCameraModels()[0].right().imageHeight())});
params.init_options.camera_intrinsics.insert({0, std::make_shared<ov_core::CamRadtan>(
data.stereoCameraModels()[0].left().imageWidth(), data.stereoCameraModels()[0].left().imageHeight())});
params.init_options.camera_intrinsics.insert({1, std::make_shared<ov_core::CamRadtan>(
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
{
Expand All @@ -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<double>(0,0),
data.stereoCameraModels()[0].left().K_raw().at<double>(1,1),
data.stereoCameraModels()[0].left().K_raw().at<double>(0,2),
data.stereoCameraModels()[0].left().K_raw().at<double>(1,2),
data.stereoCameraModels()[0].left().D_raw().at<double>(0,0),
data.stereoCameraModels()[0].left().D_raw().at<double>(0,1),
data.stereoCameraModels()[0].left().D_raw().at<double>(0,fisheye?4:2),
data.stereoCameraModels()[0].left().D_raw().at<double>(0,fisheye?5:3);
camRight <<
data.stereoCameraModels()[0].right().K_raw().at<double>(0,0),
data.stereoCameraModels()[0].right().K_raw().at<double>(1,1),
data.stereoCameraModels()[0].right().K_raw().at<double>(0,2),
data.stereoCameraModels()[0].right().K_raw().at<double>(1,2),
data.stereoCameraModels()[0].right().D_raw().at<double>(0,0),
data.stereoCameraModels()[0].right().D_raw().at<double>(0,1),
data.stereoCameraModels()[0].right().D_raw().at<double>(0,fisheye?4:2),
data.stereoCameraModels()[0].right().D_raw().at<double>(0,fisheye?5:3);
camLeft << data.stereoCameraModels()[0].left().K_raw().at<double>(0,0),
data.stereoCameraModels()[0].left().K_raw().at<double>(1,1),
data.stereoCameraModels()[0].left().K_raw().at<double>(0,2),
data.stereoCameraModels()[0].left().K_raw().at<double>(1,2),
data.stereoCameraModels()[0].left().D_raw().at<double>(0,0),
data.stereoCameraModels()[0].left().D_raw().at<double>(0,1),
data.stereoCameraModels()[0].left().D_raw().at<double>(0,fisheye?4:2),
data.stereoCameraModels()[0].left().D_raw().at<double>(0,fisheye?5:3);
camRight << data.stereoCameraModels()[0].right().K_raw().at<double>(0,0),
data.stereoCameraModels()[0].right().K_raw().at<double>(1,1),
data.stereoCameraModels()[0].right().K_raw().at<double>(0,2),
data.stereoCameraModels()[0].right().K_raw().at<double>(1,2),
data.stereoCameraModels()[0].right().D_raw().at<double>(0,0),
data.stereoCameraModels()[0].right().D_raw().at<double>(0,1),
data.stereoCameraModels()[0].right().D_raw().at<double>(0,fisheye?4:2),
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(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();
Expand All @@ -288,46 +289,33 @@ Transform OdometryOpenVINS::computeTransform(
Eigen::Matrix4d cam0_eigen = imuCam0.toEigen4d();
Eigen::Matrix4d cam1_eigen = imuCam1.toEigen4d();
Eigen::Matrix<double,7,1> 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<double,7,1> 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<ov_msckf::VioManager>(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;
Expand Down Expand Up @@ -406,7 +394,7 @@ Transform OdometryOpenVINS::computeTransform(
}
else
{
Eigen::Matrix<double,6,6> covariance_posori = ov_msckf::StateHelper::get_marginal_covariance(vioManager_->get_state(),statevars);
Eigen::Matrix<double,6,6> 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);
Expand Down

0 comments on commit 7800282

Please sign in to comment.