rpng · mzahana · May 20, 2024
diff --git a/ov_plane/CMakeLists.txt b/ov_plane/CMakeLists.txt
@@ -143,6 +143,14 @@ install(TARGETS timing_custom
         RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
 )
 
+add_executable(run_subscribe_msckf src/run_subscribe_msckf.cpp)
+target_link_libraries(run_subscribe_msckf ov_plane_lib ${thirdparty_libraries})
+install(TARGETS run_subscribe_msckf
+        ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+        LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+        RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
 ##################################################
 # Launch files!
 ##################################################

diff --git a/ov_plane/launch/subscribe.launch b/ov_plane/launch/subscribe.launch
@@ -0,0 +1,89 @@
+<launch>
+    <!-- what config we are going to run (should match folder name) -->
+    <arg name="verbosity"   default="INFO" /> <!-- ALL, DEBUG, INFO, WARNING, ERROR, SILENT -->
+    <arg name="config"      default="rpng_plane" /> <!-- euroc_mav, rpng_plane -->
+    <arg name="config_path" default="$(find ov_plane)/../config/$(arg config)/estimator_config.yaml" />
+    <arg name="object"      default="$(find ov_plane)/../data/resources/teapot.obj" />
+
+    <!-- mono or stereo and what ros bag to play -->
+    <arg name="max_cameras" default="1" />
+    <arg name="use_stereo"  default="true" />
+    <arg name="bag_start"   default="0" />
+    <!-- V1_01_easy, V1_02_medium, V1_03_difficult, V2_01_easy, V2_02_medium, V2_03_difficult -->
+    <!-- table_01, table_02, table_03 -->
+    <arg name="dataset"     default="table_02" />
+    <arg name="bag"         default="/home/patrick/datasets/$(arg config)/$(arg dataset).bag" />
+    <!-- <arg name="bag"         default="/media/patrick/RPNG_DATA_6/$(arg config)/$(arg dataset).bag" /> -->
+
+    <!-- saving trajectory path and timing information -->
+    <arg name="dosave"      default="true" />
+    <arg name="dotime"      default="true" />
+    <arg name="dotrackinfo" default="true" />
+    <arg name="path_est"    default="/tmp/traj_estimate.txt" />
+    <arg name="path_time"   default="/tmp/traj_timing.txt" />
+    <arg name="path_track"  default="/tmp/traj_tracking.txt" />
+
+    <!-- if we should viz the groundtruth -->
+    <arg name="dolivetraj"  default="true" />
+    <arg name="path_gt"     default="$(find ov_data)/$(arg config)/$(arg dataset).txt" />
+
+    <!-- what plane constraints to apply -->
+    <arg name="use_plane_constraint"        default="true" />
+    <arg name="use_plane_constraint_msckf"  default="true" />
+    <arg name="use_plane_constraint_slamu"  default="true" />
+    <arg name="use_plane_constraint_slamd"  default="true" />
+
+    <!-- parameters -->
+    <arg name="use_plane_slam_feats"  default="true" />
+    <arg name="use_refine_plane_feat" default="true" />
+
+    <!-- noises -->
+    <arg name="sigma_constraint"      default="0.010" />
+    <arg name="const_init_multi"      default="1.0" />
+    <arg name="const_init_chi2"       default="1.0" />
+    <arg name="max_msckf_plane"       default="20" />
+
+    <!-- tracking stuff -->
+    <arg name="num_slam"              default="15" />
+    <arg name="num_pts"               default="200" />
+
+    <!-- MASTER NODE! -->
+<!--    <node name="ov_plane" pkg="ov_plane" type="ros1_serial_msckf" output="screen" clear_params="true" required="true" launch-prefix="gdb -ex run &#45;&#45;args">-->
+    <node name="ov_plane" pkg="ov_plane" type="run_subscribe_msckf" output="screen" clear_params="true" required="true">
+
+        <!-- master configuration object -->
+        <param name="verbosity"                 type="str"    value="$(arg verbosity)" />
+        <param name="config_path"               type="str"    value="$(arg config_path)" />
+
+        <!-- world/filter parameters -->
+        <param name="num_pts"                   type="int"    value="$(arg num_pts)" />
+        <param name="use_stereo"                type="bool"   value="$(arg use_stereo)" />
+        <param name="max_cameras"               type="int"    value="$(arg max_cameras)" />
+        <param name="max_slam"                  type="int"    value="$(arg num_slam)" />
+
+        <param name="use_plane_constraint"         type="bool"   value="$(arg use_plane_constraint)" />
+        <param name="use_plane_constraint_msckf"   type="bool"   value="$(arg use_plane_constraint_msckf)" />
+        <param name="use_plane_constraint_slamu"   type="bool"   value="$(arg use_plane_constraint_slamu)" />
+        <param name="use_plane_constraint_slamd"   type="bool"   value="$(arg use_plane_constraint_slamd)" />
+
+        <param name="use_plane_slam_feats"   type="bool"   value="$(arg use_plane_slam_feats)" />
+        <param name="use_refine_plane_feat"  type="bool"   value="$(arg use_refine_plane_feat)" />
+
+        <param name="sigma_constraint"       type="double" value="$(arg sigma_constraint)" />
+        <param name="const_init_multi"       type="double" value="$(arg const_init_multi)" />
+        <param name="const_init_chi2"        type="double" value="$(arg const_init_chi2)" />
+        <param name="max_msckf_plane"        type="int"    value="$(arg max_msckf_plane)" />
+
+        <!-- timing statistics recording -->
+        <param name="record_timing_information" type="bool"   value="$(arg dotime)" />
+        <param name="record_timing_filepath"    type="str"    value="$(arg path_time)" />
+
+        <!-- plane tracking statistics recording -->
+        <param name="record_plane_tracking_information" type="bool"   value="$(arg dotrackinfo)" />
+        <param name="record_plane_tracking_filepath"    type="str"    value="$(arg path_track)" />
+
+        <!-- AR display object file -->
+        <param name="path_object"              type="str"   value="$(arg object)" />
+
+    </node>
+</launch>
diff --git a/ov_plane/src/run_subscribe_msckf.cpp b/ov_plane/src/run_subscribe_msckf.cpp
@@ -0,0 +1,125 @@
+/*
+ * OpenVINS: An Open Platform for Visual-Inertial Research
+ * Copyright (C) 2018-2023 Patrick Geneva
+ * Copyright (C) 2018-2023 Guoquan Huang
+ * Copyright (C) 2018-2023 OpenVINS Contributors
+ * Copyright (C) 2018-2019 Kevin Eckenhoff
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+#include <memory>
+
+#include "core/VioManager.h"
+#include "core/VioManagerOptions.h"
+#include "utils/dataset_reader.h"
+
+#if ROS_AVAILABLE == 1
+#include "ros/ROS1Visualizer.h"
+#include <ros/ros.h>
+#elif ROS_AVAILABLE == 2
+#include "ros/ROS2Visualizer.h"
+#include <rclcpp/rclcpp.hpp>
+#endif
+
+using namespace ov_plane;
+
+std::shared_ptr<VioManager> sys;
+#if ROS_AVAILABLE == 1
+std::shared_ptr<ROS1Visualizer> viz;
+#elif ROS_AVAILABLE == 2
+std::shared_ptr<ROS2Visualizer> viz;
+#endif
+
+// Main function
+int main(int argc, char **argv) {
+
+  // Ensure we have a path, if the user passes it then we should use it
+  std::string config_path = "unset_path_to_config.yaml";
+  if (argc > 1) {
+    config_path = argv[1];
+  }
+
+#if ROS_AVAILABLE == 1
+  // Launch our ros node
+  ros::init(argc, argv, "run_subscribe_msckf");
+  auto nh = std::make_shared<ros::NodeHandle>("~");
+  nh->param<std::string>("config_path", config_path, config_path);
+#elif ROS_AVAILABLE == 2
+  // Launch our ros node
+  rclcpp::init(argc, argv);
+  rclcpp::NodeOptions options;
+  options.allow_undeclared_parameters(true);
+  options.automatically_declare_parameters_from_overrides(true);
+  auto node = std::make_shared<rclcpp::Node>("run_subscribe_msckf", options);
+  node->get_parameter<std::string>("config_path", config_path);
+#endif
+
+  // Load the config
+  auto parser = std::make_shared<ov_core::YamlParser>(config_path);
+#if ROS_AVAILABLE == 1
+  parser->set_node_handler(nh);
+#elif ROS_AVAILABLE == 2
+  parser->set_node(node);
+#endif
+
+  // Verbosity
+  std::string verbosity = "DEBUG";
+  parser->parse_config("verbosity", verbosity);
+  ov_core::Printer::setPrintLevel(verbosity);
+
+  // Create our VIO system
+  VioManagerOptions params;
+  params.print_and_load(parser);
+  params.use_multi_threading_subs = true;
+  sys = std::make_shared<VioManager>(params);
+#if ROS_AVAILABLE == 1
+  viz = std::make_shared<ROS1Visualizer>(nh, sys);
+  viz->setup_subscribers(parser);
+#elif ROS_AVAILABLE == 2
+  viz = std::make_shared<ROS2Visualizer>(node, sys);
+  viz->setup_subscribers(parser);
+#endif
+
+  // Ensure we read in all parameters required
+  if (!parser->successful()) {
+    PRINT_ERROR(RED "unable to parse all parameters, please fix\n" RESET);
+    std::exit(EXIT_FAILURE);
+  }
+
+  // Spin off to ROS
+  PRINT_DEBUG("done...spinning to ros\n");
+#if ROS_AVAILABLE == 1
+  // ros::spin();
+  ros::AsyncSpinner spinner(0);
+  spinner.start();
+  ros::waitForShutdown();
+#elif ROS_AVAILABLE == 2
+  // rclcpp::spin(node);
+  rclcpp::executors::MultiThreadedExecutor executor;
+  executor.add_node(node);
+  executor.spin();
+#endif
+
+  // Final visualization
+  viz->visualize_final();
+#if ROS_AVAILABLE == 1
+  ros::shutdown();
+#elif ROS_AVAILABLE == 2
+  rclcpp::shutdown();
+#endif
+
+  // Done!
+  return EXIT_SUCCESS;
+}