diff --git a/corelib/include/rtabmap/core/odometry/OdometryOpenVINS.h b/corelib/include/rtabmap/core/odometry/OdometryOpenVINS.h
index 6f5dbd2fc1..ac4129a613 100644
--- a/corelib/include/rtabmap/core/odometry/OdometryOpenVINS.h
+++ b/corelib/include/rtabmap/core/odometry/OdometryOpenVINS.h
@@ -32,6 +32,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 namespace ov_msckf {
 class VioManager;
+struct VioManagerOptions;
 }
 
 namespace rtabmap {
@@ -52,11 +53,10 @@ class RTABMAP_CORE_EXPORT OdometryOpenVINS : public Odometry
 private:
 #ifdef RTABMAP_OPENVINS
 	std::unique_ptr<ov_msckf::VioManager> vioManager_;
+	std::unique_ptr<ov_msckf::VioManagerOptions> params_;
 	bool initGravity_;
 	Transform previousPose_;
-	Transform previousLocalTransform_;
-	Transform imuLocalTransform_;
-	std::map<double, IMU> imuBuffer_;
+	Transform imuLocalTransformInv_;
 #endif
 };
 
diff --git a/corelib/src/odometry/OdometryOpenVINS.cpp b/corelib/src/odometry/OdometryOpenVINS.cpp
index 25d7a96afe..f4ecea20cd 100644
--- a/corelib/src/odometry/OdometryOpenVINS.cpp
+++ b/corelib/src/odometry/OdometryOpenVINS.cpp
@@ -48,6 +48,64 @@ OdometryOpenVINS::OdometryOpenVINS(const ParametersMap & parameters) :
 	previousPose_(Transform::getIdentity())
 #endif
 {
+#ifdef RTABMAP_OPENVINS
+	ov_core::Printer::setPrintLevel("WARNING");
+	int enum_index;
+	params_ = std::make_unique<ov_msckf::VioManagerOptions>();
+	Parameters::parse(parameters, Parameters::kOdomOpenVINSUseStereo(), params_->use_stereo);
+	Parameters::parse(parameters, Parameters::kOdomOpenVINSUseKLT(), params_->use_klt);
+	Parameters::parse(parameters, Parameters::kOdomOpenVINSNumPts(), params_->num_pts);
+	Parameters::parse(parameters, Parameters::kOdomOpenVINSFastThreshold(), params_->fast_threshold);
+	Parameters::parse(parameters, Parameters::kOdomOpenVINSGridX(), params_->grid_x);
+	Parameters::parse(parameters, Parameters::kOdomOpenVINSGridY(), params_->grid_y);
+	Parameters::parse(parameters, Parameters::kOdomOpenVINSMinPxDist(), params_->min_px_dist);
+	Parameters::parse(parameters, Parameters::kOdomOpenVINSKNNRatio(), params_->knn_ratio);
+	Parameters::parse(parameters, Parameters::kOdomOpenVINSUseFEJ(), params_->state_options.do_fej);
+	Parameters::parse(parameters, Parameters::kOdomOpenVINSIntegration(), enum_index);
+	params_->state_options.integration_method = ov_msckf::StateOptions::IntegrationMethod(enum_index);
+	Parameters::parse(parameters, Parameters::kOdomOpenVINSMaxClones(), params_->state_options.max_clone_size);
+	Parameters::parse(parameters, Parameters::kOdomOpenVINSMaxSLAM(), params_->state_options.max_slam_features);
+	Parameters::parse(parameters, Parameters::kOdomOpenVINSMaxSLAMInUpdate(), params_->state_options.max_slam_in_update);
+	Parameters::parse(parameters, Parameters::kOdomOpenVINSMaxMSCKFInUpdate(), params_->state_options.max_msckf_in_update);
+	Parameters::parse(parameters, Parameters::kOdomOpenVINSFeatRepMSCKF(), enum_index);
+	params_->state_options.feat_rep_msckf = ov_type::LandmarkRepresentation::Representation(enum_index);
+	Parameters::parse(parameters, Parameters::kOdomOpenVINSFeatRepSLAM(), enum_index);
+	params_->state_options.feat_rep_slam = ov_type::LandmarkRepresentation::Representation(enum_index);
+	Parameters::parse(parameters, Parameters::kOdomOpenVINSDtSLAMDelay(), params_->dt_slam_delay);
+	Parameters::parse(parameters, Parameters::kOdomOpenVINSGravityMag(), params_->gravity_mag);
+	Parameters::parse(parameters, Parameters::kOdomOpenVINSInitWindowTime(), params_->init_options.init_window_time);
+	Parameters::parse(parameters, Parameters::kOdomOpenVINSInitIMUThresh(), params_->init_options.init_imu_thresh);
+	Parameters::parse(parameters, Parameters::kOdomOpenVINSInitMaxDisparity(), params_->init_options.init_max_disparity);
+	Parameters::parse(parameters, Parameters::kOdomOpenVINSInitMaxFeatures(), params_->init_options.init_max_features);
+	Parameters::parse(parameters, Parameters::kOdomOpenVINSTryZUPT(), params_->try_zupt);
+	Parameters::parse(parameters, Parameters::kOdomOpenVINSZUPTChi2Multiplier(), params_->zupt_options.chi2_multipler);
+	Parameters::parse(parameters, Parameters::kOdomOpenVINSZUPTMaxVelodicy(), params_->zupt_max_velocity);
+	Parameters::parse(parameters, Parameters::kOdomOpenVINSZUPTNoiseMultiplier(), params_->zupt_noise_multiplier);
+	Parameters::parse(parameters, Parameters::kOdomOpenVINSZUPTMaxDisparity(), params_->zupt_max_disparity);
+	Parameters::parse(parameters, Parameters::kOdomOpenVINSZUPTOnlyAtBeginning(), params_->zupt_only_at_beginning);
+	Parameters::parse(parameters, Parameters::kOdomOpenVINSAccelerometerNoiseDensity(), params_->imu_noises.sigma_a);
+	Parameters::parse(parameters, Parameters::kOdomOpenVINSAccelerometerRandomWalk(), params_->imu_noises.sigma_ab);
+	Parameters::parse(parameters, Parameters::kOdomOpenVINSGyroscopeNoiseDensity(), params_->imu_noises.sigma_w);
+	Parameters::parse(parameters, Parameters::kOdomOpenVINSGyroscopeRandomWalk(), params_->imu_noises.sigma_wb);
+	Parameters::parse(parameters, Parameters::kOdomOpenVINSUpMSCKFSigmaPx(), params_->msckf_options.sigma_pix);
+	Parameters::parse(parameters, Parameters::kOdomOpenVINSUpMSCKFChi2Multiplier(), params_->msckf_options.chi2_multipler);
+	Parameters::parse(parameters, Parameters::kOdomOpenVINSUpSLAMSigmaPx(), params_->slam_options.sigma_pix);
+	Parameters::parse(parameters, Parameters::kOdomOpenVINSUpSLAMChi2Multiplier(), params_->slam_options.chi2_multipler);
+	params_->vec_dw << 1, 0, 0, 1, 0, 1;
+	params_->vec_da << 1, 0, 0, 1, 0, 1;
+	params_->vec_tg << 0, 0, 0, 0, 0, 0, 0, 0, 0;
+	params_->q_ACCtoIMU << 0, 0, 0, 1;
+	params_->q_GYROtoIMU << 0, 0, 0, 1;
+	params_->use_aruco = false;
+	params_->num_opencv_threads = -1;
+	params_->histogram_method = ov_core::TrackBase::HistogramMethod::NONE;
+	params_->init_options.sigma_a = params_->imu_noises.sigma_a;
+	params_->init_options.sigma_ab = params_->imu_noises.sigma_ab;
+	params_->init_options.sigma_w = params_->imu_noises.sigma_w;
+	params_->init_options.sigma_wb = params_->imu_noises.sigma_wb;
+	params_->init_options.sigma_pix = params_->slam_options.sigma_pix;
+	params_->init_options.gravity_mag = params_->gravity_mag;
+#endif
 }
 
 void OdometryOpenVINS::reset(const Transform & initialPose)
@@ -58,8 +116,7 @@ void OdometryOpenVINS::reset(const Transform & initialPose)
 	{
 		vioManager_.reset();
 		previousPose_.setIdentity();
-		previousLocalTransform_.setNull();
-		imuBuffer_.clear();
+		imuLocalTransformInv_.setNull();
 	}
 	initGravity_ = false;
 #endif
@@ -73,369 +130,236 @@ Transform OdometryOpenVINS::computeTransform(
 {
 	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() && data.stereoCameraModels().size() == 1)
+	if(!vioManager_)
 	{
-		if(imuBuffer_.empty())
-		{
-			UWARN("Waiting IMU for initialization...");
-			return t;
-		}
-		if(!vioManager_)
-		{
-			UINFO("OpenVINS Initialization");
-
-			// intialize
-			ov_msckf::VioManagerOptions params;
+		if(!data.imu().empty())
+			imuLocalTransformInv_ = data.imu().localTransform().inverse();
 
-			// 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 = 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 == 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)
+		if(!data.imageRaw().empty() && !imuLocalTransformInv_.isNull())
+		{
+			Transform T_imu_left;
+			Eigen::VectorXd left_calib(8), right_calib(8);
+			if(!data.rightRaw().empty())
 			{
-				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 = params.init_options.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;
-
+				params_->state_options.num_cameras = params_->init_options.num_cameras = 2;
+				T_imu_left = imuLocalTransformInv_ * data.stereoCameraModels()[0].localTransform();
 
-			// CAMERA ======================================================================
-			bool fisheye = data.stereoCameraModels()[0].left().isFisheye() && !this->imagesAlreadyRectified();
-			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())});
-			}
+				bool is_fisheye = data.stereoCameraModels()[0].left().isFisheye() && !this->imagesAlreadyRectified();
+				if(is_fisheye)
+				{
+					params_->camera_intrinsics.emplace(0, std::make_shared<ov_core::CamEqui>(
+						data.stereoCameraModels()[0].left().imageWidth(), data.stereoCameraModels()[0].left().imageHeight()));
+					params_->camera_intrinsics.emplace(1, std::make_shared<ov_core::CamEqui>(
+						data.stereoCameraModels()[0].right().imageWidth(), data.stereoCameraModels()[0].right().imageHeight()));
+				}
+				else
+				{
+					params_->camera_intrinsics.emplace(0, std::make_shared<ov_core::CamRadtan>(
+						data.stereoCameraModels()[0].left().imageWidth(), data.stereoCameraModels()[0].left().imageHeight()));
+					params_->camera_intrinsics.emplace(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())
-			{
-				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;
+				if(this->imagesAlreadyRectified() || data.stereoCameraModels()[0].left().D_raw().empty())
+				{
+					left_calib << data.stereoCameraModels()[0].left().fx(),
+						data.stereoCameraModels()[0].left().fy(),
+					 	data.stereoCameraModels()[0].left().cx(),
+					 	data.stereoCameraModels()[0].left().cy(), 0, 0, 0, 0;
+					right_calib << data.stereoCameraModels()[0].right().fx(),
+						data.stereoCameraModels()[0].right().fy(),
+					 	data.stereoCameraModels()[0].right().cx(),
+					 	data.stereoCameraModels()[0].right().cy(), 0, 0, 0, 0;
+				}
+				else
+				{
+					UASSERT(data.stereoCameraModels()[0].left().D_raw().cols == data.stereoCameraModels()[0].right().D_raw().cols);
+					UASSERT(data.stereoCameraModels()[0].left().D_raw().cols >= 4);
+					UASSERT(data.stereoCameraModels()[0].right().D_raw().cols >= 4);
+					left_calib << 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,is_fisheye?4:2),
+					 	data.stereoCameraModels()[0].left().D_raw().at<double>(0,is_fisheye?5:3);
+					right_calib << 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,is_fisheye?4:2),
+					 	data.stereoCameraModels()[0].right().D_raw().at<double>(0,is_fisheye?5:3);
+				}
 			}
 			else
 			{
-				UASSERT(data.stereoCameraModels()[0].left().D_raw().cols == data.stereoCameraModels()[0].right().D_raw().cols);
-				UASSERT(data.stereoCameraModels()[0].left().D_raw().cols >= 4);
-				UASSERT(data.stereoCameraModels()[0].right().D_raw().cols >= 4);
+				params_->state_options.num_cameras = params_->init_options.num_cameras = 1;
+				T_imu_left = imuLocalTransformInv_ * data.cameraModels()[0].localTransform();
 
-				//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)
+				bool is_fisheye = data.cameraModels()[0].isFisheye() && !this->imagesAlreadyRectified();
+				if(is_fisheye)
+				{
+					params_->camera_intrinsics.emplace(0, std::make_shared<ov_core::CamEqui>(
+						data.cameraModels()[0].imageWidth(), data.cameraModels()[0].imageHeight()));
+				}
+				else
+				{
+					params_->camera_intrinsics.emplace(0, std::make_shared<ov_core::CamRadtan>(
+						data.cameraModels()[0].imageWidth(), data.cameraModels()[0].imageHeight()));
+				}
 
-				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);
+				if(this->imagesAlreadyRectified() || data.cameraModels()[0].D_raw().empty())
+				{
+					left_calib << data.cameraModels()[0].fx(),
+						data.cameraModels()[0].fy(),
+					 	data.cameraModels()[0].cx(),
+					 	data.cameraModels()[0].cy(), 0, 0, 0, 0;
+				}
+				else
+				{
+					UASSERT(data.cameraModels()[0].D_raw().cols >= 4);
+					left_calib << data.cameraModels()[0].K_raw().at<double>(0,0),
+					 	data.cameraModels()[0].K_raw().at<double>(1,1),
+					 	data.cameraModels()[0].K_raw().at<double>(0,2),
+					 	data.cameraModels()[0].K_raw().at<double>(1,2),
+					 	data.cameraModels()[0].D_raw().at<double>(0,0),
+					 	data.cameraModels()[0].D_raw().at<double>(0,1),
+					 	data.cameraModels()[0].D_raw().at<double>(0,is_fisheye?4:2),
+					 	data.cameraModels()[0].D_raw().at<double>(0,is_fisheye?5:3);
+				}
 			}
-			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();
-			Transform imuCam0 = imuLocalTransform_.inverse() * data.stereoCameraModels()[0].localTransform();
-			Transform cam0cam1;
-			if(this->imagesAlreadyRectified() || data.stereoCameraModels()[0].stereoTransform().isNull())
+			Eigen::Matrix4d T_LtoI = T_imu_left.toEigen4d();
+			Eigen::Matrix<double,7,1> left_eigen;
+			left_eigen.block(0,0,4,1) = ov_core::rot_2_quat(T_LtoI.block(0,0,3,3).transpose());
+			left_eigen.block(4,0,3,1) = -T_LtoI.block(0,0,3,3).transpose()*T_LtoI.block(0,3,3,1);
+			params_->camera_intrinsics.at(0)->set_value(left_calib);
+			params_->camera_extrinsics.emplace(0, left_eigen);
+			if(!data.rightRaw().empty())
 			{
-				cam0cam1 = Transform(
+				Transform T_left_right;
+				if(this->imagesAlreadyRectified() || data.stereoCameraModels()[0].stereoTransform().isNull())
+				{
+					T_left_right = Transform(
 						1, 0, 0, data.stereoCameraModels()[0].baseline(),
 						0, 1, 0, 0,
 						0, 0, 1, 0);
+				}
+				else
+				{
+					T_left_right = data.stereoCameraModels()[0].stereoTransform().inverse();
+				}
+				UASSERT(!T_left_right.isNull());
+				Transform T_imu_right = T_imu_left * T_left_right;
+				Eigen::Matrix4d T_RtoI = T_imu_right.toEigen4d();
+				Eigen::Matrix<double,7,1> right_eigen;
+				right_eigen.block(0,0,4,1) = ov_core::rot_2_quat(T_RtoI.block(0,0,3,3).transpose());
+				right_eigen.block(4,0,3,1) = -T_RtoI.block(0,0,3,3).transpose()*T_RtoI.block(0,3,3,1);
+				params_->camera_intrinsics.at(1)->set_value(right_calib);
+				params_->camera_extrinsics.emplace(1, right_eigen);
 			}
-			else
-			{
-				cam0cam1 = data.stereoCameraModels()[0].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) = 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) = 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));
-
-			vioManager_ = std::make_unique<ov_msckf::VioManager>(params);
+			params_->init_options.camera_intrinsics = params_->camera_intrinsics;
+			params_->init_options.camera_extrinsics = params_->camera_extrinsics;
+			vioManager_ = std::make_unique<ov_msckf::VioManager>(*params_);
 		}
-
-		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;
-		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);
-		message.masks.push_back(cv::Mat::zeros(left.size(), CV_8UC1));
-		message.masks.push_back(cv::Mat::zeros(right.size(), CV_8UC1));
-
-		// 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())
+	}
+	else
+	{
+		if(!data.imu().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
+			message.timestamp = data.stamp();
+			message.wm << data.imu().angularVelocity().val[0], data.imu().angularVelocity().val[1], data.imu().angularVelocity().val[2];
+			message.am << data.imu().linearAcceleration().val[0], data.imu().linearAcceleration().val[1], data.imu().linearAcceleration().val[2];
 			vioManager_->feed_measurement_imu(message);
+		}
 
-			lastIMUstamp = iter->first;
-
-			imuBuffer_.erase(iter);
-
-			if(lastIMUstamp > data.stamp())
+		if(!data.imageRaw().empty())
+		{
+			cv::Mat image;
+			if(data.imageRaw().type() == CV_8UC3)
+				cv::cvtColor(data.imageRaw(), image, CV_BGR2GRAY);
+			else if(data.imageRaw().type() == CV_8UC1)
+				image = data.imageRaw().clone();
+			else
+				UFATAL("Not supported color type!");
+			ov_core::CameraData message;
+			message.timestamp = data.stamp();
+			message.sensor_ids.emplace_back(0);
+			message.images.emplace_back(image);
+			message.masks.emplace_back(cv::Mat::zeros(image.size(), CV_8UC1));
+			if(!data.rightRaw().empty())
 			{
-				break;
+				if(data.rightRaw().type() == CV_8UC3)
+					cv::cvtColor(data.rightRaw(), image, CV_BGR2GRAY);
+				else if(data.rightRaw().type() == CV_8UC1)
+					image = data.rightRaw().clone();
+				else
+					UFATAL("Not supported color type!");
+				message.sensor_ids.emplace_back(1);
+				message.images.emplace_back(image);
+				message.masks.emplace_back(cv::Mat::zeros(image.size(), CV_8UC1));
 			}
+			vioManager_->feed_measurement_camera(message);
 		}
 
 		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);
-					}
-				}
-			}
-
+			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));
 			if(!p.isNull())
 			{
-				p =  p * imuLocalTransform_.inverse();
+				p = p * imuLocalTransformInv_;
 
 				if(this->getPose().rotation().isIdentity())
 				{
 					initGravity_ = true;
-					this->reset(this->getPose()*p.rotation());
+					this->reset(this->getPose() * p.rotation());
 				}
 
 				if(previousPose_.isIdentity())
-				{
 					previousPose_ = p;
-				}
 
-				// make it incremental
 				Transform previousPoseInv = previousPose_.inverse();
-				t = previousPoseInv*p;
+				t = previousPoseInv * p;
 				previousPose_ = p;
 
 				if(info)
 				{
 					info->type = this->getType();
-					info->reg.covariance = covariance;
+					info->reg.covariance = cv::Mat(6,6,CV_64FC1);
+					std::vector<std::shared_ptr<ov_type::Type>> statevars;
+					statevars.push_back(state->_imu->pose()->p());
+					statevars.push_back(state->_imu->pose()->q());
+					Eigen::Matrix<double,6,6> covariance_posori = ov_msckf::StateHelper::get_marginal_covariance(state, statevars);
+					for (int r = 0; r < 6; r++)
+					{
+						for (int c = 0; c < 6; c++)
+						{
+							((double *)info->reg.covariance.data)[6*r+c] = covariance_posori(r,c);
+						}
+					}
+
+					Transform fixT = this->getPose() * previousPoseInv;
+					Transform camLocalTransformInv;
+					if(!data.rightRaw().empty())
+						camLocalTransformInv = data.stereoCameraModels()[0].localTransform().inverse() * this->getPose().inverse();
+					else
+						camLocalTransformInv = data.cameraModels()[0].localTransform().inverse() * this->getPose().inverse();
 
-					// feature map
-					Transform fixT = this->getPose()*previousPoseInv;
-					Transform camLocalTransformInv = data.stereoCameraModels()[0].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];
+						cv::Point3f pt3d(it_per_id[0], it_per_id[1], it_per_id[2]);
 						pt3d = util3d::transformPoint(pt3d, fixT);
 						info->localMap.insert(std::make_pair(info->localMap.size(), pt3d));
 
@@ -443,7 +367,10 @@ Transform OdometryOpenVINS::computeTransform(
 						{
 							cv::Point2f pt;
 							pt3d = util3d::transformPoint(pt3d, camLocalTransformInv);
-							data.stereoCameraModels()[0].left().reproject(pt3d.x, pt3d.y, pt3d.z, pt.x, pt.y);
+							if(!data.rightRaw().empty())
+								data.stereoCameraModels()[0].left().reproject(pt3d.x, pt3d.y, pt3d.z, pt.x, pt.y);
+							else
+								data.cameraModels()[0].reproject(pt3d.x, pt3d.y, pt3d.z, pt.x, pt.y);
 							info->reg.inliersIDs.push_back(info->newCorners.size());
 							info->newCorners.push_back(pt);
 						}
@@ -451,22 +378,9 @@ Transform OdometryOpenVINS::computeTransform(
 					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("OpenVINS requires stereo images (only one stereo camera and should be calibrated)!");
-	}
 
 #else
 	UERROR("RTAB-Map is not built with OpenVINS support! Select another visual odometry approach.");