Merge pull request #117 from rpng/develop_v2.3

Development v2.3 - Memory Management, Active Feature Publishing, Small Fixes

.github/workflows/build_catkin.yml

@@ -0,0 +1,64 @@
+name: C/C++ CI
+
+on:
+  push:
+    branches: [ master, develop_v2.3 ]
+  pull_request:
+    branches: [ master, develop_v2.3 ]
+
+jobs:
+  build:
+    runs-on: ${{ matrix.os }}
+    if: "!contains(github.event.head_commit.message, 'skip ci')"
+    strategy:
+      fail-fast: false
+      matrix:
+        os: [ubuntu-16.04, ubuntu-18.04, ubuntu-20.04]
+        compiler: ["/usr/bin/g++"]
+    steps:
+    - uses: actions/checkout@v2
+    - name: Checkout submodules
+      uses: textbook/git-checkout-submodule-action@master
+
+    - name: Install apt dependencies (ubuntu-16.04)
+      if: matrix.os == 'ubuntu-16.04'
+      run: |
+        sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list' &&
+        sudo apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654 &&
+        curl -sSL 'http://keyserver.ubuntu.com/pks/lookup?op=get&search=0xC1CF6E31E6BADE8868B172B4F42ED6FBAB17C654' | sudo apt-key add - &&
+        sudo apt update && sudo apt -y install build-essential ros-kinetic-desktop python-catkin-pkg python-catkin-tools libeigen3-dev git &&
+        touch ~/.bashrc && echo "source /opt/ros/kinetic/setup.bash" >> ~/.bashrc
+
+    - name: Install apt dependencies (ubuntu-18.04)
+      if: matrix.os == 'ubuntu-18.04'
+      run: |
+        sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list' &&
+        sudo apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654 &&
+        curl -sSL 'http://keyserver.ubuntu.com/pks/lookup?op=get&search=0xC1CF6E31E6BADE8868B172B4F42ED6FBAB17C654' | sudo apt-key add - &&
+        sudo apt update && sudo apt -y install build-essential ros-melodic-desktop python-catkin-pkg python-catkin-tools libeigen3-dev git &&
+        touch ~/.bashrc && echo "source /opt/ros/melodic/setup.bash" >> ~/.bashrc
+
+    - name: Install apt dependencies (ubuntu-20.04)
+      if: matrix.os == 'ubuntu-20.04'
+      run: |
+        sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list' &&
+        sudo apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654 &&
+        curl -sSL 'http://keyserver.ubuntu.com/pks/lookup?op=get&search=0xC1CF6E31E6BADE8868B172B4F42ED6FBAB17C654' | sudo apt-key add - &&
+        sudo apt update && sudo apt -y install build-essential ros-noetic-desktop python3-catkin-pkg python3-catkin-tools python3-osrf-pycommon libeigen3-dev git &&
+        touch ~/.bashrc && echo "source /opt/ros/noetic/setup.bash" >> ~/.bashrc
+
+    - name: Create catkin workspace and compile
+      run: |
+        export REPO=$(basename $GITHUB_REPOSITORY) &&
+        cd $GITHUB_WORKSPACE/.. && mkdir src/ &&
+        mv $REPO/ src/ && mkdir $REPO/ && mv src/ $REPO/ && cd $REPO/ &&
+        source /opt/ros/$(ls -1 /opt/ros/ | head -n1)/setup.bash && echo "ros version: $ROS_DISTRO" &&
+        catkin build -j2 --no-status
+
+    - name: Run simulation dataset
+      run: |
+        source $GITHUB_WORKSPACE/devel/setup.bash &&
+        roscore &
+        export REPO=$(basename $GITHUB_REPOSITORY) &&
+        source $GITHUB_WORKSPACE/devel/setup.bash &&
+        rosrun ov_msckf run_simulation _sim_traj_path:=$GITHUB_WORKSPACE/src/$REPO/ov_data/sim/udel_gore.txt

.gitignore

@@ -3,4 +3,6 @@ build
 Build
 *.*~
 .idea
-doxgen_generated
+doxgen_generated
+*.swp
+*.swo

ReadMe.md

@@ -1,6 +1,5 @@
-
-
 # OpenVINS
+![C/C++ CI](https://github.com/rpng/open_vins/workflows/C/C++%20CI/badge.svg)
 
 
 Welcome to the OpenVINS project!
@@ -19,6 +18,7 @@ Please take a look at the feature list below for full details on what the system
 
 ## News / Events
 
+* **December 1, 2020** - Released improved memory management, active feature pointcloud publishing, limiting number of features in update to bound compute, and other small fixes. See v2.3 [PR#117](https://github.com/rpng/open_vins/pull/117) for details.
 * **November 18, 2020** - Released groundtruth generation utility package, [vicon2gt](https://github.com/rpng/vicon2gt) to enable creation of groundtruth trajectories in a motion capture room for evaulating VIO methods.
 * **July 7, 2020** - Released zero velocity update for vehicle applications and direct initialization when standing still. See [PR#79](https://github.com/rpng/open_vins/pull/79) for details. 
 * **May 18, 2020** - Released secondary pose graph example repository [ov_secondary](https://github.com/rpng/ov_secondary) based on [VINS-Fusion](https://github.com/HKUST-Aerial-Robotics/VINS-Fusion). OpenVINS now publishes marginalized feature track, feature 3d position, and first camera intrinsics and extrinsics. See [PR#66](https://github.com/rpng/open_vins/pull/66) for details and discussion.

docs/dev-profiling.dox

@@ -0,0 +1,157 @@
+/**
+
+
+@page dev-profiling System Profiling
+@tableofcontents
+
+
+@section dev-profiling-compute Profiling Processing Time
+
+One way (besides inserting timing statements into the code) is to leverage a profiler such as [valgrind](https://www.valgrind.org/).
+This tool allows for recording of the call stack of the system.
+To use this with a ROS node, we can do the following (based on [this](http://wiki.ros.org/roslaunch/Tutorials/Roslaunch%20Nodes%20in%20Valgrind%20or%20GDB) guide):
+
+- Edit `roslaunch ov_msckf pgeneva_serial_eth.launch` launch file
+- Append `launch-prefix="valgrind --tool=callgrind --callgrind-out-file=/tmp/callgrind.txt"` to your ROS node. This will cause the node to run with valgrind.
+- Change the bag length to be only 10 or so seconds (since profiling is slow)
+
+@code{.shell-session}
+sudo apt install valgrind
+roslaunch ov_msckf pgeneva_serial_eth.launch
+@endcode
+
+After running the profiling program we will want to visualize it.
+There are some good tools for that, specifically we are using [gprof2dot](https://github.com/jrfonseca/gprof2dot) and [xdot.py](https://github.com/jrfonseca/xdot.py).
+First we will post-process it into a xdot graph format and then visualize it for inspection.
+
+@image html example_callgrind.png width=80%
+
+@code{.shell-session}
+// install viz programs
+apt-get install python3 graphviz
+apt-get install gir1.2-gtk-3.0 python3-gi python3-gi-cairo graphviz
+pip install gprof2dot xdot
+// actually process and then viz call file
+gprof2dot --format callgrind --strip /tmp/callgrind.txt --output /tmp/callgrind.xdot
+xdot /tmp/callgrind.xdot
+@endcode
+
+
+
+@section dev-profiling-leaks Memory Leaks
+
+One can leverage a profiler such as [valgrind](https://www.valgrind.org/) to perform memory leak check of the codebase.
+Ensure you have installed the `valgrind` package (see above).
+We can change the node launch file as follows:
+
+- Edit `roslaunch ov_msckf pgeneva_serial_eth.launch` launch file
+- Append `launch-prefix="valgrind --tool=memcheck --leak-check=yes"` to your ROS node. This will cause the node to run with valgrind.
+- Change the bag length to be only 10 or so seconds (since profiling is slow)
+
+
+This [page](https://web.stanford.edu/class/archive/cs/cs107/cs107.1206/resources/valgrind.html) has some nice support material for FAQ.
+An example loss is shown below which was found by memcheck.
+
+@code{.text}
+==5512== 1,578,860 (24 direct, 1,578,836 indirect) bytes in 1 blocks are definitely lost in loss record 6,585 of 6,589
+==5512==    at 0x4C3017F: operator new(unsigned long) (in /usr/lib/valgrind/vgpreload_memcheck-amd64-linux.so)
+....
+==5512==    by 0x543F868: operator[] (unordered_map.h:973)
+==5512==    by 0x543F868: ov_core::TrackKLT::feed_stereo(double, cv::Mat&, cv::Mat&, unsigned long, unsigned long) (TrackKLT.cpp:165)
+==5512==    by 0x4EF8C52: ov_msckf::VioManager::feed_measurement_stereo(double, cv::Mat&, cv::Mat&, unsigned long, unsigned long) (VioManager.cpp:245)
+==5512==    by 0x1238A9: main (ros_serial_msckf.cpp:247)
+@endcode
+
+
+
+
+
+
+
+@section dev-profiling-compiler Compiler Profiling
+
+Here is a small guide on how to perform compiler profiling for building of the codebase.
+This should be used to try to minimize compile times which in general hurt developer productivity.
+It is recommended to read the following pages which this is a condenced form of:
+
+- https://aras-p.info/blog/2019/01/16/time-trace-timeline-flame-chart-profiler-for-Clang/
+- https://aras-p.info/blog/2019/09/28/Clang-Build-Analyzer/
+
+First we need to ensure we have a compiler that can profile the build time.
+Clang greater then 9 should work, but we have tested only with 11.
+We can get the [latest Clang](https://apt.llvm.org/) by using the follow auto-install script:
+
+
+@code{.shell-session}
+sudo bash -c "$(wget -O - https://apt.llvm.org/llvm.sh)"
+export CC=/usr/bin/clang-11
+export CXX=/usr/bin/clang++-11
+@endcode
+
+We then need to clone the analyzer repository, which allows for summary generation.
+
+@code{.shell-session}
+git clone https://github.com/aras-p/ClangBuildAnalyzer
+cd ClangBuildAnalyzer
+cmake . && make
+@endcode
+
+We can finally build our ROS package and time how long it takes.
+Note that we are using [catkin tools](https://catkin-tools.readthedocs.io/en/latest/) to build here.
+The prefix *CBA* means to run the command in the ClangBuildAnalyzer repository clone folder.
+While the prefix *WS* means run in the root of your ROS workspace.
+
+@code{.shell-session}
+(WS) cd ~/workspace/
+(WS) catkin clean -y && mkdir build
+(CBA) ./ClangBuildAnalyzer --start ~/workspace/build/
+(WS) export CC=/usr/bin/clang-11 && export CXX=/usr/bin/clang++-11
+(WS) catkin build ov_msckf -DCMAKE_CXX_FLAGS="-ftime-trace"
+(CBA) ./ClangBuildAnalyzer --stop ~/workspace/build/ capture_file.bin
+(CBA) ./ClangBuildAnalyzer --analyze capture_file.bin > timing_results.txt
+@endcode
+
+The `time-trace` flag should generate a bunch of .json files in your build folder.
+These can be opened in your chrome browser `chrome://tracing` for viewing.
+In general the ClangBuildAnalyzer is more useful for finding what files take long.
+An example output of what is generated in the timing_results.txt file is:
+
+
+@code{.text}
+Analyzing build trace from 'capture_file.bin'...
+**** Time summary:
+Compilation (86 times):
+  Parsing (frontend):          313.9 s
+  Codegen & opts (backend):    222.9 s
+
+**** Files that took longest to parse (compiler frontend):
+ 13139 ms: /build//ov_msckf/CMakeFiles/ov_msckf_lib.dir/src/update/UpdaterSLAM.cpp.o
+ 12843 ms: /build//ov_msckf/CMakeFiles/run_serial_msckf.dir/src/ros_serial_msckf.cpp.o
+ ...
+
+**** Functions that took longest to compile:
+  1639 ms: main (/src/open_vins/ov_eval/src/error_comparison.cpp)
+  1337 ms: ov_core::BsplineSE3::get_acceleration(double, Eigen::Matrix<double, ... (/src/open_vins/ov_core/src/sim/BsplineSE3.cpp)
+  1156 ms: ov_eval::ResultSimulation::plot_state(bool, double) (/src/open_vins/ov_eval/src/calc/ResultSimulation.cpp)
+  ...
+
+ *** Expensive headers: 
+ 27505 ms: /src/open_vins/ov_core/src/track/TrackBase.h (included 12 times, avg 2292 ms), included via:
+   TrackKLT.cpp.o TrackKLT.h  (4372 ms)
+   TrackBase.cpp.o  (4297 ms)
+   TrackSIM.cpp.o TrackSIM.h  (4252 ms)
+   ...
+ @endcode
+
+ Some key methods to reduce compile times are as follows:
+ - Only include headers that are required for your class
+ - Don't include headers in your header files `.h` that are only required in your `.cpp` source files.
+ - Consider [forward declarations](https://www.wikiwand.com/en/Forward_declaration) of methods and types
+ - Ensure you are using an include guard in your headers
+
+
+
+
+
+
+*/

docs/dev-welcome.dox

@@ -7,6 +7,7 @@
 -   @subpage dev-docs --- Developer guide on how documentation can be built
 -   @subpage dev-index --- Description of the covariance index system
 -   @subpage dev-roadmap --- Where we plan to go in the future
+-   @subpage dev-profiling --- Some notes on performing profiling
 
 
 

docs/eval-error.dox

@@ -2,6 +2,7 @@
 
 
 @page eval-error Filter Error Evaluation Methods
+@tableofcontents
 
 
 @m_class{m-note m-warning}

docs/eval-metric.dox

@@ -2,6 +2,7 @@
 
 
 @page eval-metrics Filter Evaluation Metrics
+@tableofcontents
 
 
 

docs/eval-timing.dox

@@ -2,6 +2,7 @@
 
 
 @page eval-timing Filter Timing Analysis
+@tableofcontents
 
 
 @m_class{m-note m-warning}

docs/fej.dox

@@ -2,6 +2,7 @@
 
 
 @page fej First Estimate Jacobian (FEJ) Estimation
+@tableofcontents
 
 
 

docs/gs-installing.dox

@@ -6,39 +6,48 @@
 
 @section gs-install-ros ROS Dependency
 
-Our codebase is built on top of the [Robot Operating System (ROS)](https://www.ros.org/) and has been tested building on Ubuntu 16.04 systems with ROS Kinetic Kame.
+Our codebase is built on top of the [Robot Operating System (ROS)](https://www.ros.org/) and has been tested building on Ubuntu 16.04, 18.04, 20.04 systems with ROS Kinetic, Melodic, and Noetic.
 We also recommend installing the [catkin_tools](https://github.com/catkin/catkin_tools) build for easy ROS building.
-Please see the official instructions [here](http://wiki.ros.org/kinetic/Installation/Ubuntu), which have also been copied below.
+All ROS installs include [OpenCV](https://github.com/opencv/opencv), but if you need to build OpenCV from source ensure you build the contributed modules as we use Aruco feature extraction.
+See the [opencv_contrib](https://github.com/opencv/opencv_contrib) readme on how to configure your cmake command when you build the core OpenCV library.
+We have tested building with OpenCV 3.2, 3.3, 3.4, 4.2, and 4.5.
+Please see the official instructions to install ROS:
+
+* [Ubuntu 16.04 ROS Kinetic](http://wiki.ros.org/kinetic/Installation/Ubuntu) (uses OpenCV 3.3)
+* [Ubuntu 18.04 ROS Melodic](http://wiki.ros.org/melodic/Installation/Ubuntu) (uses OpenCV 3.2)
+* [Ubuntu 20.04 ROS Noetic](http://wiki.ros.org/noetic/Installation/Ubuntu) (uses OpenCV 4.2)
+
 
 @m_class{m-block m-warning}
 
 @par ROS Usage in OpenVINS
     We do support ROS-free builds, but don't recommend using this interface as we have limited support for it.
+    You will need to ensure you have installed OpenCV and Eigen3 which are the only dependencies.
     If ROS is not found on the system, one can use command line options to run the simulation without any visualization.
     If you are using the ROS-free interface, you will need to properly construct the @ref ov_msckf::VioManagerOptions struct with proper information and feed inertial and image data into the correct functions.
 
-
 @code{.shell-session}
 sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list'
 sudo apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654
 sudo apt-get update
-sudo apt-get install ros-kinetic-desktop-full
-sudo apt-get install python-catkin-tools
-echo "source /opt/ros/kinetic/setup.bash" >> ~/.bashrc
+export ROS_DISTRO=melodic # kinetic=16.04, melodic=18.04, noetic=20.04
+sudo apt-get install ros-$ROS_DISTRO-desktop-full
+echo "source /opt/ros/$ROS_DISTRO/setup.bash" >> ~/.bashrc
 source ~/.bashrc
+sudo apt-get install libeigen3-dev python-catkin-tools # ubuntu 16.04 or 18.04
+sudo apt-get install libeigen3-dev python3-catkin-tools python3-osrf-pycommon # ubuntu 20.04
 @endcode
 
 
+@section gs-install-opencv OpenCV Dependency (from source)
 
-@section gs-install-opencv OpenCV Dependency
-
-We leverage [OpenCV 3.4.6](https://opencv.org/) for this project and recommend you compile it from source.
-You can try linking to the one included with ROS but we have always had success with building from source to ensure we have all contributed OpenCV libraries.
-One should make sure you can see some of the "contrib" (e.g. xfeature2d) to ensure you have linked to the contrib modules.
+We leverage [OpenCV](https://opencv.org/) for this project which you can typically use the install from ROS.
+If these do not work (or are using non-ROS building), then you can try building OpenCV from source ensuring you include the contrib modules.
+One should make sure you can see some of the "contrib" (e.g. aruco) when you cmake to ensure you have linked to the contrib modules.
 
 @code{.shell-session}
-git clone --branch 3.4.6 https://github.com/opencv/opencv/
-git clone --branch 3.4.6 https://github.com/opencv/opencv_contrib/
+git clone https://github.com/opencv/opencv/
+git clone https://github.com/opencv/opencv_contrib/
 mkdir opencv/build/
 cd opencv/build/
 cmake -DOPENCV_EXTRA_MODULES_PATH=../../opencv_contrib/modules ..
@@ -48,16 +57,15 @@ sudo make install
 
 
 
-
 @section gs-install-openvins Cloning the OpenVINS Project
 
-
-Now that we have ROS and OpenCV install we can setup a catkin workspace and build the project!
+Now that we have ROS installed we can setup a catkin workspace and build the project!
 If you did not install the catkin_tools build system, you should be able to build using the standard `catkin_make` command that is included with ROS.
 If you run into any problems please google search the issue first and if you are unable to find a solution please open an issue on our github page.
 After the build is successful please following the @ref gs-tutorial guide on getting a dataset and running the system.
 
 @code{.shell-session}
+sudo apt-get install python-catkin-tools
 mkdir -p ~/workspace/catkin_ws_ov/src/
 cd ~/workspace/catkin_ws_ov/src/
 git clone https://github.com/rpng/open_vins/
@@ -83,4 +91,4 @@ catkin build -DDISABLE_MATPLOTLIB=ON # build without viz
 
 
 
-*/
+*/

docs/gs-tutorial.dox

@@ -33,6 +33,7 @@ One can take a look in the [launch](https://github.com/rpng/open_vins/tree/maste
         <param name="topic_imu"      type="string" value="/imu0" />
         <param name="topic_camera0"  type="string" value="/cam0/image_raw" />
         <param name="topic_camera1"  type="string" value="/cam1/image_raw" />
+        <rosparam param="stereo_pairs">[0,1]</rosparam>
 
         <!-- bag parameters -->
         <param name="path_bag"    type="string" value="/<path>/V1_01_easy.bag" />
@@ -111,6 +112,7 @@ Please take a look at the other launch files or the ov_msckf::VioManager constru
 | Group | Description |
 |---|---|
 | bag topics | ROS topics that we will parse the IMU and camera data from. If we are only using one camera, i.e. monocular, then only the first camera topic is used. |
+| stereo pairs | Even set of camera ids, which stereo tracking will be tried to be performed on. For example a 0,1,2,3 means that you want to perform stereo tracking of the 0,1 and 2,3 camera pairs. |
 | bag params. | Location of the bag we will read along with the start time, in seconds, and duration we want to run on. |
 | world/filter params. | This has most of the core parameters that can be tuned to improve filter performance including the sliding window size, representation, gravity, and number of environmental SLAM features. One can also change the number of cameras from 1 to 2 to do stereo matching and update. |
 | tracker/extractor params. | For our visual front-end tracker we have a few key parameters that we can tune, most importantly is the number of features extracted. |