Added OpenVINS minimal support (tested with EuRoC dataset)

CMakeLists.txt

@@ -21,7 +21,7 @@ SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules")
 #######################
 SET(RTABMAP_MAJOR_VERSION 0)
 SET(RTABMAP_MINOR_VERSION 20)
-SET(RTABMAP_PATCH_VERSION 9)
+SET(RTABMAP_PATCH_VERSION 10)
 SET(RTABMAP_VERSION
   ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION})
   
@@ -201,6 +201,7 @@ option(WITH_ORB_SLAM      "Include ORB_SLAM2 or ORB_SLAM3  support" ON)
 option(WITH_OKVIS         "Include OKVIS support"                ON)
 option(WITH_MSCKF_VIO     "Include MSCKF_VIO support"           OFF)
 option(WITH_VINS          "Include VINS-Fusion support"          ON)
+option(WITH_OPENVINS      "Include OpenVINS support"             ON)
 option(WITH_MADGWICK      "Include Madgwick IMU filtering support" ON)
 option(WITH_FASTCV        "Include FastCV support"               ON)
 IF(ANDROID)
@@ -635,6 +636,13 @@ IF(WITH_VINS)
     ENDIF(vins_FOUND)
 ENDIF(WITH_VINS)
 
+IF(WITH_OPENVINS)
+    FIND_PACKAGE(ov_msckf QUIET)
+    IF(ov_msckf_FOUND)
+       MESSAGE(STATUS "Found ov_msckf: ${ov_msckf_INCLUDE_DIRS}")
+    ENDIF(ov_msckf_FOUND)
+ENDIF(WITH_OPENVINS)
+
 IF(WITH_FASTCV)
     FIND_PACKAGE(FastCV QUIET)
     IF(FastCV_FOUND)
@@ -676,6 +684,7 @@ IF(NOT MSVC)
 	       open_chisel_FOUND OR
 	       msckf_vio_FOUND OR
 	       vins_FOUND OR
+	       ov_msckf_FOUND OR
 	       libpointmatcher_FOUND))
 	 #Newest versions require std11
 	 include(CheckCXXCompilerFlag)
@@ -896,6 +905,11 @@ IF(NOT vins_FOUND)
 ELSE()
    SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${vins_LIBRARIES}) 
 ENDIF()
+IF(NOT ov_msckf_FOUND)
+   SET(OPENVINS "//")
+ELSE()
+   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${ov_msckf_LIBRARIES}) 
+ENDIF()
 IF(NOT ORB_SLAM_FOUND)
    SET(ORB_SLAM "//")
 ELSE()
@@ -1478,6 +1492,14 @@ ELSE()
 MESSAGE(STATUS "  With VINS-Fusion          = NO (VINS-Fusion not found)")
 ENDIF()
 
+IF(ov_msckf_FOUND)
+MESSAGE(STATUS "  With OpenVINS             = YES (License: GPLv3)")
+ELSEIF(NOT WITH_OPENVINS)
+MESSAGE(STATUS "  With OpenVINS             = NO (WITH_OPENVINS=OFF)")
+ELSE()
+MESSAGE(STATUS "  With OpenVINS             = NO (ov_msckf not found)")
+ENDIF()
+
 IF(ORB_SLAM_FOUND)
 MESSAGE(STATUS "  With ORB_SLAM${ORB_SLAM_VERSION}            = YES (License: GPLv3)")
 ELSEIF(NOT WITH_ORB_SLAM)

Version.h.in

@@ -74,6 +74,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 @OKVIS@#define RTABMAP_OKVIS
 @MSCKF_VIO@#define RTABMAP_MSCKF_VIO
 @VINS@#define RTABMAP_VINS
+@OPENVINS@#define RTABMAP_OPENVINS
 @ORB_SLAM@#define RTABMAP_ORB_SLAM @ORB_SLAM_VERSION@
 @ORB_OCTREE@#define RTABMAP_ORB_OCTREE
 @TORCH@#define RTABMAP_TORCH

corelib/include/rtabmap/core/CameraModel.h

@@ -124,6 +124,7 @@ public:
 	double fovY() const; // in radians
 	double horizontalFOV() const; // in degrees
 	double verticalFOV() const;   // in degrees
+	bool isFisheye() const {return D_.cols == 6;}
 
 	bool load(const std::string & filePath);
 	bool load(const std::string & directory, const std::string & cameraName);

corelib/include/rtabmap/core/Odometry.h

@@ -53,7 +53,8 @@ public:
 		kTypeOkvis = 6,
 		kTypeLOAM = 7,
 		kTypeMSCKF = 8,
-		kTypeVINS = 9
+		kTypeVINS = 9,
+		kTypeOpenVINS = 10
 	};
 
 public:

corelib/include/rtabmap/core/odometry/OdometryOpenVINS.h

@@ -0,0 +1,66 @@
+/*
+Copyright (c) 2010-2021, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef ODOMETRYOPENVINS_H_
+#define ODOMETRYOPENVINS_H_
+
+#include <rtabmap/core/Odometry.h>
+
+namespace ov_msckf {
+class VioManager;
+}
+
+namespace rtabmap {
+
+class RTABMAP_EXP 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;}
+	virtual bool canProcessRawImages() const {return true;}
+	virtual bool canProcessAsyncIMU() const {return true;}
+
+private:
+	virtual Transform computeTransform(SensorData & image, const Transform & guess = Transform(), OdometryInfo * info = 0);
+
+private:
+#ifdef RTABMAP_OPENVINS
+	ov_msckf::VioManager * vioManager_;
+	bool initGravity_;
+	Transform previousPose_;
+	Transform previousLocalTransform_;
+	Transform imuLocalTransform_;
+	std::map<double, IMU> imuBuffer_;
+#endif
+};
+
+}
+
+#endif /* ODOMETRYOPENVINS_H_ */

corelib/include/rtabmap/core/odometry/OdometryVINS.h

@@ -51,7 +51,6 @@ private:
 private:
 #ifdef RTABMAP_VINS
 	VinsEstimator * vinsEstimator_;
-	int imagesProcessed_;
 	bool initGravity_;
 	Transform previousPose_;
 	Transform previousLocalTransform_;

corelib/src/CMakeLists.txt

@@ -91,6 +91,7 @@ SET(SRC_FILES
     odometry/OdometryLOAM.cpp
     odometry/OdometryMSCKF.cpp
     odometry/OdometryVINS.cpp
+    odometry/OdometryOpenVINS.cpp
     
     IMU.cpp
     IMUThread.cpp
@@ -609,6 +610,17 @@ IF(vins_FOUND)
 	)
 ENDIF(vins_FOUND)
 
+IF(ov_msckf_FOUND)
+    SET(INCLUDE_DIRS
+		${ov_msckf_INCLUDE_DIRS}
+		${INCLUDE_DIRS}
+	)
+	SET(LIBRARIES
+		${ov_msckf_LIBRARIES}
+		${LIBRARIES}
+	)
+ENDIF(ov_msckf_FOUND)
+
 IF(ORB_SLAM_FOUND)
     SET(INCLUDE_DIRS
         ${ORB_SLAM_INCLUDE_DIRS} #before so that g2o includes are taken from ORB_SLAM directory before the official g2o one

corelib/src/Odometry.cpp

@@ -36,6 +36,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/odometry/OdometryLOAM.h"
 #include "rtabmap/core/odometry/OdometryMSCKF.h"
 #include "rtabmap/core/odometry/OdometryVINS.h"
+#include "rtabmap/core/odometry/OdometryOpenVINS.h"
 #include "rtabmap/core/OdometryInfo.h"
 #include "rtabmap/core/util3d.h"
 #include "rtabmap/core/util3d_mapping.h"
@@ -95,6 +96,9 @@ Odometry * Odometry::create(Odometry::Type & type, const ParametersMap & paramet
 	case Odometry::kTypeVINS:
 		odometry = new OdometryVINS(parameters);
 		break;
+	case Odometry::kTypeOpenVINS:
+		odometry = new OdometryOpenVINS(parameters);
+		break;
 	default:
 		UERROR("Unknown odometry type %d, using F2M instead...", (int)type);
 		odometry = new OdometryF2M(parameters);

corelib/src/OdometryThread.cpp

@@ -191,10 +191,19 @@ bool OdometryThread::getData(SensorData & data)
 	{
 		if(!_dataBuffer.empty())
 		{
-			while(!_imuBuffer.empty() && _imuBuffer.front().stamp() <= _dataBuffer.front().stamp())
+			if(!_imuBuffer.empty())
 			{
-				_odometry->process(_imuBuffer.front());
+				// Send IMU up to stamp greater than image (OpenVINS needs this).
-				_imuBuffer.pop_front();
+				while(!_imuBuffer.empty())
+				{
+					_odometry->process(_imuBuffer.front());
+					double stamp = _imuBuffer.front().stamp();
+					_imuBuffer.pop_front();
+					if(stamp > _dataBuffer.front().stamp())
+					{
+						break;
+					}
+				}
 			}
 
 			data = _dataBuffer.front();

corelib/src/Parameters.cpp

@@ -872,6 +872,12 @@ ParametersMap Parameters::parseArguments(int argc, char * argv[], bool onlyParam
 				std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl;
 #else
 				std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
+#endif
+				str = "With OpenVINS:";
+#ifdef RTABMAP_OPENVINS
+				std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl;
+#else
+				std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
 #endif
 				exit(0);
 			}

corelib/src/odometry/OdometryOpenVINS.cpp

@@ -0,0 +1,486 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+ * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+ * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+ * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include "rtabmap/core/odometry/OdometryOpenVINS.h"
+#include "rtabmap/core/OdometryInfo.h"
+#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"
+#endif
+
+namespace rtabmap {
+
+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;
+		previousPose_.setIdentity();
+		previousLocalTransform_.setNull();
+		imuBuffer_.clear();
+	}
+	initGravity_ = false;
+#endif
+}
+
+// return not null transform if odometry is correctly computed
+Transform OdometryOpenVINS::computeTransform(
+		SensorData & data,
+		const Transform & guess,
+		OdometryInfo * info)
+{
+	Transform t;
+#ifdef RTABMAP_OPENVINS
+	UTimer timer;
+
+	// 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())
+	{
+		if(imuBuffer_.empty())
+		{
+			UWARN("Waiting IMU for initialization...");
+			return t;
+		}
+		if(vioManager_ == 0)
+		{
+			UINFO("OpenVINS Initialization");
+
+			// intialize
+			ov_msckf::VioManagerOptions params;
+
+			// ESTIMATOR ======================================================================
+
+			// Main EKF parameters
+			//params.state_options.do_fej = true;
+			//params.state_options.imu_avg =false;
+			//params.state_options.use_rk4_integration;
+			//params.state_options.do_calib_camera_pose = false;
+			//params.state_options.do_calib_camera_intrinsics = false;
+			//params.state_options.do_calib_camera_timeoffset = false;
+			//params.state_options.max_clone_size = 11;
+			//params.state_options.max_slam_features = 25;
+			//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.dt_slam_delay = 2;
+
+			params.stereo_pairs.emplace_back(0, 1);
+			params.state_options.num_unique_cameras = 1;
+
+			// 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)
+			{
+				printf(RED "VioManager(): invalid feature representation specified:\n" RESET);
+				printf(RED "\t- GLOBAL_3D\n" RESET);
+				printf(RED "\t- GLOBAL_FULL_INVERSE_DEPTH\n" RESET);
+				printf(RED "\t- ANCHORED_3D\n" RESET);
+				printf(RED "\t- ANCHORED_FULL_INVERSE_DEPTH\n" RESET);
+				printf(RED "\t- ANCHORED_MSCKF_INVERSE_DEPTH\n" RESET);
+				printf(RED "\t- ANCHORED_INVERSE_DEPTH_SINGLE\n" RESET);
+				std::exit(EXIT_FAILURE);
+			}
+
+			// Filter initialization
+			//params.init_window_time = 1;
+			//params.init_imu_thresh = 1;
+
+			// Zero velocity update
+			//params.try_zupt = false;
+			//params.zupt_options.chi2_multipler = 5;
+			//params.zupt_max_velocity = 1;
+			//params.zupt_noise_multiplier = 1;
+
+			// NOISE ======================================================================
+
+			// Our noise values for inertial sensor
+			//params.imu_noises.sigma_w = 1.6968e-04;
+			//params.imu_noises.sigma_a = 2.0000e-3;
+			//params.imu_noises.sigma_wb = 1.9393e-05;
+			//params.imu_noises.sigma_ab = 3.0000e-03;
+
+			// Read in update parameters
+			//params.msckf_options.sigma_pix = 1;
+			//params.msckf_options.chi2_multipler = 5;
+			//params.slam_options.sigma_pix = 1;
+			//params.slam_options.chi2_multipler = 5;
+			//params.aruco_options.sigma_pix = 1;
+			//params.aruco_options.chi2_multipler = 5;
+
+
+			// STATE ======================================================================
+
+			// Timeoffset from camera to IMU
+			//params.calib_camimu_dt = 0.0;
+
+			// Global gravity
+			//params.gravity[2] = 9.81;
+
+
+			// TRACKERS ======================================================================
+
+			// Tracking flags
+			params.use_stereo = true;
+			//params.use_klt = true;
+			params.use_aruco = false;
+			//params.downsize_aruco = true;
+			//params.downsample_cameras = false;
+			//params.use_multi_threading = true;
+
+			// General parameters
+			//params.num_pts = 200;
+			//params.fast_threshold = 10;
+			//params.grid_x = 10;
+			//params.grid_y = 5;
+			//params.min_px_dist = 8;
+			//params.knn_ratio = 0.7;
+
+			// Feature initializer parameters
+			//nh.param<bool>("fi_triangulate_1d", params.featinit_options.triangulate_1d, params.featinit_options.triangulate_1d);
+			//nh.param<bool>("fi_refine_features", params.featinit_options.refine_features, params.featinit_options.refine_features);
+			//nh.param<int>("fi_max_runs", params.featinit_options.max_runs, params.featinit_options.max_runs);
+			//nh.param<double>("fi_init_lamda", params.featinit_options.init_lamda, params.featinit_options.init_lamda);
+			//nh.param<double>("fi_max_lamda", params.featinit_options.max_lamda, params.featinit_options.max_lamda);
+			//nh.param<double>("fi_min_dx", params.featinit_options.min_dx, params.featinit_options.min_dx);
+			///nh.param<double>("fi_min_dcost", params.featinit_options.min_dcost, params.featinit_options.min_dcost);
+			//nh.param<double>("fi_lam_mult", params.featinit_options.lam_mult, params.featinit_options.lam_mult);
+			//nh.param<double>("fi_min_dist", params.featinit_options.min_dist, params.featinit_options.min_dist);
+			//params.featinit_options.max_dist = 75;
+			//params.featinit_options.max_baseline = 500;
+			//params.featinit_options.max_cond_number = 5000;
+
+
+			// CAMERA ======================================================================
+			bool fisheye = data.stereoCameraModel().left().isFisheye() && !this->imagesAlreadyRectified();
+			params.camera_fisheye.insert(std::make_pair(0, fisheye));
+			params.camera_fisheye.insert(std::make_pair(1, fisheye));
+
+			Eigen::VectorXd camLeft(8), camRight(8);
+			if(this->imagesAlreadyRectified() || data.stereoCameraModel().left().D_raw().empty())
+			{
+				camLeft << data.stereoCameraModel().left().fx(),
+					 data.stereoCameraModel().left().fy(),
+					 data.stereoCameraModel().left().cx(),
+					 data.stereoCameraModel().left().cy(), 0, 0, 0, 0;
+				camRight << data.stereoCameraModel().right().fx(),
+					 data.stereoCameraModel().right().fy(),
+					 data.stereoCameraModel().right().cx(),
+					 data.stereoCameraModel().right().cy(), 0, 0, 0, 0;
+			}
+			else
+			{
+				UASSERT(data.stereoCameraModel().left().D_raw().cols == data.stereoCameraModel().right().D_raw().cols);
+				UASSERT(data.stereoCameraModel().left().D_raw().cols >= 4);
+				UASSERT(data.stereoCameraModel().right().D_raw().cols >= 4);
+
+				//https://github.com/ethz-asl/kalibr/wiki/supported-models
+				///	    radial-tangential (radtan)
+				//		(distortion_coeffs: [k1 k2 r1 r2])
+				///		equidistant (equi)
+				//		(distortion_coeffs: [k1 k2 k3 k4]) rtabmap: (k1,k2,p1,p2,k3,k4)
+
+				camLeft <<
+					 data.stereoCameraModel().left().K_raw().at<double>(0,0),
+					 data.stereoCameraModel().left().K_raw().at<double>(1,1),
+					 data.stereoCameraModel().left().K_raw().at<double>(0,2),
+					 data.stereoCameraModel().left().K_raw().at<double>(1,2),
+					 data.stereoCameraModel().left().D_raw().at<double>(0,0),
+					 data.stereoCameraModel().left().D_raw().at<double>(0,1),
+					 data.stereoCameraModel().left().D_raw().at<double>(0,fisheye?4:2),
+					 data.stereoCameraModel().left().D_raw().at<double>(0,fisheye?5:3);
+				camRight <<
+					 data.stereoCameraModel().right().K_raw().at<double>(0,0),
+					 data.stereoCameraModel().right().K_raw().at<double>(1,1),
+					 data.stereoCameraModel().right().K_raw().at<double>(0,2),
+					 data.stereoCameraModel().right().K_raw().at<double>(1,2),
+					 data.stereoCameraModel().right().D_raw().at<double>(0,0),
+					 data.stereoCameraModel().right().D_raw().at<double>(0,1),
+					 data.stereoCameraModel().right().D_raw().at<double>(0,fisheye?4:2),
+					 data.stereoCameraModel().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));
+
+			const IMU & imu = imuBuffer_.begin()->second;
+			imuLocalTransform_ = imu.localTransform();
+			Transform imuCam0 = imuLocalTransform_.inverse() * data.stereoCameraModel().localTransform();
+			Transform cam0cam1;
+			if(this->imagesAlreadyRectified() || data.stereoCameraModel().stereoTransform().isNull())
+			{
+				cam0cam1 = Transform(
+						1, 0, 0, data.stereoCameraModel().baseline(),
+						0, 1, 0, 0,
+						0, 0, 1, 0);
+			}
+			else
+			{
+				cam0cam1 = data.stereoCameraModel().stereoTransform().inverse();
+			}
+			UASSERT(!cam0cam1.isNull());
+			Transform imuCam1 = imuCam0 * cam0cam1;
+			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(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(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.camera_wh.insert({0, std::make_pair(data.stereoCameraModel().left().imageWidth(),data.stereoCameraModel().left().imageHeight())});
+			params.camera_wh.insert({1, std::make_pair(data.stereoCameraModel().right().imageWidth(),data.stereoCameraModel().right().imageHeight())});
+
+			vioManager_ = new ov_msckf::VioManager(params);
+		}
+
+		cv::Mat left;
+		cv::Mat 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;
+		message.timestamp = data.stamp();
+		message.sensor_ids.push_back(0);
+		message.sensor_ids.push_back(1);
+		message.images.push_back(left);
+		message.images.push_back(right);
+
+		// send it to our VIO system
+		vioManager_->feed_measurement_camera(message);
+		UDEBUG("Image update stamp=%f", data.stamp());
+
+		double lastIMUstamp = 0.0;
+		while(!imuBuffer_.empty())
+		{
+			std::map<double, IMU>::iterator iter = imuBuffer_.begin();
+
+			// Process IMU data until stamp is over image stamp
+			ov_core::ImuData message;
+			message.timestamp = iter->first;
+			message.wm << iter->second.angularVelocity().val[0], iter->second.angularVelocity().val[1], iter->second.angularVelocity().val[2];
+			message.am << iter->second.linearAcceleration().val[0], iter->second.linearAcceleration().val[1], iter->second.linearAcceleration().val[2];
+
+			UDEBUG("IMU update stamp=%f", message.timestamp);
+
+			// send it to our VIO system
+			vioManager_->feed_measurement_imu(message);
+
+			lastIMUstamp = iter->first;
+
+			imuBuffer_.erase(iter);
+
+			if(lastIMUstamp > data.stamp())
+			{
+				break;
+			}
+		}
+
+		if(vioManager_->initialized())
+		{
+			// Get the current state
+			std::shared_ptr<ov_msckf::State> state = vioManager_->get_state();
+
+			if(state->_timestamp != data.stamp())
+			{
+				UWARN("OpenVINS: Stamp of the current state %f is not the same "
+						"than last image processed %f (last IMU stamp=%f). There could be "
+						"a synchronization issue between camera and IMU. ",
+						state->_timestamp,
+						data.stamp(),
+						lastIMUstamp);
+			}
+
+			Transform p(
+					(float)state->_imu->pos()(0),
+					(float)state->_imu->pos()(1),
+					(float)state->_imu->pos()(2),
+					(float)state->_imu->quat()(0),
+					(float)state->_imu->quat()(1),
+					(float)state->_imu->quat()(2),
+					(float)state->_imu->quat()(3));
+
+
+			// Finally set the covariance in the message (in the order position then orientation as per ros convention)
+			std::vector<std::shared_ptr<ov_type::Type>> statevars;
+			statevars.push_back(state->_imu->pose()->p());
+			statevars.push_back(state->_imu->pose()->q());
+
+			cv::Mat covariance = cv::Mat::eye(6,6, CV_64FC1);
+			if(this->framesProcessed() == 0)
+			{
+				covariance *= 9999;
+			}
+			else
+			{
+				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);
+					}
+				}
+			}
+
+			if(!p.isNull())
+			{
+				p =  p * imuLocalTransform_.inverse();
+
+				if(this->getPose().rotation().isIdentity())
+				{
+					initGravity_ = true;
+					this->reset(this->getPose()*p.rotation());
+				}
+
+				if(previousPose_.isIdentity())
+				{
+					previousPose_ = p;
+				}
+
+				// make it incremental
+				Transform previousPoseInv = previousPose_.inverse();
+				t = previousPoseInv*p;
+				previousPose_ = p;
+
+				if(info)
+				{
+					info->type = this->getType();
+					info->reg.covariance = covariance;
+
+					// feature map
+					Transform fixT = this->getPose()*previousPoseInv;
+					Transform camLocalTransformInv = data.stereoCameraModel().localTransform().inverse()*this->getPose().inverse();
+					for (auto &it_per_id : vioManager_->get_features_SLAM())
+					{
+						cv::Point3f pt3d;
+						pt3d.x = it_per_id[0];
+						pt3d.y = it_per_id[1];
+						pt3d.z = it_per_id[2];
+						pt3d = util3d::transformPoint(pt3d, fixT);
+						info->localMap.insert(std::make_pair(info->localMap.size(), pt3d));
+
+						if(this->imagesAlreadyRectified())
+						{
+							cv::Point2f pt;
+							pt3d = util3d::transformPoint(pt3d, camLocalTransformInv);
+							data.stereoCameraModel().left().reproject(pt3d.x, pt3d.y, pt3d.z, pt.x, pt.y);
+							info->reg.inliersIDs.push_back(info->newCorners.size());
+							info->newCorners.push_back(pt);
+						}
+					}
+					info->features = info->newCorners.size();
+					info->localMapSize = info->localMap.size();
+				}
+				UINFO("Odom update time = %fs p=%s", timer.elapsed(), p.prettyPrint().c_str());
+			}
+		}
+	}
+	else if(!data.imageRaw().empty() && !data.depthRaw().empty())
+	{
+		UERROR("OpenVINS doesn't work with RGB-D data, stereo images are required!");
+	}
+	else if(!data.imageRaw().empty() && data.depthOrRightRaw().empty())
+	{
+		UERROR("OpenVINS requires stereo images!");
+	}
+
+#else
+	UERROR("RTAB-Map is not built with OpenVINS support! Select another visual odometry approach.");
+#endif
+	return t;
+}
+
+} // namespace rtabmap

corelib/src/odometry/OdometryVINS.cpp

@@ -288,7 +288,6 @@ OdometryVINS::OdometryVINS(const ParametersMap & parameters) :
 #ifdef RTABMAP_VINS
     ,
 	vinsEstimator_(0),
-	imagesProcessed_(0),
 	initGravity_(false),
 	previousPose_(Transform::getIdentity())
 #endif
@@ -431,7 +430,7 @@ Transform OdometryVINS::computeTransform(
 			{
 				if(!lastImu_.localTransform().isNull())
 				{
-					p = Transform(0,1,0,0,-1,0,0,0, 0,0,1,0) * p * lastImu_.localTransform().inverse();
+					p = p * lastImu_.localTransform().inverse();
 				}
 
 				if(this->getPose().rotation().isIdentity())
@@ -457,7 +456,7 @@ Transform OdometryVINS::computeTransform(
 					info->reg.covariance *= this->framesProcessed() == 0?9999:0.0001;
 
 					// feature map
-					Transform fixT = this->getPose()*previousPoseInv*Transform(0,1,0,0,-1,0,0,0, 0,0,1,0);
+					Transform fixT = this->getPose()*previousPoseInv;
 					for (auto &it_per_id : vinsEstimator_->f_manager.feature)
 					{
 						int used_num;
@@ -467,7 +466,7 @@ Transform OdometryVINS::computeTransform(
 						if (it_per_id.start_frame > WINDOW_SIZE * 3.0 / 4.0 || it_per_id.solve_flag != 1)
 							continue;
 						int imu_i = it_per_id.start_frame;
-						Vector3d pts_i = it_per_id.feature_per_frame[0].point * it_per_id.estimated_depth;
+						Vector3d pts_i = it_per_id.feature_per_frame[it_per_id.feature_per_frame.size()-1].point * it_per_id.estimated_depth;
 						Vector3d w_pts_i = vinsEstimator_->Rs[imu_i] * (vinsEstimator_->ric[0] * pts_i + vinsEstimator_->tic[0]) + vinsEstimator_->Ps[imu_i];
 
 						cv::Point3f p;

guilib/src/AboutDialog.cpp

@@ -234,6 +234,22 @@ AboutDialog::AboutDialog(QWidget * parent) :
 	_ui->label_msckf_license->setEnabled(false);
 #endif
 
+#ifdef RTABMAP_VINS
+	_ui->label_vins_fusion->setText("Yes");
+	_ui->label_vins_fusion_license->setEnabled(true);
+#else
+	_ui->label_vins_fusion->setText("No");
+	_ui->label_vins_fusion_license->setEnabled(false);
+#endif
+
+#ifdef RTABMAP_OPENVINS
+	_ui->label_openvins->setText("Yes");
+	_ui->label_openvins_license->setEnabled(true);
+#else
+	_ui->label_openvins->setText("No");
+	_ui->label_openvins_license->setEnabled(false);
+#endif
+
 }
 
 AboutDialog::~AboutDialog()

guilib/src/MainWindow.cpp

@@ -1492,7 +1492,8 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI
 					odom.info().type == (int)Odometry::kTypeViso2 ||
 					odom.info().type == (int)Odometry::kTypeFovis ||
 					odom.info().type == (int)Odometry::kTypeMSCKF ||
-					odom.info().type == (int)Odometry::kTypeVINS)
+					odom.info().type == (int)Odometry::kTypeVINS ||
+					odom.info().type == (int)Odometry::kTypeOpenVINS)
 			{
 				std::vector<cv::KeyPoint> kpts;
 				cv::KeyPoint::convert(odom.info().newCorners, kpts, 7);
@@ -1541,7 +1542,8 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI
 			if( odom.info().type == (int)Odometry::kTypeF2M ||
 				odom.info().type == (int)Odometry::kTypeORBSLAM ||
 				odom.info().type == (int)Odometry::kTypeMSCKF ||
-				odom.info().type == (int)Odometry::kTypeVINS)
+				odom.info().type == (int)Odometry::kTypeVINS ||
+				odom.info().type == (int)Odometry::kTypeOpenVINS)
 			{
 				if(_ui->imageView_odometry->isFeaturesShown() && !_preferencesDialog->isOdomOnlyInliersShown())
 				{
@@ -5427,7 +5429,10 @@ void MainWindow::startDetection()
 					_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcImages) &&
 				   !_preferencesDialog->getIMUPath().isEmpty())
 				{
-					if(odomStrategy != Odometry::kTypeOkvis && odomStrategy != Odometry::kTypeMSCKF && odomStrategy != Odometry::kTypeVINS)
+					if( odomStrategy != Odometry::kTypeOkvis &&
+						odomStrategy != Odometry::kTypeMSCKF &&
+						odomStrategy != Odometry::kTypeVINS &&
+						odomStrategy != Odometry::kTypeOpenVINS)
 					{
 						QMessageBox::warning(this, tr("Source IMU Path"),
 								tr("IMU path is set but odometry chosen doesn't support IMU, ignoring IMU..."), QMessageBox::Ok);

guilib/src/PreferencesDialog.cpp

@@ -192,6 +192,9 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 #ifndef RTABMAP_VINS
 	_ui->odom_strategy->setItemData(9, 0, Qt::UserRole - 1);
 #endif
+#ifndef RTABMAP_OPENVINS
+	_ui->odom_strategy->setItemData(10, 0, Qt::UserRole - 1);
+#endif
 
 #if CV_MAJOR_VERSION < 3
 	_ui->stereosgbm_mode->setItemData(2, 0, Qt::UserRole - 1);
@@ -6371,7 +6374,8 @@ void PreferencesDialog::testOdometry()
 	{
 		if(this->getOdomStrategy() != Odometry::kTypeOkvis &&
 		   this->getOdomStrategy() != Odometry::kTypeMSCKF &&
-		   this->getOdomStrategy() != Odometry::kTypeVINS)
+		   this->getOdomStrategy() != Odometry::kTypeVINS &&
+		   this->getOdomStrategy() != Odometry::kTypeOpenVINS)
 		{
 			QMessageBox::warning(this, tr("Source IMU Path"),
 					tr("IMU path is set but odometry chosen doesn't support IMU, ignoring IMU..."), QMessageBox::Ok);

guilib/src/ui/preferencesDialog.ui

@@ -63,7 +63,7 @@
           <property name="geometry">
            <rect>
             <x>0</x>
-            <y>-1011</y>
+            <y>-120</y>
             <width>686</width>
             <height>3414</height>
            </rect>
@@ -95,7 +95,7 @@
                 <enum>QFrame::Raised</enum>
                </property>
                <property name="currentIndex">
-                <number>22</number>
+                <number>19</number>
                </property>
                <widget class="QWidget" name="page_22">
                 <layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,1">
@@ -13621,6 +13621,11 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                           <string>VINS-Fusion</string>
                          </property>
                         </item>
+                        <item>
+                         <property name="text">
+                          <string>OpenVINS</string>
+                         </property>
+                        </item>
                        </widget>
                       </item>
                       <item row="2" column="1">
@@ -13924,7 +13929,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                     <item>
                      <widget class="QStackedWidget" name="stackedWidget_odometryType">
                       <property name="currentIndex">
-                       <number>5</number>
+                       <number>10</number>
                       </property>
                       <widget class="QWidget" name="page_52">
                        <layout class="QVBoxLayout" name="verticalLayout_77">
@@ -17100,6 +17105,48 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </item>
                        </layout>
                       </widget>
+                      <widget class="QWidget" name="page_93">
+                       <layout class="QVBoxLayout" name="verticalLayout_162">
+                        <item>
+                         <widget class="QGroupBox" name="groupBox_odomVINS_2">
+                          <property name="title">
+                           <string>VINS-Fusion</string>
+                          </property>
+                          <layout class="QVBoxLayout" name="verticalLayout_161" stretch="0">
+                           <item>
+                            <widget class="QLabel" name="label_632">
+                             <property name="text">
+                              <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;OpenVINS: &lt;a href=&quot;https://github.com/rpng/open_vins&quot;&gt;&lt;span style=&quot; text-decoration: underline; color:#0000ff;&quot;&gt;https://github.com/rpng/open_vins&lt;/span&gt;&lt;/a&gt;&lt;/p&gt;&lt;p&gt;Currently tested only with EuRoC data set.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
+                             </property>
+                             <property name="wordWrap">
+                              <bool>true</bool>
+                             </property>
+                             <property name="openExternalLinks">
+                              <bool>true</bool>
+                             </property>
+                             <property name="textInteractionFlags">
+                              <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                             </property>
+                            </widget>
+                           </item>
+                          </layout>
+                         </widget>
+                        </item>
+                        <item>
+                         <spacer name="verticalSpacer_89">
+                          <property name="orientation">
+                           <enum>Qt::Vertical</enum>
+                          </property>
+                          <property name="sizeHint" stdset="0">
+                           <size>
+                            <width>20</width>
+                            <height>1049</height>
+                           </size>
+                          </property>
+                         </spacer>
+                        </item>
+                       </layout>
+                      </widget>
                       <widget class="QWidget" name="page_26">
                        <layout class="QVBoxLayout" name="verticalLayout_88">
                         <item>

package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package>
   <name>rtabmap</name>
-  <version>0.20.9</version>
+  <version>0.20.10</version>
   <description>RTAB-Map's standalone library. RTAB-Map is a RGB-D SLAM approach with real-time constraints.</description>
   <maintainer email="[email protected]">Mathieu Labbe</maintainer>
   <author>Mathieu Labbe</author>