Feat/bias feature

config/aria/estimator_config.yaml

@@ -0,0 +1,123 @@
+%YAML:1.0 # need to specify the file type at the top!
+
+verbosity: "ERROR" # ALL, DEBUG, INFO, WARNING, ERROR, SILENT
+
+use_fej: true # if first-estimate Jacobians should be used (enable for good consistency)
+integration: "rk4" # discrete, rk4, analytical (if rk4 or analytical used then analytical covariance propagation is used)
+use_stereo: false # if we have more than 1 camera, if we should try to track stereo constraints
+max_cameras: 2 # how many cameras we have 1 = mono, 2 = stereo, >2 = binocular (all mono tracking)
+
+calib_cam_extrinsics: false # degenerate motion
+calib_cam_intrinsics: true
+calib_cam_timeoffset: true # degenerate motion
+calib_imu_intrinsics: false
+calib_imu_g_sensitivity: false
+
+max_clones: 11
+max_slam: 50
+max_slam_in_update: 25
+max_msckf_in_update: 50
+dt_slam_delay: 1
+
+gravity_mag: 9.81
+
+feat_rep_msckf: "ANCHORED_MSCKF_INVERSE_DEPTH"
+feat_rep_slam: "ANCHORED_MSCKF_INVERSE_DEPTH"
+feat_rep_aruco: "ANCHORED_MSCKF_INVERSE_DEPTH"
+
+# zero velocity update parameters we can use
+# we support either IMU-based or disparity detection.
+try_zupt: true
+zupt_chi2_multipler: 0.5 # set to 0 for only disp-based
+zupt_max_velocity: 0.1
+zupt_noise_multiplier: 1
+zupt_max_disparity: 0.4 # set to 0 for only imu-based
+zupt_only_at_beginning: false
+
+# ==================================================================
+# ==================================================================
+
+init_window_time: 2.0
+init_imu_thresh: 0.5
+init_max_disparity: 1.5
+init_max_features: 50
+
+init_dyn_use: true
+init_dyn_mle_opt_calib: false
+init_dyn_mle_max_iter: 50
+init_dyn_mle_max_time: 0.05
+init_dyn_mle_max_threads: 6
+init_dyn_num_pose: 6
+init_dyn_min_deg: 5.0 # traj is mostly straight line motion
+
+init_dyn_inflation_ori: 10
+init_dyn_inflation_vel: 100
+init_dyn_inflation_bg: 10
+init_dyn_inflation_ba: 100
+init_dyn_min_rec_cond: 1e-20 # traj is mostly straight line motion
+
+init_dyn_bias_g: [0.0, 0.0, 0.0]
+init_dyn_bias_a: [0.0, 0.0, 0.0]
+
+
+# ==================================================================
+# ==================================================================
+
+record_timing_information: false
+record_timing_filepath: "/tmp/traj_timing.txt"
+
+save_total_state: false
+filepath_est: "/tmp/ov_estimate.txt"
+filepath_std: "/tmp/ov_estimate_std.txt"
+filepath_gt: "/tmp/ov_groundtruth.txt"
+
+# ==================================================================
+# ==================================================================
+
+# our front-end feature tracking parameters
+# we have a KLT and descriptor based (KLT is better implemented...)
+use_klt: true
+num_pts: 200
+fast_threshold: 30
+grid_x: 5
+grid_y: 5
+min_px_dist: 20
+knn_ratio: 0.65
+track_frequency: 31.0
+downsample_cameras: false
+num_opencv_threads: 4 # -1: auto, 0-1: serial, >1: number of threads
+histogram_method: "HISTOGRAM" # NONE, HISTOGRAM, CLAHE
+
+fi_min_dist: 0.25
+fi_max_dist: 150.0
+fi_max_baseline: 200
+fi_max_cond_number: 20000
+fi_triangulate_1d: false
+
+# aruco tag tracker for the system
+# DICT_6X6_1000 from https://chev.me/arucogen/
+use_aruco: false
+num_aruco: 1024
+downsize_aruco: true
+
+# ==================================================================
+# ==================================================================
+
+# camera noises and chi-squared threshold multipliers
+up_msckf_sigma_px: 1.5
+up_msckf_chi2_multipler: 1
+up_slam_sigma_px: 1.5
+up_slam_chi2_multipler: 1
+up_aruco_sigma_px: 1
+up_aruco_chi2_multipler: 1
+
+# masks for our images
+use_mask: false
+
+# imu and camera spacial-temporal
+# imu config should also have the correct noise values
+relative_config_imu: "kalibr_imu_chain.yaml"
+relative_config_imucam: "kalibr_imucam_chain.yaml"
+
+
+

config/aria/kalibr_imu_chain.yaml

@@ -0,0 +1,37 @@
+%YAML:1.0
+
+imu0:
+  T_i_b:
+    - [1.0, 0.0, 0.0, 0.0]
+    - [0.0, 1.0, 0.0, 0.0]
+    - [0.0, 0.0, 1.0, 0.0]
+    - [0.0, 0.0, 0.0, 1.0]
+  accelerometer_noise_density: 0.010843559005996556
+  accelerometer_random_walk: 48.7810758784797e-05
+  gyroscope_noise_density: 0.002509873909262778
+  gyroscope_random_walk: 22.7931834950152e-06
+  model: calibrated
+  rostopic: /imu_right
+  time_offset: 0.0
+  update_rate: 1000
+
+  Tw:
+    - [ 1.0, 0.0, 0.0 ]
+    - [ 0.0, 1.0, 0.0 ]
+    - [ 0.0, 0.0, 1.0 ]
+  R_IMUtoGYRO:
+    - [ 1.0, 0.0, 0.0 ]
+    - [ 0.0, 1.0, 0.0 ]
+    - [ 0.0, 0.0, 1.0 ]
+  Ta:
+    - [ 1.0, 0.0, 0.0 ]
+    - [ 0.0, 1.0, 0.0 ]
+    - [ 0.0, 0.0, 1.0 ]
+  R_IMUtoACC:
+    - [ 1.0, 0.0, 0.0 ]
+    - [ 0.0, 1.0, 0.0 ]
+    - [ 0.0, 0.0, 1.0 ]
+  Tg:
+    - [ 0.0, 0.0, 0.0 ]
+    - [ 0.0, 0.0, 0.0 ]
+    - [ 0.0, 0.0, 0.0 ]

config/aria/kalibr_imucam_chain.yaml

@@ -0,0 +1,49 @@
+%YAML:1.0
+# cam0: camera_slam_left
+# cam1: camera_slam_right
+# cam2: camera_rgb  # This topic should really be called /camera_rgb_righted/image, as the image
+                    # information (and the extrinsics here) are for images that have been righted
+                    # (i.e. rotated by 90 degrees from the Aria glass' raw image)
+
+cam0:
+  T_cam_imu:
+    - [0.9782501693075571, 0.05018607300803547, -0.2012659045287295, 0.006435676135584448]
+    - [0.20742334480631525, -0.22979728423453438, 0.950877891312966, -0.10326121731212001]
+    - [0.0014704690054501979, -0.9719437052805396, -0.23520899533487083, -0.08721481970331113]
+    - [0.0, 0.0, 0.0, 1.0]
+  cam_overlaps: [1]
+  camera_model: pinhole
+  distortion_coeffs: [-0.03521708561298138, 0.13993068272515677, -0.1382658712726599, 0.054157317943265844]
+  distortion_model: equidistant
+  intrinsics: [241.78621144978587, 242.39804316458955, 317.3114290668952, 242.6087760646631]
+  resolution: [640, 480]
+  rostopic: /camera_slam_left/image
+  timeshift_cam_imu: 0.004270217900919078
+cam1:
+  T_cam_imu:
+    - [0.9874777193231493, 0.011345848775221044, -0.15735000971058993, 0.0015316425604920388]
+    - [0.010616662632099013, -0.9999286587861259, -0.005473921148227179, -0.010071955292996279]
+    - [-0.15740089045144923, 0.00373484320295129, -0.9875277265126997, -0.004942054217553331]
+    - [0.0, 0.0, 0.0, 1.0]
+  cam_overlaps: [0]
+  camera_model: pinhole
+  distortion_coeffs: [-0.026866455232950476, 0.10790535425845371, -0.08882084656895586, 0.02627360209817728]
+  distortion_model: equidistant
+  intrinsics: [242.1538223221408, 242.14114214084816, 317.8751234840723, 244.03555111568332]
+  resolution: [640, 480]
+  rostopic: /camera_slam_right/image
+  timeshift_cam_imu: 0.004075260451014888
+cam2:
+  T_cam_imu:
+    - [-0.1061564415671857, 0.7958374298669715, -0.5961321960241086, 0.1258230800185664]
+    - [0.9936801665484081, 0.06291350562179068, -0.09296030022921153, 0.005361067249285295]
+    - [-0.03647652014818453, -0.6022330745094548, -0.7974865437389955, -0.016394991903615534]
+    - [0.0, 0.0, 0.0, 1.0]
+  cam_overlaps: []
+  camera_model: pinhole
+  distortion_coeffs: [0.205362163753855, 0.8177824523079911, -1.4284013566280807, 1.07228296107305]
+  distortion_model: equidistant
+  intrinsics: [609.2948192551593, 608.364081254451, 703.7315385970343, 702.9153221781557]
+  resolution: [1408, 1408]
+  rostopic: /camera_rgb/image
+  timeshift_cam_imu: 0.006857702469605033

ov_msckf/cmake/ROS2.cmake

@@ -25,11 +25,13 @@ add_definitions(-DROS_AVAILABLE=2)
 # Add message files
 set(msg_files
         msg/ros2/OVRuntimeStatus.msg
+        msg/ros2/OVActiveFeature.msg
+        msg/ros2/OVActiveFeatureArray.msg
 )
 
 rosidl_generate_interfaces(${PROJECT_NAME}
         ${msg_files}
-        DEPENDENCIES std_msgs geometry_msgs
+        DEPENDENCIES std_msgs geometry_msgs sensor_msgs
 )
 ament_export_dependencies(rosidl_default_runtime)
 
@@ -131,6 +133,13 @@ target_link_libraries(test_sim_repeat ov_msckf_lib ${thirdparty_libraries})
 target_link_libraries(test_sim_repeat ${cpp_typesupport_target})
 install(TARGETS test_sim_repeat DESTINATION lib/${PROJECT_NAME})
 
+add_executable(listen_runtime src/listen_runtime.cpp)
+ament_target_dependencies(listen_runtime ${ament_libraries})
+target_link_libraries(listen_runtime ov_msckf_lib ${thirdparty_libraries})
+target_link_libraries(listen_runtime ${cpp_typesupport_target})
+install(TARGETS listen_runtime DESTINATION lib/${PROJECT_NAME})
+
+
 # Install launch and config directories
 install(DIRECTORY launch/ DESTINATION share/${PROJECT_NAME}/launch/)
 install(DIRECTORY ../config/ DESTINATION share/${PROJECT_NAME}/config/)

ov_msckf/launch/aria.launch.py

@@ -0,0 +1,127 @@
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, LogInfo, OpaqueFunction
+from launch.conditions import IfCondition
+from launch.substitutions import LaunchConfiguration, TextSubstitution
+from launch_ros.actions import Node
+from ament_index_python.packages import get_package_share_directory, get_package_prefix
+import os
+import sys
+
+launch_args = [
+    DeclareLaunchArgument(name="namespace", default_value="ov_msckf", description="namespace"),
+    DeclareLaunchArgument(
+        name="ov_enable", default_value="true", description="enable OpenVINS node"
+    ),
+    DeclareLaunchArgument(
+        name="rviz_enable", default_value="false", description="enable rviz node"
+    ),
+    DeclareLaunchArgument(
+        name="config",
+        default_value="aria",
+        description="euroc_mav, tum_vi, rpng_aruco...",
+    ),
+    DeclareLaunchArgument(
+        name="config_path",
+        default_value="",
+        description="path to estimator_config.yaml. If not given, determined based on provided 'config' above",
+    ),
+    DeclareLaunchArgument(
+        name="verbosity",
+        default_value="INFO",
+        description="ALL, DEBUG, INFO, WARNING, ERROR, SILENT",
+    ),
+    DeclareLaunchArgument(
+        name="use_stereo",
+        default_value="false",
+        description="if we have more than 1 camera, if we should try to track stereo constraints between pairs",
+    ),
+    DeclareLaunchArgument(
+        name="max_cameras",
+        default_value="2",
+        description="how many cameras we have 1 = mono, 2 = stereo, >2 = binocular (all mono tracking)",
+    ),
+    DeclareLaunchArgument(
+        name="save_total_state",
+        default_value="false",
+        description="record the total state with calibration and features to a txt file",
+    ),
+    DeclareLaunchArgument(
+        name="save_path",
+        default_value="aria",
+        description="save path for csv files"
+    )
+]
+
+def launch_setup(context):
+    config_path = LaunchConfiguration("config_path").perform(context)
+    if not config_path:
+        configs_dir = os.path.join(get_package_share_directory("ov_msckf"), "config/")
+        available_configs = os.listdir(configs_dir)
+        config = LaunchConfiguration("config").perform(context)
+        if config in available_configs:
+            config_path = os.path.join(
+                            get_package_share_directory("ov_msckf"),
+                            "config",config,"estimator_config.yaml"
+                        )
+        else:
+            return [
+                LogInfo(
+                    msg="ERROR: unknown config: '{}' - Available configs are: {} - not starting OpenVINS".format(
+                        config, ", ".join(available_configs)
+                    )
+                )
+            ]
+    else:
+        if not os.path.isfile(config_path):
+            return [
+                LogInfo(
+                    msg="ERROR: config_path file: '{}' - does not exist. - not starting OpenVINS".format(
+                        config_path)
+                    )
+            ]
+    node1 = Node(
+        package="ov_msckf",
+        executable="run_subscribe_msckf",
+        condition=IfCondition(LaunchConfiguration("ov_enable")),
+        namespace=LaunchConfiguration("namespace"),
+        output='screen',
+        parameters=[
+            {"verbosity": LaunchConfiguration("verbosity")},
+            {"use_stereo": LaunchConfiguration("use_stereo")},
+            {"max_cameras": LaunchConfiguration("max_cameras")},
+            {"save_total_state": LaunchConfiguration("save_total_state")},
+            {"config_path": config_path},
+        ],
+    )
+
+    node2 = Node(
+        package="rviz2",
+        executable="rviz2",
+        condition=IfCondition(LaunchConfiguration("rviz_enable")),
+        arguments=[
+            "-d"
+            + os.path.join(
+                get_package_share_directory("ov_msckf"), "launch", "display_ros2.rviz"
+            ),
+            "--ros-args",
+            "--log-level",
+            "warn",
+            ],
+    )
+
+    node3 = Node(
+        package="ov_msckf",
+        executable="listen_runtime",
+        parameters=[
+            {"save_path": LaunchConfiguration("save_path")}
+        ]
+    )
+
+    return [node1, node2, node3]
+
+
+def generate_launch_description():
+    opfunc = OpaqueFunction(function=launch_setup)
+    ld = LaunchDescription(launch_args)
+    ld.add_action(opfunc)
+    return ld