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