small tweaks to launch files and docs, also mark feats with negative …

…depth to be marginalized
rpng · Jul 8, 2020 · 0606e7f · 0606e7f
1 parent 6dd70b4
commit 0606e7f
Show file tree

Hide file tree

Showing 8 changed files with 76 additions and 42 deletions.
diff --git a/Doxyfile b/Doxyfile
@@ -811,7 +811,7 @@ RECURSIVE              = YES
 # Note that relative paths are relative to the directory from which doxygen is
 # run.
 
-EXCLUDE                =
+EXCLUDE                = ov_core/src/utils/CLI11.hpp
 
 # The EXCLUDE_SYMLINKS tag can be used to select whether or not files or
 # directories that are symbolic links (a Unix file system feature) are excluded

diff --git a/Doxyfile-mcss b/Doxyfile-mcss
@@ -32,12 +32,10 @@ ALIASES += \
     "m_keyword{3}=@xmlonly<mcss:search xmlns:mcss=\"http://mcss.mosra.cz/doxygen/\" mcss:keyword=\"\1\" mcss:title=\"\2\" mcss:suffix-length=\"\3\" />@endxmlonly" \
     "m_enum_values_as_keywords=@xmlonly<mcss:search xmlns:mcss=\"http://mcss.mosra.cz/doxygen/\" mcss:enum-values-as-keywords=\"true\" />@endxmlonly"
 
-
 ##! M_THEME_COLOR           = #2f73a3
 ##! M_FAVICON               = docs/img/favicon-light.png
 
-
-##! M_LINKS_NAVBAR1 = pages namespaceov__core namespaceov__eval namespaceov__msckf annotated
+##! M_LINKS_NAVBAR1 = pages namespaceov__core namespaceov__type namespaceov__msckf namespaceov__eval annotated
 ##! M_LINKS_NAVBAR2 = \
 ##!     "<a href=\"https://github.com/rpng/open_vins/">GitHub</a>"
 

diff --git a/docs/css/m-udel+documentation.compiled.css b/docs/css/m-udel+documentation.compiled.css
@@ -3,7 +3,7 @@
 /*
     This file is part of m.css.
 
-    Copyright © 2017, 2018, 2019 Vladimír Vondruš <[email protected]>
+    Copyright © 2017, 2018, 2019, 2020 Vladimír Vondruš <[email protected]>
 
     Permission is hereby granted, free of charge, to any person obtaining a
     copy of this software and associated documentation files (the "Software"),
@@ -422,6 +422,9 @@ mark {
   background-color: #f0c63e;
   color: #4c93d3;
 }
+.m-link-wrap {
+  word-break: break-all;
+}
 pre, code {
   font-family: 'Source Code Pro', monospace, monospace, monospace;
   font-size: 0.8em;
@@ -2556,6 +2559,23 @@ article:last-child, article section:last-child { margin-bottom: 0; }
 .m-code .il { color: #0000cf; font-weight: bold }
 
 .m-console .hll { background-color: #ffffcc }
+.m-console .g-AnsiBackgroundBlack { background-color: #232627 }
+.m-console .g-AnsiBackgroundBlue { background-color: #1d99f3 }
+.m-console .g-AnsiBackgroundBrightBlack { background-color: #7f8c8d }
+.m-console .g-AnsiBackgroundBrightBlue { background-color: #3daee9 }
+.m-console .g-AnsiBackgroundBrightCyan { background-color: #16a085 }
+.m-console .g-AnsiBackgroundBrightGreen { background-color: #1cdc9a }
+.m-console .g-AnsiBackgroundBrightMagenta { background-color: #8e44ad }
+.m-console .g-AnsiBackgroundBrightRed { background-color: #c0392b }
+.m-console .g-AnsiBackgroundBrightWhite { background-color: #ffffff }
+.m-console .g-AnsiBackgroundBrightYellow { background-color: #fdbc4b }
+.m-console .g-AnsiBackgroundCyan { background-color: #1abc9c }
+.m-console .g-AnsiBackgroundDefault { background-color: #fcfcfc }
+.m-console .g-AnsiBackgroundGreen { background-color: #11d116 }
+.m-console .g-AnsiBackgroundMagenta { background-color: #9b59b6 }
+.m-console .g-AnsiBackgroundRed { background-color: #ed1515 }
+.m-console .g-AnsiBackgroundWhite { background-color: #fcfcfc }
+.m-console .g-AnsiBackgroundYellow { background-color: #f67400 }
 .m-console .g-AnsiBlack { color: #232627 }
 .m-console .g-AnsiBlue { color: #1d99f3 }
 .m-console .g-AnsiBrightBlack { color: #7f8c8d; font-weight: bold }

diff --git a/docs/gs-datasets.dox b/docs/gs-datasets.dox
@@ -82,12 +82,14 @@ Here are the specifics of the sensors that each dataset uses:
     - Core visual-inertial sensor is the [VI-Sensor](https://furgalep.github.io/bib/nikolic_icra14.pdf)
     - Stereo global shutter images at 20 Hz
     - ADIS16448 IMU at 200 Hz
+    - Kalibr calibration file can be found [here](https://drive.google.com/file/d/1I0C-z3ZrTKne4bdbgBI6CtH1Rk4EQim0/view?usp=sharing)
 - Ironsides Datasets:
     - Core visual-inertial sensor is the [ironsides](https://arxiv.org/pdf/1710.00893v1.pdf)
     - Has two [Reach RTK](https://docs.emlid.com/reach/) one subscribed to a base station for corrections
     - Stereo global shutter fisheye images at 20 Hz
     - InvenSense IMU at 200 Hz
     - GPS fixes at 5 Hz (/reach01/tcpfix has corrections from [NYSNet](https://cors.dot.ny.gov/sbc))
+    - Kalibr calibration file can be found [here](https://drive.google.com/file/d/1bhn0GrIYNEeAabQAbWoP8l_514cJ0KrZ/view?usp=sharing)
 
 Most of these datasets do not have perfect calibration parameters, and some are not time synchronised.
 Thus, please ensure that you have enabled online calibration of these parameters.

diff --git a/docs/update-initfeat.dox → docs/update-featinit.dox b/docs/update-initfeat.dox → docs/update-featinit.dox
@@ -24,7 +24,7 @@ Thus we have the following mapping to a feature seen from the current frame:
 
 \f{align*}{
 	{}^{C_i}\mathbf{p}_f &
-	= z_{f} {}^{C_i}\mathbf{b}_{f}
+	= {}^{C_i}z_{f} {}^{C_i}\mathbf{b}_{f}
 	= {}^{C_i}z_{f}
 	\begin{bmatrix}
 	u_n \\ v_n \\ 1

diff --git a/ov_core/src/types/Landmark.h b/ov_core/src/types/Landmark.h
@@ -80,7 +80,7 @@ namespace ov_type {
             // Ensure we are not near zero in the z-direction
             if (LandmarkRepresentation::is_relative_representation(_feat_representation) && _value(_value.rows()-1) < 1e-8) {
                 printf(YELLOW "WARNING DEPTH %.8f BECAME CLOSE TO ZERO IN UPDATE!!!\n" RESET, _value(_value.rows()-1));
-                //_value(2) = 1e-8;
+                should_marg = true;
             }
         }
 

diff --git a/ov_msckf/launch/pgeneva_ros_kaist.launch b/ov_msckf/launch/pgeneva_ros_kaist.launch
@@ -1,9 +1,5 @@
 <launch>
 
-    <!-- mono or stereo and what ros bag to play -->
-    <arg name="max_cameras" default="2" />
-    <arg name="use_stereo"  default="true" />
-
     <!-- imu starting thresholds -->
     <arg name="init_window_time"  default="0.75" />
     <arg name="init_imu_thresh"   default="0.12" />
@@ -27,23 +23,23 @@
         <param name="use_fej"                type="bool"   value="true" />
         <param name="use_imuavg"             type="bool"   value="true" />
         <param name="use_rk4int"             type="bool"   value="true" />
-        <param name="use_stereo"             type="bool"   value="$(arg use_stereo)" />
+        <param name="use_stereo"             type="bool"   value="true" />
         <param name="calib_cam_extrinsics"   type="bool"   value="true" />
         <param name="calib_cam_intrinsics"   type="bool"   value="true" />
         <param name="calib_cam_timeoffset"   type="bool"   value="true" />
         <param name="calib_camimu_dt"        type="double" value="0.0" />
         <param name="max_clones"             type="int"    value="15" />
-        <param name="max_slam"               type="int"    value="75" />
+        <param name="max_slam"               type="int"    value="50" />
         <param name="max_slam_in_update"     type="int"    value="25" /> <!-- 25 seems to work well -->
         <param name="max_msckf_in_update"    type="int"    value="999" />
-        <param name="max_cameras"            type="int"    value="$(arg max_cameras)" />
+        <param name="max_cameras"            type="int"    value="2" />
         <param name="dt_slam_delay"          type="double" value="3" />
         <param name="init_window_time"       type="double" value="$(arg init_window_time)" />
         <param name="init_imu_thresh"        type="double" value="$(arg init_imu_thresh)" />
         <rosparam param="gravity">[0.0,0.0,9.80]</rosparam>
-        <param name="feat_rep_msckf"         type="string" value="GLOBAL_3D" />
-        <param name="feat_rep_slam"          type="string" value="ANCHORED_FULL_INVERSE_DEPTH" />
-        <param name="feat_rep_aruco"         type="string" value="ANCHORED_FULL_INVERSE_DEPTH" />
+        <param name="feat_rep_msckf"         type="string" value="ANCHORED_MSCKF_INVERSE_DEPTH" />
+        <param name="feat_rep_slam"          type="string" value="ANCHORED_MSCKF_INVERSE_DEPTH" />
+        <param name="feat_rep_aruco"         type="string" value="ANCHORED_MSCKF_INVERSE_DEPTH" />
 
         <!-- zero velocity update parameters -->
         <param name="try_zupt"               type="bool"   value="true" />
@@ -67,7 +63,7 @@
 
         <param name="fi_max_dist"        type="double" value="75" />
         <param name="fi_max_baseline"    type="double" value="500" />
-        <param name="fi_max_cond_number" type="double" value="1000" />
+        <param name="fi_max_cond_number" type="double" value="5000" />
 
         <!-- aruco tag/mapping properties -->
         <param name="use_aruco"        type="bool"   value="false" />

diff --git a/ov_msckf/launch/pgeneva_serial_ironsides.launch b/ov_msckf/launch/pgeneva_serial_ironsides.launch
@@ -1,5 +1,14 @@
 <launch>
 
+    <!-- mono or stereo and what ros bag to play -->
+    <arg name="max_cameras" default="2" />
+    <arg name="use_stereo"  default="true" />
+
+    <!-- saving trajectory path and timing information -->
+    <arg name="dosave"      default="false" />
+    <arg name="dotime"      default="false" />
+    <arg name="path_est"    default="/tmp/traj_estimate.txt" />
+    <arg name="path_time"   default="/tmp/traj_timing.txt" />
 
     <!-- MASTER NODE! -->
     <node name="run_serial_msckf" pkg="ov_msckf" type="run_serial_msckf" output="screen" clear_params="true" required="true">
@@ -19,23 +28,23 @@
         <param name="use_fej"                type="bool"   value="true" />
         <param name="use_imuavg"             type="bool"   value="true" />
         <param name="use_rk4int"             type="bool"   value="true" />
-        <param name="use_stereo"             type="bool"   value="true" />
+        <param name="use_stereo"             type="bool"   value="$(arg use_stereo)" />
         <param name="calib_cam_extrinsics"   type="bool"   value="true" />
         <param name="calib_cam_intrinsics"   type="bool"   value="true" />
         <param name="calib_cam_timeoffset"   type="bool"   value="true" />
         <param name="calib_camimu_dt"        type="double" value="0.02" />
         <param name="max_clones"             type="int"    value="15" />
-        <param name="max_slam"               type="int"    value="75" />
+        <param name="max_slam"               type="int"    value="50" />
         <param name="max_slam_in_update"     type="int"    value="25" /> <!-- 15 seems to work well -->
         <param name="max_msckf_in_update"    type="int"    value="999" />
-        <param name="max_cameras"            type="int"    value="1" />
+        <param name="max_cameras"            type="int"    value="$(arg max_cameras)" />
         <param name="dt_slam_delay"          type="double" value="1" />
         <param name="init_window_time"       type="double" value="1.0" />
         <param name="init_imu_thresh"        type="double" value="0.5" />
         <rosparam param="gravity">[0.0,0.0,9.802]</rosparam>
-        <param name="feat_rep_msckf"         type="string" value="GLOBAL_3D" />
-        <param name="feat_rep_slam"          type="string" value="ANCHORED_FULL_INVERSE_DEPTH" />
-        <param name="feat_rep_aruco"         type="string" value="ANCHORED_FULL_INVERSE_DEPTH" />
+        <param name="feat_rep_msckf"         type="string" value="ANCHORED_MSCKF_INVERSE_DEPTH" />
+        <param name="feat_rep_slam"          type="string" value="ANCHORED_MSCKF_INVERSE_DEPTH" />
+        <param name="feat_rep_aruco"         type="string" value="ANCHORED_MSCKF_INVERSE_DEPTH" />
 
         <!-- zero velocity update parameters -->
         <param name="try_zupt"               type="bool"   value="true" />
@@ -44,22 +53,22 @@
         <param name="zupt_noise_multiplier"  type="double" value="25" />
 
         <!-- timing statistics recording -->
-        <param name="record_timing_information"   type="bool"   value="false" />
-        <param name="record_timing_filepath"      type="string" value="/tmp/ov_msckf_timing.txt" />
+        <param name="record_timing_information"   type="bool"   value="$(arg dotime)" />
+        <param name="record_timing_filepath"      type="string" value="$(arg path_time)" />
 
         <!-- tracker/extractor properties -->
         <param name="use_klt"            type="bool"   value="true" />
         <param name="num_pts"            type="int"    value="200" />
-        <param name="fast_threshold"     type="int"    value="15" />
+        <param name="fast_threshold"     type="int"    value="10" />
         <param name="grid_x"             type="int"    value="5" />
         <param name="grid_y"             type="int"    value="5" />
-        <param name="min_px_dist"        type="int"    value="5" />
+        <param name="min_px_dist"        type="int"    value="10" />
         <param name="knn_ratio"          type="double" value="0.65" />
         <param name="downsample_cameras" type="bool"   value="false" />
 
-        <param name="fi_max_dist"        type="double" value="50" />
+        <param name="fi_max_dist"        type="double" value="75" />
         <param name="fi_max_baseline"    type="double" value="500" />
-        <param name="fi_max_cond_number" type="double" value="1000" />
+        <param name="fi_max_cond_number" type="double" value="5000" />
 
         <!-- aruco tag/mapping properties -->
         <param name="use_aruco"        type="bool"   value="false" />
@@ -92,24 +101,33 @@
 
         <!-- camera extrinsics -->
         <rosparam param="T_C0toI">
-        [
-        0.99992127, -0.0078594, 0.0097819, -0.05845078,
-        0.00784873, 0.99996856, 0.00112822, -0.00728728,
-        -0.00979046, -0.00105136, 0.99995152, 0.0623674,
-        0.00000000, 0.00000000, 0.00000000, 1.00000000
-        ]
+            [
+            0.99992127, -0.0078594, 0.0097819, -0.05845078,
+            0.00784873, 0.99996856, 0.00112822, -0.00728728,
+            -0.00979046, -0.00105136, 0.99995152, 0.0623674,
+            0.00000000, 0.00000000, 0.00000000, 1.00000000
+            ]
         </rosparam>
         <rosparam param="T_C1toI">
-        [
-        0.99995933, 0.00327998, 0.00840069, 0.00793529,
-        -0.00328309, 0.99999455, 0.000356, -0.00716413,
-        -0.00839948, -0.00038357, 0.99996465, 0.06245421,
-        -0.00000000,0.00000000,0.00000000,1.00000000
-        ]
+            [
+            0.99995933, 0.00327998, 0.00840069, 0.00793529,
+            -0.00328309, 0.99999455, 0.000356, -0.00716413,
+            -0.00839948, -0.00038357, 0.99996465, 0.06245421,
+            -0.00000000,0.00000000,0.00000000,1.00000000
+            ]
         </rosparam>
 
 
     </node>
 
+    <!-- record the trajectory if enabled -->
+    <group if="$(arg dosave)">
+        <node name="recorder_estimate" pkg="ov_eval" type="pose_to_file" output="screen" required="true">
+            <param name="topic"      type="str" value="/ov_msckf/poseimu" />
+            <param name="topic_type" type="str" value="PoseWithCovarianceStamped" />
+            <param name="output"     type="str" value="$(arg path_est)" />
+        </node>
+    </group>
+
 
 </launch>