VINS Fusion overhaul (#1580)

* Updated correct focal length used by VINS, updated VINS visualization.

* Renamed OdometryVINS to OdometryVINSFusion

CMakeLists.txt

@@ -20,7 +20,7 @@ SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules")
 #######################
 SET(RTABMAP_MAJOR_VERSION 0)
 SET(RTABMAP_MINOR_VERSION 23)
-SET(RTABMAP_PATCH_VERSION 0)
+SET(RTABMAP_PATCH_VERSION 1)
 SET(RTABMAP_VERSION
   ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION})
   
@@ -218,7 +218,7 @@ option(WITH_DVO           "Include DVO support"                  OFF)
 option(WITH_ORB_SLAM      "Include ORB_SLAM2 or ORB_SLAM3  support" OFF)
 option(WITH_OKVIS         "Include OKVIS support"                OFF)
 option(WITH_MSCKF_VIO     "Include MSCKF_VIO support"            OFF)
-option(WITH_VINS          "Include VINS-Fusion support"          OFF)
+option(WITH_VINS_FUSION   "Include VINS-Fusion support"          OFF)
 option(WITH_OPENVINS      "Include OpenVINS support"             OFF)
 option(WITH_MADGWICK      "Include Madgwick IMU filtering support" ON)
 option(WITH_FASTCV        "Include FastCV support"               ON)
@@ -794,15 +794,20 @@ IF(WITH_MSCKF_VIO)
     ENDIF(msckf_vio_FOUND)
 ENDIF(WITH_MSCKF_VIO)
 
-IF(WITH_VINS)
+IF(WITH_VINS AND NOT WITH_VINS_FUSION)
+message(DEPRECATION "The option WITH_VINS is deprecated and will be removed in a future version. Please use WITH_VINS_FUSION instead.")
+set(WITH_VINS_FUSION ON)
+ENDIF(WITH_VINS AND NOT WITH_VINS_FUSION)
+
+IF(WITH_VINS_FUSION)
     FIND_PACKAGE(vins QUIET)
     IF(vins_FOUND)
-       MESSAGE(STATUS "Found vins: ${vins_INCLUDE_DIRS}")
+       MESSAGE(STATUS "Found vins-fusion: ${vins_INCLUDE_DIRS}")
        IF(okvis_FOUND)
-           MESSAGE(WARNING "VINS and OKVIS will be both linked to project, make sure VINS has been built with against same Ceres version than OKVIS to avoid some crashes.")
+           MESSAGE(WARNING "VINS-Fusion and OKVIS will be both linked to project, make sure VINS-Fusion has been built with against same Ceres version than OKVIS to avoid some crashes.")
        ENDIF(okvis_FOUND)
     ENDIF(vins_FOUND)
-ENDIF(WITH_VINS)
+ENDIF(WITH_VINS_FUSION)
 
 IF(WITH_OPENVINS)
     FIND_PACKAGE(ov_msckf QUIET)
@@ -1101,7 +1106,7 @@ IF(NOT msckf_vio_FOUND)
    SET(MSCKF_VIO "//")
 ENDIF()
 IF(NOT vins_FOUND)
-   SET(VINS "//")
+   SET(VINSFUSION "//")
 ENDIF()
 IF(NOT ov_msckf_FOUND)
    SET(OPENVINS "//")
@@ -1800,7 +1805,7 @@ ENDIF()
 IF(vins_FOUND)
 MESSAGE(STATUS "  With VINS-Fusion          = YES (License: GPLv3)")
 ELSEIF(NOT WITH_VINS)
-MESSAGE(STATUS "  With VINS-Fusion          = NO (WITH_VINS=OFF)")
+MESSAGE(STATUS "  With VINS-Fusion          = NO (WITH_VINS_FUSION=OFF)")
 ELSE()
 MESSAGE(STATUS "  With VINS-Fusion          = NO (VINS-Fusion not found)")
 ENDIF()

Version.h.in

@@ -82,7 +82,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 @DVO@#define RTABMAP_DVO
 @OKVIS@#define RTABMAP_OKVIS
 @MSCKF_VIO@#define RTABMAP_MSCKF_VIO
-@VINS@#define RTABMAP_VINS
+@VINSFUSION@#define RTABMAP_VINS_FUSION
 @OPENVINS@#define RTABMAP_OPENVINS
 @ORB_SLAM@#define RTABMAP_ORB_SLAM @ORB_SLAM_VERSION@
 @ORB_OCTREE@#define RTABMAP_ORB_OCTREE

corelib/include/rtabmap/core/Odometry.h

@@ -53,7 +53,7 @@ public:
 		kTypeOkvis = 6,
 		kTypeLOAM = 7,
 		kTypeMSCKF = 8,
-		kTypeVINS = 9,
+		kTypeVINSFusion = 9,
 		kTypeOpenVINS = 10,
 		kTypeFLOAM = 11,
 		kTypeOpen3D = 12

corelib/include/rtabmap/core/Parameters.h

@@ -606,8 +606,8 @@ class RTABMAP_CORE_EXPORT Parameters
     RTABMAP_PARAM(OdomMSCKF, InitCovExTrans,  double,  0.000025,  "");
     RTABMAP_PARAM(OdomMSCKF, MaxCamStateSize,  int,  20,  "");
 
-    // Odometry VINS
-    RTABMAP_PARAM_STR(OdomVINS, ConfigPath,     "",  "Path of VINS config file.");
+    // Odometry VINS-Fusion
+    RTABMAP_PARAM_STR(OdomVINSFusion, ConfigPath,     "",  "Path of VINS-Fusion config file.");
 
     // Odometry OpenVINS
     RTABMAP_PARAM(OdomOpenVINS, UseStereo,                 bool,   true,   "If we have more than 1 camera, if we should try to track stereo constraints between pairs");

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

@@ -25,39 +25,7 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
-#ifndef ODOMETRYVINS_H_
-#define ODOMETRYVINS_H_
+#pragma once
+#pragma message("Warning: OdometryVINS.h is deprecated. Please use OdometryVINSFusion.h instead.")
 
-#include <rtabmap/core/Odometry.h>
-
-namespace rtabmap {
-
-class VinsEstimator;
-
-class RTABMAP_CORE_EXPORT OdometryVINS : public Odometry
-{
-public:
-	OdometryVINS(const rtabmap::ParametersMap & parameters = rtabmap::ParametersMap());
-	virtual ~OdometryVINS();
-
-	virtual void reset(const Transform & initialPose = Transform::getIdentity());
-	virtual Odometry::Type getType() {return Odometry::kTypeVINS;}
-	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_VINS
-	VinsEstimator * vinsEstimator_;
-	bool initGravity_;
-	Transform previousPose_;
-	Transform previousLocalTransform_;
-	IMU lastImu_;
-#endif
-};
-
-}
-
-#endif /* ODOMETRYVINS_H_ */
+#include "rtabmap/core/odometry/OdometryVINSFusion.h"

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

@@ -0,0 +1,64 @@
+/*
+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.
+*/
+
+#ifndef ODOMETRYVINSFUSION_H_
+#define ODOMETRYVINSFUSION_H_
+
+#include <rtabmap/core/Odometry.h>
+
+namespace rtabmap {
+
+class VinsFusionEstimator;
+
+class RTABMAP_CORE_EXPORT OdometryVINSFusion : public Odometry
+{
+public:
+	OdometryVINSFusion(const rtabmap::ParametersMap & parameters = rtabmap::ParametersMap());
+	virtual ~OdometryVINSFusion();
+
+	virtual void reset(const Transform & initialPose = Transform::getIdentity());
+	virtual Odometry::Type getType() {return Odometry::kTypeVINSFusion;}
+	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_VINS_FUSION
+	VinsFusionEstimator * vinsEstimator_;
+	bool initGravity_;
+	Transform previousPose_;
+	Transform previousLocalTransform_;
+	IMU lastImu_;
+	double lastImuStamp_;
+#endif
+};
+
+}
+
+#endif /* ODOMETRYVINSFUSION_H_ */

corelib/src/CMakeLists.txt

@@ -98,7 +98,7 @@ SET(SRC_FILES
     odometry/OdometryLOAM.cpp
     odometry/OdometryFLOAM.cpp
     odometry/OdometryMSCKF.cpp
-    odometry/OdometryVINS.cpp
+    odometry/OdometryVINSFusion.cpp
     odometry/OdometryOpenVINS.cpp
     odometry/OdometryOpen3D.cpp
     

corelib/src/Odometry.cpp

@@ -36,7 +36,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/odometry/OdometryLOAM.h"
 #include "rtabmap/core/odometry/OdometryFLOAM.h"
 #include "rtabmap/core/odometry/OdometryMSCKF.h"
-#include "rtabmap/core/odometry/OdometryVINS.h"
+#include "rtabmap/core/odometry/OdometryVINSFusion.h"
 #include "rtabmap/core/odometry/OdometryOpenVINS.h"
 #include "rtabmap/core/odometry/OdometryOpen3D.h"
 #include "rtabmap/core/OdometryInfo.h"
@@ -103,8 +103,8 @@ Odometry * Odometry::create(Odometry::Type & type, const ParametersMap & paramet
 	case Odometry::kTypeMSCKF:
 		odometry = new OdometryMSCKF(parameters);
 		break;
-	case Odometry::kTypeVINS:
-		odometry = new OdometryVINS(parameters);
+	case Odometry::kTypeVINSFusion:
+		odometry = new OdometryVINSFusion(parameters);
 		break;
 	case Odometry::kTypeOpenVINS:
 		odometry = new OdometryOpenVINS(parameters);

corelib/src/Parameters.cpp

@@ -236,6 +236,9 @@ const std::map<std::string, std::pair<bool, std::string> > & Parameters::getRemo
 	{
 		// removed parameters
 
+		// 0.23.1
+		removedParameters_.insert(std::make_pair("OdomVINS/ConfigPath",    std::make_pair(true, Parameters::kOdomVINSFusionConfigPath())));
+
 		// 0.21.13
 		removedParameters_.insert(std::make_pair("Vis/ForwardEstOnly",    std::make_pair(false, "")));
 
@@ -936,7 +939,7 @@ ParametersMap Parameters::parseArguments(int argc, char * argv[], bool onlyParam
 				std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
 #endif
 				str = "With VINS-Fusion:";
-#ifdef RTABMAP_VINS
+#ifdef RTABMAP_VINS_FUSION
 				std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl;
 #else
 				std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;

corelib/src/odometry/OdometryVINSFusion.cpp

@@ -25,7 +25,7 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  */
 
-#include "rtabmap/core/odometry/OdometryVINS.h"
+#include "rtabmap/core/odometry/OdometryVINSFusion.h"
 #include "rtabmap/core/OdometryInfo.h"
 #include "rtabmap/core/util3d_transforms.h"
 #include "rtabmap/utilite/ULogger.h"
@@ -35,7 +35,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/utilite/UDirectory.h"
 #include <opencv2/imgproc/types_c.h>
 
-#ifdef RTABMAP_VINS
+#ifdef RTABMAP_VINS_FUSION
 #include <estimator/estimator.h>
 #include <estimator/parameters.h>
 #include <camodocal/camera_models/PinholeCamera.h>
@@ -45,11 +45,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 namespace rtabmap {
 
-#ifdef RTABMAP_VINS
-class VinsEstimator: public Estimator
+#ifdef RTABMAP_VINS_FUSION
+class VinsFusionEstimator: public Estimator
 {
 public:
-	VinsEstimator(
+	VinsFusionEstimator(
 			const Transform & imuLocalTransform,
 			const StereoCameraModel & model,
 			bool rectified) : Estimator()
@@ -57,6 +57,9 @@ public:
 		MULTIPLE_THREAD = 0;
 		setParameter();
 
+		ROW=model.left().imageHeight();
+		COL=model.left().imageWidth();
+
 		//overwrite camera calibration only if received model is radtan, otherwise use config
 		UASSERT(NUM_OF_CAM >= 1 && NUM_OF_CAM <=2);
 
@@ -81,6 +84,15 @@ public:
 			camera->setParameters(params);
 			featureTracker.m_camera.push_back(camera);
 
+			double originalParalax = MIN_PARALLAX * FOCAL_LENGTH;
+			// If you have compiler error about FOCAL_LENGTH being const, make sure to use the following patch:
+			// https://gist.github.com/matlabbe/795ab37067367dca58bbadd8201d986c#file-vins-fusion_pull136-patch
+			FOCAL_LENGTH = params.fx();
+			MIN_PARALLAX = originalParalax / FOCAL_LENGTH;
+			ProjectionTwoFrameOneCamFactor::sqrt_info = FOCAL_LENGTH / 1.5 * Matrix2d::Identity();
+			ProjectionTwoFrameTwoCamFactor::sqrt_info = FOCAL_LENGTH / 1.5 * Matrix2d::Identity();
+			ProjectionOneFrameTwoCamFactor::sqrt_info = FOCAL_LENGTH / 1.5 * Matrix2d::Identity();
+
 			if(NUM_OF_CAM == 2)
 			{
 				camodocal::PinholeCameraPtr camera( new camodocal::PinholeCamera );
@@ -120,8 +132,8 @@ public:
 
 		Transform imuCam0 = imuLocalTransform.inverse() * model.localTransform();
 
-		tic[0] = Vector3d(imuCam0.x(), imuCam0.y(), imuCam0.z());
-		ric[0] = imuCam0.toEigen4d().block<3,3>(0,0);
+		tic[0] = TIC[0] = Vector3d(imuCam0.x(), imuCam0.y(), imuCam0.z());
+		ric[0] = RIC[0] = imuCam0.toEigen4d().block<3,3>(0,0);
 
 		if(NUM_OF_CAM == 2)
 		{
@@ -140,54 +152,39 @@ public:
 			UASSERT(!cam0cam1.isNull());
 			Transform imuCam1 = imuCam0 * cam0cam1;
 
-			tic[1] = Vector3d(imuCam1.x(), imuCam1.y(), imuCam1.z());
-			ric[1] = imuCam1.toEigen4d().block<3,3>(0,0);
+			tic[1] = TIC[0] = Vector3d(imuCam1.x(), imuCam1.y(), imuCam1.z());
+			ric[1] = RIC[0] = imuCam1.toEigen4d().block<3,3>(0,0);
 		}
 
 		for (int i = 0; i < NUM_OF_CAM; i++)
 		{
-			cout << " exitrinsic cam " << i << endl  << ric[i] << endl << tic[i].transpose() << endl;
+			cout << " new extrinsic cam " << i << endl  << ric[i] << endl << tic[i].transpose() << endl;
+		}
+		for (int i = 0; i < NUM_OF_CAM; i++)
+		{
+			cout << " new intrinsic cam " << i << endl  << featureTracker.m_camera[i]->parametersToString() << endl;
 		}
 		f_manager.setRic(ric);
-		ProjectionTwoFrameOneCamFactor::sqrt_info = FOCAL_LENGTH / 1.5 * Matrix2d::Identity();
-		ProjectionTwoFrameTwoCamFactor::sqrt_info = FOCAL_LENGTH / 1.5 * Matrix2d::Identity();
-		ProjectionOneFrameTwoCamFactor::sqrt_info = FOCAL_LENGTH / 1.5 * Matrix2d::Identity();
-		td = TD;
-		g = G;
-		cout << "set g " << g.transpose() << endl;
 	}
 
 	// Copy of original inputImage() so that overridden processMeasurements() is used and threading is disabled.
 	void inputImage(double t, const cv::Mat &_img, const cv::Mat &_img1)
 	{
+		TicToc processTime;
 	    inputImageCnt++;
 	    map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> featureFrame;
-	    TicToc featureTrackerTime;
-	    if(_img1.empty())
+	    if(_img1.empty()) {
 	        featureFrame = featureTracker.trackImage(t, _img);
-	    else
+		}
+	    else {
 	        featureFrame = featureTracker.trackImage(t, _img, _img1);
-	    //printf("featureTracker time: %f\n", featureTrackerTime.toc());
-
-	    //if(MULTIPLE_THREAD)
-	    //{
-	    //    if(inputImageCnt % 2 == 0)
-	    //    {
-	    //        mBuf.lock();
-	    //        featureBuf.push(make_pair(t, featureFrame));
-	    //        mBuf.unlock();
-	    //    }
-	    //}
-	    //else
-	    {
-	        mBuf.lock();
-	        featureBuf.push(make_pair(t, featureFrame));
-	        mBuf.unlock();
-	        TicToc processTime;
-	        processMeasurements();
-	        UDEBUG("VINS process time: %f", processTime.toc());
-	    }
+		}
 
+		mBuf.lock();
+		featureBuf.push(make_pair(t, featureFrame));
+		mBuf.unlock();
+		processMeasurements();
+		UDEBUG("VINS process time: %f", processTime.toc());
 	}
 
 	// Copy of original inputIMU() but with publisher commented
@@ -199,127 +196,98 @@ public:
 		//printf("input imu with time %f \n", t);
 		mBuf.unlock();
 
-		fastPredictIMU(t, linearAcceleration, angularVelocity);
-		//if (solver_flag == NON_LINEAR)
-		//	pubLatestOdometry(latest_P, latest_Q, latest_V, t);
+		if (solver_flag == NON_LINEAR)
+		{
+			mPropagate.lock();
+			fastPredictIMU(t, linearAcceleration, angularVelocity);
+			mPropagate.unlock();
+		}
 	}
 
 	// Copy of original processMeasurements() but with publishers commented and threading disabled
 	void processMeasurements()
 	{
-		//while (1)
+		pair<double, map<int, vector<pair<int, Eigen::Matrix<double, 7, 1> > > > > feature;
+		vector<pair<double, Eigen::Vector3d>> accVector, gyrVector;
+		if(!featureBuf.empty())
 		{
-			//printf("process measurments\n");
-			pair<double, map<int, vector<pair<int, Eigen::Matrix<double, 7, 1> > > > > feature;
-			vector<pair<double, Eigen::Vector3d>> accVector, gyrVector;
-			if(!featureBuf.empty())
+			feature = featureBuf.front();
+			curTime = feature.first + td;
+			if (USE_IMU && !IMUAvailable(feature.first + td))
 			{
-				feature = featureBuf.front();
-				curTime = feature.first + td;
-				//while(1)
-				//{
-					if (!((!USE_IMU  || IMUAvailable(feature.first + td))))
-					//if ((!USE_IMU  || IMUAvailable(feature.first + td)))
-					//	break;
-					//else
-					{
-						printf("wait for imu ... \n");
-						//if (! MULTIPLE_THREAD)
-							return;
-						//std::chrono::milliseconds dura(5);
-						//std::this_thread::sleep_for(dura);
-					}
-				//}
-				mBuf.lock();
-				if(USE_IMU)
-					getIMUInterval(prevTime, curTime, accVector, gyrVector);
-
-				featureBuf.pop();
-				mBuf.unlock();
-
-				if(USE_IMU)
-				{
-					if(!initFirstPoseFlag)
-						initFirstIMUPose(accVector);
-					UDEBUG("accVector.size() = %d", accVector.size());
-					for(size_t i = 0; i < accVector.size(); i++)
-					{
-						double dt;
-						if(i == 0)
-							dt = accVector[i].first - prevTime;
-						else if (i == accVector.size() - 1)
-							dt = curTime - accVector[i - 1].first;
-						else
-							dt = accVector[i].first - accVector[i - 1].first;
-						processIMU(accVector[i].first, dt, accVector[i].second, gyrVector[i].second);
-					}
-				}
-
-				processImage(feature.second, feature.first);
-
-				prevTime = curTime;
-
-				printStatistics(*this, 0);
-
-				//std_msgs::Header header;
-				//header.frame_id = "world";
-				//header.stamp = ros::Time(feature.first);
-
-				//pubOdometry(*this, header);
-				//pubKeyPoses(*this, header);
-				//pubCameraPose(*this, header);
-				//pubPointCloud(*this, header);
-				//pubKeyframe(*this);
-				//pubTF(*this, header);
+				printf("wait for imu ... \n");
+				return;
 			}
+			mBuf.lock();
+			if(USE_IMU)
+				getIMUInterval(prevTime, curTime, accVector, gyrVector);
 
-			//if (! MULTIPLE_THREAD)
-			//	break;
+			featureBuf.pop();
+			mBuf.unlock();
 
-			//std::chrono::milliseconds dura(2);
-			//std::this_thread::sleep_for(dura);
+			if(USE_IMU)
+			{
+				if(!initFirstPoseFlag)
+					initFirstIMUPose(accVector);
+
+				for(size_t i = 0; i < accVector.size(); i++)
+				{
+					double dt;
+					if(i == 0)
+						dt = accVector[i].first - prevTime;
+					else if (i == accVector.size() - 1)
+						dt = curTime - accVector[i - 1].first;
+					else
+						dt = accVector[i].first - accVector[i - 1].first;
+					processIMU(accVector[i].first, dt, accVector[i].second, gyrVector[i].second);
+				}
+			}
+			mProcess.lock();
+			processImage(feature.second, feature.first);
+			prevTime = curTime;																
+			mProcess.unlock();
 		}
 	}
 };
 #endif
 
-OdometryVINS::OdometryVINS(const ParametersMap & parameters) :
+OdometryVINSFusion::OdometryVINSFusion(const ParametersMap & parameters) :
 	Odometry(parameters)
-#ifdef RTABMAP_VINS
+#ifdef RTABMAP_VINS_FUSION
     ,
 	vinsEstimator_(0),
 	initGravity_(false),
 	previousPose_(Transform::getIdentity())
 #endif
 {
-#ifdef RTABMAP_VINS
+#ifdef RTABMAP_VINS_FUSION
 	// intialize
 	std::string configFilename;
-	Parameters::parse(parameters, Parameters::kOdomVINSConfigPath(), configFilename);
+	Parameters::parse(parameters, Parameters::kOdomVINSFusionConfigPath(), configFilename);
 	if(configFilename.empty())
 	{
-		UERROR("VINS config file is empty (%s=%s)!",
-				Parameters::kOdomVINSConfigPath().c_str(),
-				Parameters::kOdomVINSConfigPath().c_str());
+		UERROR("VINS config file is empty (%s)!",
+				Parameters::kOdomVINSFusionConfigPath().c_str());
 	}
 	else
 	{
+		UINFO("Using config file %s", configFilename.c_str());
 		readParameters(uReplaceChar(configFilename, '~', UDirectory::homeDir()));
 	}
 #endif
 }
 
-OdometryVINS::~OdometryVINS()
+OdometryVINSFusion::~OdometryVINSFusion()
 {
-#ifdef RTABMAP_VINS
+#ifdef RTABMAP_VINS_FUSION
 	delete vinsEstimator_;
 #endif
 }
 
-void OdometryVINS::reset(const Transform & initialPose)
+void OdometryVINSFusion::reset(const Transform & initialPose)
 {
 	Odometry::reset(initialPose);
-#ifdef RTABMAP_VINS
+#ifdef RTABMAP_VINS_FUSION
 	if(!initGravity_)
 	{
 		delete vinsEstimator_;
@@ -333,18 +301,19 @@ void OdometryVINS::reset(const Transform & initialPose)
 }
 
 // return not null transform if odometry is correctly computed
-Transform OdometryVINS::computeTransform(
+Transform OdometryVINSFusion::computeTransform(
 		SensorData & data,
 		const Transform & guess,
 		OdometryInfo * info)
 {
 	Transform t;
-#ifdef RTABMAP_VINS
+#ifdef RTABMAP_VINS_FUSION
 	UTimer timer;
 
+	bool hasImage = !data.imageRaw().empty() && !data.rightRaw().empty() && data.stereoCameraModels().size() == 1 && data.stereoCameraModels()[0].isValidForProjection();
+
 	if(USE_IMU!=0 && !data.imu().empty())
 	{
-		double t = data.stamp();
 		double dx = data.imu().linearAcceleration().val[0];
 		double dy = data.imu().linearAcceleration().val[1];
 		double dz = data.imu().linearAcceleration().val[2];
@@ -358,16 +327,19 @@ Transform OdometryVINS::computeTransform(
 
 		if(vinsEstimator_ != 0)
 		{
-			vinsEstimator_->inputIMU(t, acc, gyr);
+			vinsEstimator_->inputIMU(data.stamp(), acc, gyr);
 		}
 		else
 		{
 			lastImu_ = data.imu();
-			UWARN("Waiting an image for initialization...");
+			lastImuStamp_ = data.stamp();
+			if(!hasImage) {
+				UWARN("Waiting an image for initialization...");
+			}
 		}
 	}
 
-	if(!data.imageRaw().empty() && !data.rightRaw().empty() && data.stereoCameraModels().size() == 1 && data.stereoCameraModels()[0].isValidForProjection())
+	if(hasImage)
 	{
 		if(USE_IMU==1 && lastImu_.localTransform().isNull())
 		{
@@ -377,10 +349,23 @@ Transform OdometryVINS::computeTransform(
 		if(vinsEstimator_ == 0)
 		{
 			// intialize
-			vinsEstimator_ = new VinsEstimator(
+			UINFO("Initializing with image %f", data.stamp());
+			vinsEstimator_ = new VinsFusionEstimator(
 					lastImu_.localTransform().isNull()?Transform::getIdentity():lastImu_.localTransform(),
 					data.stereoCameraModels()[0],
 					this->imagesAlreadyRectified());
+
+			if(USE_IMU) {
+				double dx = lastImu_.linearAcceleration().val[0];
+				double dy = lastImu_.linearAcceleration().val[1];
+				double dz = lastImu_.linearAcceleration().val[2];
+				double rx = lastImu_.angularVelocity().val[0];
+				double ry = lastImu_.angularVelocity().val[1];
+				double rz = lastImu_.angularVelocity().val[2];
+				Vector3d acc(dx, dy, dz);
+				Vector3d gyr(rx, ry, rz);
+				vinsEstimator_->inputIMU(lastImuStamp_, acc, gyr);
+			}
 		}
 
 		UDEBUG("Image update stamp=%f", data.stamp());
@@ -455,36 +440,45 @@ Transform OdometryVINS::computeTransform(
 					info->reg.covariance = cv::Mat::eye(6,6, CV_64FC1);
 					info->reg.covariance *= this->framesProcessed() == 0?9999:0.0001;
 
-					// feature map
-					Transform fixT = this->getPose()*previousPoseInv;
+					// feature map: based on code from pubPointCloud() of vins's visualization.cpp
 					for (auto &it_per_id : vinsEstimator_->f_manager.feature)
 					{
-						int used_num;
-						used_num = it_per_id.feature_per_frame.size();
-						if (!(used_num >= 2 && it_per_id.start_frame < WINDOW_SIZE - 2))
-							continue;
-						if (it_per_id.start_frame > WINDOW_SIZE * 3.0 / 4.0 || it_per_id.solve_flag != 1)
+						if(it_per_id.feature_per_frame.size() < 2) {
+							// feature just added but not tracked, or old feature not tracked anymore
 							continue;
+						}
+
 						int imu_i = it_per_id.start_frame;
-						Vector3d pts_i = it_per_id.feature_per_frame[it_per_id.feature_per_frame.size()-1].point * it_per_id.estimated_depth;
+						Vector3d pts_i = it_per_id.feature_per_frame[0].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;
 						p.x = w_pts_i(0);
 						p.y = w_pts_i(1);
 						p.z = w_pts_i(2);
-						p = util3d::transformPoint(p, fixT);
-						info->localMap.insert(std::make_pair(it_per_id.feature_id, p));
 
-						if(this->imagesAlreadyRectified())
-						{
-							cv::Point2f pt;
-							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);
+						int featureIndex = info->localMap.size();
+						info->localMap.insert(std::make_pair(featureIndex, p));
+
+						FeaturePerFrame & refFrame = it_per_id.feature_per_frame[0]; // First frame it was seen
+						FeaturePerFrame & newFrame = it_per_id.feature_per_frame[it_per_id.feature_per_frame.size()-1]; // Last frame it was seen (not necessary in last frame)
+						cv::Point2f refUV(refFrame.uv[0], refFrame.uv[1]);
+						cv::Point2f newUV(newFrame.uv[0], newFrame.uv[1]);
+						info->refCorners.push_back(refUV);
+						info->newCorners.push_back(newUV);
+
+						info->reg.matchesIDs.push_back(featureIndex);
+						if(it_per_id.solve_flag > 0) {
+							// Feature correctly tracked
+							info->words.insert(std::make_pair(featureIndex, cv::KeyPoint(newUV, 3.0f)));
+							info->cornerInliers.push_back(featureIndex);
+							info->reg.inliersIDs.push_back(featureIndex);
 						}
+
+						++featureIndex;
 					}
-					info->features = info->newCorners.size();
+					info->features = info->localMap.size();
+					info->reg.inliers = info->reg.inliersIDs.size();
 					info->localMapSize = info->localMap.size();
 				}
 				UINFO("Odom update time = %fs p=%s", timer.elapsed(), p.prettyPrint().c_str());
@@ -492,7 +486,7 @@ Transform OdometryVINS::computeTransform(
 		}
 		else
 		{
-			UWARN("VINS not yet initialized... waiting to get enough IMU messages");
+			UWARN("VINS-Fusion not yet initialized... needing more data.");
 		}
 	}
 	else if(!data.imageRaw().empty() && !data.depthRaw().empty())

guilib/include/rtabmap/gui/PreferencesDialog.h

@@ -367,7 +367,7 @@ private Q_SLOTS:
 	void changeDictionaryPath();
 	void changeOdometryORBSLAMVocabulary();
 	void changeOdometryOKVISConfigPath();
-	void changeOdometryVINSConfigPath();
+	void changeOdometryVINSFusionConfigPath();
 	void changeOdometryOpenVINSLeftMask();
 	void changeOdometryOpenVINSRightMask();
 	void changeIcpPMConfigPath();

guilib/src/AboutDialog.cpp

@@ -281,7 +281,7 @@ AboutDialog::AboutDialog(QWidget * parent) :
 	_ui->label_msckf_license->setEnabled(false);
 #endif
 
-#ifdef RTABMAP_VINS
+#ifdef RTABMAP_VINS_FUSION
 	_ui->label_vins_fusion->setText("Yes");
 	_ui->label_vins_fusion_license->setEnabled(true);
 #else

guilib/src/MainWindow.cpp

@@ -1675,7 +1675,7 @@ 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::kTypeVINSFusion ||
 					odom.info().type == (int)Odometry::kTypeOpenVINS)
 			{
 				std::vector<cv::KeyPoint> kpts;
@@ -1725,7 +1725,6 @@ 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::kTypeOpenVINS)
 			{
 				if(_ui->imageView_odometry->isFeaturesShown() && !_preferencesDialog->isOdomOnlyInliersShown())
@@ -1742,6 +1741,7 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI
 			}
 			if((odom.info().type == (int)Odometry::kTypeF2F ||
 				odom.info().type == (int)Odometry::kTypeViso2 ||
+				odom.info().type == (int)Odometry::kTypeVINSFusion ||
 				odom.info().type == (int)Odometry::kTypeFovis) && odom.info().refCorners.size())
 			{
 				if(_ui->imageView_odometry->isFeaturesShown() || _ui->imageView_odometry->isLinesShown())

guilib/src/PreferencesDialog.cpp

@@ -226,7 +226,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 #ifndef RTABMAP_MSCKF_VIO
 	_ui->odom_strategy->setItemData(8, 0, Qt::UserRole - 1);
 #endif
-#ifndef RTABMAP_VINS
+#ifndef RTABMAP_VINS_FUSION
 	_ui->odom_strategy->setItemData(9, 0, Qt::UserRole - 1);
 #endif
 #ifndef RTABMAP_OPENVINS
@@ -1556,8 +1556,8 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->OdomMSCKFInitCovExTrans->setObjectName(Parameters::kOdomMSCKFInitCovExTrans().c_str());
 
 	// Odometry VINS
-	_ui->lineEdit_OdomVinsPath->setObjectName(Parameters::kOdomVINSConfigPath().c_str());
-	connect(_ui->toolButton_OdomVinsPath, SIGNAL(clicked()), this, SLOT(changeOdometryVINSConfigPath()));
+	_ui->lineEdit_OdomVinsFusionPath->setObjectName(Parameters::kOdomVINSFusionConfigPath().c_str());
+	connect(_ui->toolButton_OdomVinsFusionPath, SIGNAL(clicked()), this, SLOT(changeOdometryVINSFusionConfigPath()));
 
 	// Odometry OpenVINS
 	_ui->checkBox_OdomOpenVINSUseStereo->setObjectName(Parameters::kOdomOpenVINSUseStereo().c_str());
@@ -5476,7 +5476,7 @@ void PreferencesDialog::updateOdometryStackedIndex(int index)
 	_ui->groupBox_odomOKVIS->setVisible(index==6);
 	_ui->groupBox_odomLOAM->setVisible(index==7);
 	_ui->groupBox_odomMSCKF->setVisible(index==8);
-	_ui->groupBox_odomVINS->setVisible(index==9);
+	_ui->groupBox_odomVINSFusion->setVisible(index==9);
 	_ui->groupBox_odomOpenVINS->setVisible(index==10);
 	_ui->groupBox_odomOpen3D->setVisible(index==12);
 }
@@ -5567,20 +5567,20 @@ void PreferencesDialog::changeOdometryOKVISConfigPath()
 	}
 }
 
-void PreferencesDialog::changeOdometryVINSConfigPath()
+void PreferencesDialog::changeOdometryVINSFusionConfigPath()
 {
 	QString path;
-	if(_ui->lineEdit_OdomVinsPath->text().isEmpty())
+	if(_ui->lineEdit_OdomVinsFusionPath->text().isEmpty())
 	{
 		path = QFileDialog::getOpenFileName(this, tr("VINS-Fusion Config"), this->getWorkingDirectory(), tr("VINS-Fusion config (*.yaml)"));
 	}
 	else
 	{
-		path = QFileDialog::getOpenFileName(this, tr("VINS-Fusion Config"), _ui->lineEdit_OdomVinsPath->text(), tr("VINS-Fusion config (*.yaml)"));
+		path = QFileDialog::getOpenFileName(this, tr("VINS-Fusion Config"), _ui->lineEdit_OdomVinsFusionPath->text(), tr("VINS-Fusion config (*.yaml)"));
 	}
 	if(!path.isEmpty())
 	{
-		_ui->lineEdit_OdomVinsPath->setText(path);
+		_ui->lineEdit_OdomVinsFusionPath->setText(path);
 	}
 }
 

guilib/src/ui/preferencesDialog.ui

@@ -19461,7 +19461,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                       <widget class="QWidget" name="page_79">
                        <layout class="QVBoxLayout" name="verticalLayout_139">
                         <item>
-                         <widget class="QGroupBox" name="groupBox_odomVINS">
+                         <widget class="QGroupBox" name="groupBox_odomVINSFusion">
                           <property name="title">
                            <string>VINS-Fusion</string>
                           </property>
@@ -19485,7 +19485,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                            <item>
                             <layout class="QGridLayout" name="gridLayout_107" columnstretch="0,0,1">
                              <item row="0" column="1">
-                              <widget class="QLineEdit" name="lineEdit_OdomVinsPath"/>
+                              <widget class="QLineEdit" name="lineEdit_OdomVinsFusionPath"/>
                              </item>
                              <item row="0" column="2">
                               <widget class="QLabel" name="label_526">
@@ -19501,7 +19501,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                               </widget>
                              </item>
                              <item row="0" column="0">
-                              <widget class="QToolButton" name="toolButton_OdomVinsPath">
+                              <widget class="QToolButton" name="toolButton_OdomVinsFusionPath">
                                <property name="text">
                                 <string>...</string>
                                </property>