Merge branch 'master' of github.com:introlab/rtabmap into gtest

matlabbe · matlabbe · commit 7c2fbb87435b · 2026-06-13T17:02:28.000-07:00
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -872,22 +872,7 @@ IF(WITH_VINS_FUSION)
 ENDIF(WITH_VINS_FUSION)
 
 IF(WITH_OPENVINS)
-    FIND_PACKAGE(ov_msckf)
-    # On ROS2, the indirect includes and libraries
-    # are not forwarded by ov_msckf target, append them manually
-    FIND_PACKAGE(ov_core)
-    FIND_PACKAGE(ov_init)
-    IF(ov_msckf_FOUND AND ov_core_FOUND AND ov_init_FOUND)
-      SET(ov_msckf_INCLUDE_DIRS
-         ${ov_msckf_INCLUDE_DIRS}
-         ${ov_core_INCLUDE_DIRS}
-         ${ov_init_INCLUDE_DIRS})
-      SET(ov_msckf_LIBRARIES
-         ${ov_msckf_LIBRARIES}
-         ${ov_core_LIBRARIES}
-         ${ov_init_LIBRARIES})
-      MESSAGE(STATUS "Found OpenVINS: ${ov_msckf_INCLUDE_DIRS}")
-    ENDIF()
+   FIND_PACKAGE(OpenVINS)
 ENDIF(WITH_OPENVINS)
 
 IF(WITH_FASTCV)
@@ -1253,7 +1238,7 @@ ENDIF()
 IF(NOT vins_FOUND)
    SET(VINSFUSION "//")
 ENDIF()
-IF(NOT ov_msckf_FOUND)
+IF(NOT OpenVINS_FOUND)
    SET(OPENVINS "//")
 ENDIF()
 IF(NOT CUVSLAM_FOUND)
@@ -2013,7 +1998,7 @@ ELSE()
 MESSAGE(STATUS "  With VINS-Fusion          = NO (VINS-Fusion not found)")
 ENDIF()
 
-IF(ov_msckf_FOUND)
+IF(OpenVINS_FOUND)
 MESSAGE(STATUS "  With OpenVINS             = YES (License: GPLv3)")
 ELSEIF(NOT WITH_OPENVINS)
 MESSAGE(STATUS "  With OpenVINS             = NO (WITH_OPENVINS=OFF)")
diff --git a/cmake_modules/FindOpenVINS.cmake b/cmake_modules/FindOpenVINS.cmake
@@ -0,0 +1,44 @@
+# Find OpenVINS
+# 
+#   We search for a vins installation in ROS/ROS2 first, then fallback on 
+#   ros-free library in common install paths
+
+FIND_PACKAGE(ov_msckf QUIET)
+IF(ov_msckf_FOUND)
+   # On ROS2, the indirect includes and libraries
+   # are not forwarded by ov_msckf target, append them manually
+   FIND_PACKAGE(ov_core)
+   FIND_PACKAGE(ov_init)
+   IF(ov_msckf_FOUND AND ov_core_FOUND AND ov_init_FOUND)
+      SET(OpenVINS_FOUND TRUE)
+      SET(OpenVINS_INCLUDE_DIRS
+         ${ov_msckf_INCLUDE_DIRS}
+         ${ov_core_INCLUDE_DIRS}
+         ${ov_init_INCLUDE_DIRS})
+      SET(OpenVINS_LIBRARIES
+         ${ov_msckf_LIBRARIES}
+         ${ov_core_LIBRARIES}
+         ${ov_init_LIBRARIES})
+   ENDIF()
+ELSE()
+   find_path(OpenVINS_INCLUDE_DIR NAMES core/VioManager.h PATH_SUFFIXES open_vins)
+   find_library(OpenVINS_LIBRARY NAMES ov_msckf_lib)
+   IF (OpenVINS_INCLUDE_DIR AND OpenVINS_LIBRARY)
+      SET(OpenVINS_FOUND TRUE)
+      SET(OpenVINS_INCLUDE_DIRS ${OpenVINS_INCLUDE_DIR})
+      SET(OpenVINS_LIBRARIES ${OpenVINS_LIBRARY})
+   ENDIF()
+ENDIF()
+
+IF (OpenVINS_FOUND)
+   # show which OpenVINS was found only if not quiet
+   IF (NOT OpenVINS_FIND_QUIETLY)
+      MESSAGE(STATUS "Found OpenVINS: ${OpenVINS_LIBRARIES} ${OpenVINS_INCLUDE_DIRS}")
+   ENDIF (NOT OpenVINS_FIND_QUIETLY)
+ELSE (OpenVINS_FOUND)
+   # fatal error if OpenVINS is required but not found
+   IF (OpenVINS_FIND_REQUIRED)
+      MESSAGE(FATAL_ERROR "Could not find OpenVINS")
+   ENDIF (OpenVINS_FIND_REQUIRED)
+ENDIF (OpenVINS_FOUND)
+
diff --git a/corelib/include/rtabmap/core/Parameters.h b/corelib/include/rtabmap/core/Parameters.h
diff --git a/corelib/src/CMakeLists.txt b/corelib/src/CMakeLists.txt
@@ -781,16 +781,16 @@ IF(vins_FOUND)
 	)
 ENDIF(vins_FOUND)
 
-IF(ov_msckf_FOUND)
+IF(OpenVINS_FOUND)
     SET(INCLUDE_DIRS
-		${ov_msckf_INCLUDE_DIRS}
+		${OpenVINS_INCLUDE_DIRS}
 		${INCLUDE_DIRS}
 	)
 	SET(LIBRARIES
-		${ov_msckf_LIBRARIES}
+		${OpenVINS_LIBRARIES}
 		${LIBRARIES}
 	)
-ENDIF(ov_msckf_FOUND)
+ENDIF(OpenVINS_FOUND)
 
 IF(ORB_SLAM_FOUND)
     SET(INCLUDE_DIRS
diff --git a/corelib/src/odometry/OdometryOpenVINS.cpp b/corelib/src/odometry/OdometryOpenVINS.cpp
@@ -152,6 +152,85 @@ OdometryOpenVINS::OdometryOpenVINS(const ParametersMap & parameters) :
 	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;
+
+	if(parameters.find(Parameters::kOdomOpenVINSConfigPath()) != parameters.end())
+	{
+		// Load the config: will override all parameters above!
+		std::string configPath = parameters.at(Parameters::kOdomOpenVINSConfigPath());
+		if(!configPath.empty())
+		{
+			if(UFile::exists(configPath))
+			{
+				UWARN("OpenVINS config file is provided (%s=\"%s\"), reading it. The parameters from the config file will overwrite OdomOpenVINS/*** parameters.",
+					Parameters::kOdomOpenVINSConfigPath().c_str(), configPath.c_str());
+				auto parser = std::make_shared<ov_core::YamlParser>(configPath);
+
+				// The sequence of loading is based on VioManagerOptions::print_and_load()
+				// We removed all parts about intrinsics/extrinsics, which will be loaded later
+				// when we receive the data (which should already include intrinsics and extrinsics).
+
+				params_->state_options.print(parser);
+				
+				params_->init_options.print_and_load_initializer(parser);
+    			params_->init_options.print_and_load_noise(parser);
+				parser->parse_config("gravity_mag", params_->init_options.gravity_mag);
+				parser->parse_config("max_cameras", params_->init_options.num_cameras);
+				parser->parse_config("use_stereo", params_->init_options.use_stereo);
+				parser->parse_config("downsample_cameras", params_->init_options.downsample_cameras);
+
+				parser->parse_config("dt_slam_delay", params_->dt_slam_delay);
+				parser->parse_config("try_zupt", params_->try_zupt);
+				parser->parse_config("zupt_max_velocity",params_-> zupt_max_velocity);
+				parser->parse_config("zupt_noise_multiplier", params_->zupt_noise_multiplier);
+				parser->parse_config("zupt_max_disparity", params_->zupt_max_disparity);
+				parser->parse_config("zupt_only_at_beginning", params_->zupt_only_at_beginning);
+				parser->parse_config("record_timing_information", params_->record_timing_information);
+				parser->parse_config("record_timing_filepath", params_->record_timing_filepath);
+
+				params_->print_and_load_trackers(parser);
+				params_->print_and_load_noise(parser);
+
+				if(params_->state_options.num_cameras > 2)
+				{
+					UFATAL("OpenVINS integration in RTAB-Map doesn't support more than 2 cameras (num_cameras=%d).", params_->state_options.num_cameras);
+				}
+
+				parser->parse_config("gravity_mag", params_->gravity_mag);
+				parser->parse_config("use_mask", params_->use_mask);
+				params_->masks.clear();
+				if (params_->use_mask) {
+					for (int i = 0; i < params_->state_options.num_cameras; i++) {
+						std::string mask_path;
+						std::string mask_node = "mask" + std::to_string(i);
+						parser->parse_config(mask_node, mask_path);
+						std::string total_mask_path = parser->get_config_folder() + mask_path;
+						if (!boost::filesystem::exists(total_mask_path)) {
+							PRINT_ERROR(RED "VioManager(): invalid mask path:\n" RESET);
+							PRINT_ERROR(RED "\t- mask%d - %s\n" RESET, i, total_mask_path.c_str());
+							std::exit(EXIT_FAILURE);
+						}
+						params_->masks.emplace(i, cv::imread(total_mask_path, cv::IMREAD_GRAYSCALE));
+					}
+				}
+
+				if (!parser->successful()) {
+					UWARN("Not all expected OpenVINS parameters were read successfully "
+						"from \"%s\". Values from RTAB-Map's OpenOpenVINS/* parameters "
+						"will be used instead for the missing ones.",
+						configPath.c_str());
+				}
+				else {
+					UINFO("OpenVINS config file(%s=\"%s\") read.",
+					Parameters::kOdomOpenVINSConfigPath().c_str(), configPath.c_str());
+				}
+			}
+			else
+			{
+				UERROR("OpenVINS config file is provided (%s=\"%s\") but it doesn't exist!",
+					Parameters::kOdomOpenVINSConfigPath().c_str(), configPath.c_str());
+			}
+		}
+	}
 #endif
 }
 
diff --git a/guilib/include/rtabmap/gui/PreferencesDialog.h b/guilib/include/rtabmap/gui/PreferencesDialog.h
@@ -372,6 +372,7 @@ private Q_SLOTS:
 	void changeOdometryORBSLAMVocabulary();
 	void changeOdometryOKVISConfigPath();
 	void changeOdometryVINSFusionConfigPath();
+	void changeOdometryOpenVINSConfigPath();
 	void changeOdometryLIOSAMConfigPath();
 	void changeOdometryOpenVINSLeftMask();
 	void changeOdometryOpenVINSRightMask();
diff --git a/guilib/src/PreferencesDialog.cpp b/guilib/src/PreferencesDialog.cpp
@@ -1619,6 +1619,8 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	connect(_ui->toolButton_OdomVinsFusionPath, SIGNAL(clicked()), this, SLOT(changeOdometryVINSFusionConfigPath()));
 
 	// Odometry OpenVINS
+	_ui->lineEdit_openvinsConfigPath->setObjectName(Parameters::kOdomOpenVINSConfigPath().c_str());
+	connect(_ui->toolButton_openvinsConfigPath, SIGNAL(clicked()), this, SLOT(changeOdometryOpenVINSConfigPath()));
 	_ui->checkBox_OdomOpenVINSUseStereo->setObjectName(Parameters::kOdomOpenVINSUseStereo().c_str());
 	_ui->checkBox_OdomOpenVINSUseKLT->setObjectName(Parameters::kOdomOpenVINSUseKLT().c_str());
 	_ui->spinBox_OdomOpenVINSNumPts->setObjectName(Parameters::kOdomOpenVINSNumPts().c_str());
@@ -5794,6 +5796,23 @@ void PreferencesDialog::changeOdometryVINSFusionConfigPath()
 	}
 }
 
+void PreferencesDialog::changeOdometryOpenVINSConfigPath()
+{
+	QString path;
+	if(_ui->lineEdit_openvinsConfigPath->text().isEmpty())
+	{
+		path = QFileDialog::getOpenFileName(this, tr("OpenVINS Config"), this->getWorkingDirectory(), tr("OpenVINS config (*.yaml)"));
+	}
+	else
+	{
+		path = QFileDialog::getOpenFileName(this, tr("OpenVINS Config"), _ui->lineEdit_openvinsConfigPath->text(), tr("OpenVINS config (*.yaml)"));
+	}
+	if(!path.isEmpty())
+	{
+		_ui->lineEdit_openvinsConfigPath->setText(path);
+	}
+}
+
 void PreferencesDialog::changeOdometryLIOSAMConfigPath()
 {
 	QString path;
diff --git a/guilib/src/ui/preferencesDialog.ui b/guilib/src/ui/preferencesDialog.ui

Original file line number	Diff line number	Diff line change
`@@ -781,16 +781,16 @@ IF(vins_FOUND)`
`781`	`781`	`)`
`782`	`782`	`ENDIF(vins_FOUND)`
`783`	`783`
`784`		`-IF(ov_msckf_FOUND)`
	`784`	`+IF(OpenVINS_FOUND)`
`785`	`785`	`SET(INCLUDE_DIRS`
`786`		`- ${ov_msckf_INCLUDE_DIRS}`
	`786`	`+ ${OpenVINS_INCLUDE_DIRS}`
`787`	`787`	`${INCLUDE_DIRS}`
`788`	`788`	`)`
`789`	`789`	`SET(LIBRARIES`
`790`		`- ${ov_msckf_LIBRARIES}`
	`790`	`+ ${OpenVINS_LIBRARIES}`
`791`	`791`	`${LIBRARIES}`
`792`	`792`	`)`
`793`		`-ENDIF(ov_msckf_FOUND)`
	`793`	`+ENDIF(OpenVINS_FOUND)`
`794`	`794`
`795`	`795`	`IF(ORB_SLAM_FOUND)`
`796`	`796`	`SET(INCLUDE_DIRS`