Merge pull request #186 from rpng/develop_v2.4

Development v2.4 - Camera classes, Masks, Formatting and Smaller Fixes

.gitignore

@@ -1,4 +1,4 @@
-cmake-build-debug
+cmake-build-*
 build
 Build
 *.*~

Dockerfile

@@ -0,0 +1,33 @@
+FROM osrf/ros:noetic-desktop-full
+
+# =========================================================
+# =========================================================
+
+# Are you are looking for how to use this docker file?
+#   - https://docs.openvins.com/dev-docker.html
+#   - https://docs.docker.com/get-started/
+#   - http://wiki.ros.org/docker/Tutorials/Docker
+
+# =========================================================
+# =========================================================
+
+# Dependencies we use, catkin tools is very good build system
+# Also some helper utitiles for fast in terminal edits (nano etc)
+RUN apt-get update && apt-get install -y libeigen3-dev nano git cmake
+RUN sudo apt-get install -y python3-catkin-tools python3-osrf-pycommon
+
+# Install deps needed for clion remote debugging
+# https://blog.jetbrains.com/clion/2020/01/using-docker-with-clion/
+RUN apt-get update && apt-get install -y ssh build-essential gcc g++ \
+    gdb clang cmake rsync tar python && apt-get clean
+RUN ( \
+    echo 'LogLevel DEBUG2'; \
+    echo 'PermitRootLogin yes'; \
+    echo 'PasswordAuthentication yes'; \
+    echo 'Subsystem sftp /usr/lib/openssh/sftp-server'; \
+  ) > /etc/ssh/sshd_config_test_clion \
+  && mkdir /run/sshd
+RUN useradd -m user && yes password | passwd user
+RUN usermod -s /bin/bash user
+CMD ["/usr/sbin/sshd", "-D", "-e", "-f", "/etc/ssh/sshd_config_test_clion"]
+

ReadMe.md

@@ -18,6 +18,7 @@ Please take a look at the feature list below for full details on what the system
 
 ## News / Events
 
+* **July 19, 2021** - Camera classes, masking support, alignment utility, and other small fixes. See v2.4 [PR#117](https://github.com/rpng/open_vins/pull/186) for details.
 * **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. 
@@ -62,9 +63,10 @@ Please take a look at the feature list below for full details on what the system
     * Stereo camera
     * Binocular camera
     * KLT or descriptor based
+    * Masked tracking
 * Static IMU initialization (sfm will be open sourced later)
 * Zero velocity detection and updates
-* Out of the box evaluation on EurocMav and TUM-VI datasets
+* Out of the box evaluation on EurocMav, TUM-VI, UZH-FPV, KAIST Urban and VIO datasets
 * Extensive evaluation suite (ATE, RPE, NEES, RMSE, etc..)
 
 

docs/bib/extra.bib

@@ -216,3 +216,16 @@ @inproceedings{Davidson2009ENC
     year={2009},
     url = {http://www.tkt.cs.tut.fi/research/nappo_files/1_C2.pdf},
 }
+
+@article{Jeon2021RAL,
+    title={Run your visual-inertial odometry on NVIDIA Jetson: Benchmark tests on a micro aerial vehicle},
+    author={Jeon, Jinwoo and Jung, Sungwook and Lee, Eungchang and Choi, Duckyu and Myung, Hyun},
+    journal={IEEE Robotics and Automation Letters},
+    volume={6},
+    number={3},
+    pages={5332--5339},
+    year={2021},
+    publisher={IEEE}
+}
+
+

docs/dev-docker.dox

@@ -0,0 +1,194 @@
+/**
+
+
+@page dev-docker Developing with Docker
+@tableofcontents
+
+
+@section dev-docker-install Installing Docker
+
+This will differ on which operating system you have installed, this guide is for linux-based systems.
+Please take a look at the official Docker [Get Docker](https://docs.docker.com/get-docker/) guide.
+There is also a guide from ROS called [getting started with ROS and Docker](http://wiki.ros.org/docker/Tutorials/Docker).
+On Ubuntu one should be able to do the following to get docker:
+
+@code{.shell-session}
+curl -fsSL https://download.docker.com/linux/ubuntu/gpg | sudo gpg --dearmor -o /usr/share/keyrings/docker-archive-keyring.gpg
+echo "deb [arch=amd64 signed-by=/usr/share/keyrings/docker-archive-keyring.gpg] https://download.docker.com/linux/ubuntu $(lsb_release -cs) stable" | sudo tee /etc/apt/sources.list.d/docker.list > /dev/null
+sudo apt-get update
+sudo apt-get install docker-ce docker-ce-cli containerd.io
+@endcode
+
+From there we can install [NVIDIA Container Toolkit](https://github.com/NVIDIA/nvidia-docker) to allow for the docker to use our GPU and for easy GUI pass through.
+You might also want to check out [this](https://roboticseabass.wordpress.com/2021/04/21/docker-and-ros/) blogpost for some more details.
+
+@code{.shell-session}
+distribution=$(. /etc/os-release;echo $ID$VERSION_ID) \
+   && curl -s -L https://nvidia.github.io/nvidia-docker/gpgkey | sudo apt-key add - \
+   && curl -s -L https://nvidia.github.io/nvidia-docker/$distribution/nvidia-docker.list | sudo tee /etc/apt/sources.list.d/nvidia-docker.list
+sudo apt-get update
+sudo apt-get install -y nvidia-docker2
+sudo systemctl restart docker
+sudo docker run --rm --gpus all nvidia/cuda:11.0-base nvidia-smi #to verify install
+@endcode
+
+From this point we should be able to "test" that everything is working ok.
+First on the host machine we need to allow for x11 windows to connect.
+
+@code{.shell-session}
+xhost +
+@endcode
+
+We can now run the following command which should open gazebo GUI on your main desktop window.
+
+@code{.shell-session}
+docker run -it --net=host --gpus all \
+    --env="NVIDIA_DRIVER_CAPABILITIES=all" \
+    --env="DISPLAY" \
+    --env="QT_X11_NO_MITSHM=1" \
+    --volume="/tmp/.X11-unix:/tmp/.X11-unix:rw" \
+    osrf/ros:noetic-desktop-full \
+    bash -it -c "roslaunch gazebo_ros empty_world.launch"
+@endcode
+
+Alternatively we can launch directly into a bash shell and run commands from in there.
+This basically gives you a terminal in the docker container.
+
+@code{.shell-session}
+docker run -it --net=host --gpus all \
+    --env="NVIDIA_DRIVER_CAPABILITIES=all" \
+    --env="DISPLAY" \
+    --env="QT_X11_NO_MITSHM=1" \
+    --volume="/tmp/.X11-unix:/tmp/.X11-unix:rw" \
+    osrf/ros:noetic-desktop-full \
+    bash
+# you should be able to launch rviz once in bash
+rviz
+@endcode
+
+
+@section dev-docker-openvins Running OpenVINS with Docker
+
+Clone the OpenVINS repository, build the container and then launch it.
+The [Dockerfile](https://github.com/rpng/open_vins/blob/master/Dockerfile) will not build the repo by default, thus you will need to build the project.
+
+
+@m_class{m-block m-warning}
+
+@par Use a Workspace Directory Mount
+    Here it is important to note that we are going to create a dedicated ROS *workspace* which will then be loaded into the workspace.
+    Thus if you are going to develop packages alongside OpenVINS you would make sure you have cloned your source code into the same workspace.
+    The workspace local folder will be mounted to `/catkin_ws/` in the docker, thus all changes from the host are mirrored.
+
+@code{.shell-session}
+mkdir -p ~/workspace/catkin_ws_ov/src
+cd ~/workspace/catkin_ws_ov/src
+git clone https://github.com/rpng/open_vins.git
+cd open_vins
+docker build -t openvins .
+cd ~/workspace/catkin_ws_ov
+@endcode
+
+If the dockerfile breaks, you can remove the image and reinstall using the above
+
+@code{.shell-session}
+docker image list
+docker image rm openvins --force
+@endcode
+
+From here it is a good idea to create a nice helper command which will launch the docker and also pass the GUI to your host machine.
+Here you can append it to the bottom of the ~/.bashrc so that we always have it on startup or just run the two commands on each restart
+
+@m_class{m-block m-warning}
+
+@par Directory Binding
+    You will need to specify *absolute directory paths* to the workspace and dataset folders on the host you want to bind.
+    Bind mounts are used to ensure that the host directory is directly used and all edits made on the host are sync'ed with
+    the docker container. See the docker [bind mounts](https://docs.docker.com/storage/bind-mounts/) documentation. You
+    can add and remove mounts from this command as you see the need.
+
+@code{.shell-session}
+nano ~/.bashrc
+# add to the bashrc file
+xhost + &> /dev/null
+export DOCKER_CATKINWS=/home/username/workspace/catkin_ws_ov
+export DOCKER_DATASETS=/home/username/datasets
+alias ov_docker="docker run -it --net=host --gpus all \
+    --env=\"NVIDIA_DRIVER_CAPABILITIES=all\" --env=\"DISPLAY\" \
+    --env=\"QT_X11_NO_MITSHM=1\" --volume=\"/tmp/.X11-unix:/tmp/.X11-unix:rw\" \
+    --mount type=bind,source=$DOCKER_CATKINWS,target=/catkin_ws \
+    --mount type=bind,source=$DOCKER_DATASETS,target=/datasets \
+    openvins $1"
+# save and exit
+source ~/.bashrc
+@endcode
+
+Now we can launch RVIZ and also compile the OpenVINS codebase.
+From two different terminals on the host machine one can run the following:
+
+@code{.shell-session}
+ov_docker roscore
+ov_docker rosrun rviz rviz -d /catkin_ws/src/open_vins/ov_msckf/launch/display.rviz
+ov_docker bash
+@endcode
+
+Now once inside the docker with the bash shell we can build and launch an example simulation:
+
+@code{.shell-session}
+cd catkin_ws
+catkin build
+source devel/setup.bash
+roslaunch ov_msckf pgeneva_sim.launch
+@endcode
+
+@m_class{m-block m-danger}
+
+@par Real-time Performance
+    On my machine running inside of the docker container is not real-time in nature. I am not sure why this is the case
+    if someone knows if something is setup incorrectly please open a github issue. Thus it is recommended to only use
+    the "serial" nodes which allows for the same parameters to be used as when installing directly on an OS.
+
+
+@section dev-docker-clion Using Jetbrains Clion and Docker
+
+Jetbrains provides some instructions on their side and a youtube video.
+Basically, Clion needs to be configured to use an external compile service and this service
+needs to be exposed from the docker container.
+I still recommend users compile with `catkin build` directly in the docker, but this will allow for debugging and syntax insights.
+
+- https://blog.jetbrains.com/clion/2020/01/using-docker-with-clion/
+- https://www.youtube.com/watch?v=h69XLiMtCT8
+
+After building the OpenVINS image (as above) we can do the following which will start a detached process in the docker.
+This process will allow us to connect Clion to it.
+
+@code{.shell-session}
+docker run -d --cap-add sys_ptrace -p127.0.0.1:2222:22 --name clion_remote_env openvins
+@endcode
+
+We can now change Clion to use the docker remote:
+
+1. In short, you should add a new Toolchain entry in settings under Build, Execution, Deployment as a Remote Host type.
+2. Click in the Credentials section and fill out the SSH credentials we set-up in the Dockerfile
+    - Host: localhost
+    - Port: 2222
+    - Username: user
+    - Password: password
+3. Add a CMake profile that uses this toolchain and you’re done.
+4. Change build target to be this new CMake profile (optionally just edit / delete the default)
+
+To add support for ROS you will need to manually set environmental variables in the CMake profile.
+These were generated by going into the ROS workspace, building a package, and then looking at `printenv` output.
+It should be under `Settings > Build,Execution,Deployment > CMake > (your profile) > Environment`.
+This might be a brittle method, but not sure what else to do... (also see [this](https://www.allaban.me/posts/2020/08/ros2-setup-ide-docker/) blog post).
+
+@code{.shell-session}
+LD_PATH_LIB=/catkin_ws/devel/lib:/opt/ros/noetic/lib;PYTHON_EXECUTABLE=/usr/bin/python3;PYTHON_INCLUDE_DIR=/usr/include/python3.8;ROS_VERSION=1;CMAKE_PREFIX_PATH=/catkin_ws/devel:/opt/ros/noetic;LD_LIBRARY_PATH=/catkin_ws/devel/lib:/opt/ros/noetic/lib;PATH=/opt/ros/noetic/bin:/usr/local/sbin:/usr/local/bin:/usr/sbin:/usr/bin:/sbin:/bin;PKG_CONFIG_PATH=/catkin_ws/devel/lib/pkgconfig:/opt/ros/noetic/lib/pkgconfig;PYTHONPATH=/opt/ros/noetic/lib/python3/dist-packages;ROSLISP_PACKAGE_DIRECTORIES=/catkin_ws/devel/share/common-lisp;ROS_PACKAGE_PATH=/catkin_ws/src/open_vins/ov_core:/catkin_ws/src/open_vins/ov_data:/catkin_ws/src/open_vins/ov_eval:/catkin_ws/src/open_vins/ov_msckf:/opt/ros/noetic/share
+@endcode
+
+When you build in Clion you should see in `docker stats` that the `clion_remote_env` is building the files and maxing out the CPU during this process.
+Clion should send the source files to the remote server and then on build should build and run it remotely within the docker container.
+A user might also want to edit `Build,Execution,Deployment > Deployment` settings to exclude certain folders from copying over.
+See this [jetbrains documentation page](https://www.jetbrains.com/help/clion/remote-projects-support.html) for more details.
+
+*/

docs/dev-welcome.dox

@@ -4,11 +4,11 @@
 @page dev-welcome Internals and Developers
 
 -   @subpage dev-coding-style --- General coding styles and conventions
+-   @subpage dev-docker --- How to use docker images to develop with
 -   @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/gs-datasets.dox

@@ -157,6 +157,15 @@ The dataset was collected in Korea with a vehicle equipped with stereo camera pa
 The camera is 10 Hz, while the Xsens IMU is 100 Hz sensing rate.
 A groundtruth "baseline" trajectory is also provided which is the resulting output from fusion of the FoG, RKT GPS, and wheel encoders.
 
+@m_class{m-block m-warning}
+
+@par Dynamic Environments
+    A challenging open research question is being able to handle dynamic objects seen from the cameras.
+    By default we rely on our tracking 8 point RANSAC to handle these dynamics objects.
+    In the most of the KAIST datasets the majority of the scene can be taken up by other moving vehicles, thus the performance can suffer.
+    Please be aware of this fact.
+
+
 @code{.shell-session}
 git clone https://github.com/rpng/file_player.git
 git clone https://github.com/irapkaist/irp_sen_msgs.git
@@ -166,18 +175,8 @@ rosrun file_player file_player
 
 To use the dataset, the dataset's file player can be used to publish the sensor information on to ROS.
 See the above commands on what packages you need to clone into your ROS workspace.
-One can record a rosbag after manually and use the serial OpenVINS processing node, or use the live node and manually playback the datasets.
 It is important to *disable* the "skip stop section" to ensure that we have continuous sensor feeds.
-Typically we process the datasets at 2x rate so we get a 20 Hz image feed and the datasets can be processed in a more efficient manor.
-
-@m_class{m-block m-warning}
-
-@par Dynamic Environments
-    A challenging open research question is being able to handle dynamic objects seen from the cameras.
-    By default we rely on our tracking 8 point RANSAC to handle these dynamics objects.
-    In the most of the KAIST datasets the majority of the scene can be taken up by other moving vehicles, thus the performance can suffer.
-    Please be aware of this fact.
-
+Typically we process the datasets at 1.5x rate so we get a ~20 Hz image feed and the datasets can be processed in a more efficient manor.
 
 @m_div{m-text-center}
 | Dataset Name | Length (km) | Dataset Link | Groundtruth Traj. | Example Launch |
@@ -188,6 +187,32 @@ Typically we process the datasets at 2x rate so we get a 20 Hz image feed and th
 @m_enddiv
 
 
+@section gs-data-kaist-vio KAIST VIO Dataset
+
+The [KAIST VIO dataset](https://github.com/zinuok/kaistviodataset) @cite Jeon2021RAL is a dataset of a MAV in an indoor 3.15 x 3.60 x 2.50 meter environment which undergoes various trajectory motions.
+The camera is intel realsense D435i 25 Hz, while the IMU is 100 Hz sensing rate from the pixelhawk 4 unit.
+A groundtruth "baseline" trajectory is also provided from a OptiTrack Mocap system at 50 Hz, the bag files have the marker body frame to IMU frame already applied.
+This topic has been provided in ov_data for convinces sake.
+
+@m_div{m-text-center}
+| Dataset Name | Length (km) | Dataset Link | Groundtruth Traj. | Example Launch |
+|-------------:|--------|--------------|------------------|------------------|
+| circle | 29.99 | [download](https://urserver.kaist.ac.kr/publicdata/KAIST_VIO_Dataset/circle/circle.bag) | [link](https://github.com/rpng/open_vins/tree/master/ov_data/kaist_vio) | [launch](https://github.com/rpng/open_vins/blob/master/ov_msckf/launch/pgeneva_ros_kaistvio.launch) |
+| circle_fast | 64.15 | [download](https://urserver.kaist.ac.kr/publicdata/KAIST_VIO_Dataset/circle/circle_fast.bag) | [link](https://github.com/rpng/open_vins/tree/master/ov_data/kaist_vio) | [launch](https://github.com/rpng/open_vins/blob/master/ov_msckf/launch/pgeneva_ros_kaistvio.launch) |
+| circle_head | 35.05 | [download](https://urserver.kaist.ac.kr/publicdata/KAIST_VIO_Dataset/circle/circle_head.bag) | [link](https://github.com/rpng/open_vins/tree/master/ov_data/kaist_vio) | [launch](https://github.com/rpng/open_vins/blob/master/ov_msckf/launch/pgeneva_ros_kaistvio.launch) |
+| infinite | 29.35 | [download](https://urserver.kaist.ac.kr/publicdata/KAIST_VIO_Dataset/infinite/infinite.bag) | [link](https://github.com/rpng/open_vins/tree/master/ov_data/kaist_vio) | [launch](https://github.com/rpng/open_vins/blob/master/ov_msckf/launch/pgeneva_ros_kaistvio.launch) |
+| infinite_fast | 54.24 | [download](https://urserver.kaist.ac.kr/publicdata/KAIST_VIO_Dataset/infinite/infinite_fast.bag) | [link](https://github.com/rpng/open_vins/tree/master/ov_data/kaist_vio) | [launch](https://github.com/rpng/open_vins/blob/master/ov_msckf/launch/pgeneva_ros_kaistvio.launch) |
+| infinite_head | 37.45 | [download](https://urserver.kaist.ac.kr/publicdata/KAIST_VIO_Dataset/infinite/infinite_head.bag) | [link](https://github.com/rpng/open_vins/tree/master/ov_data/kaist_vio) | [launch](https://github.com/rpng/open_vins/blob/master/ov_msckf/launch/pgeneva_ros_kaistvio.launch) |
+| rotation | 7.82 | [download](https://urserver.kaist.ac.kr/publicdata/KAIST_VIO_Dataset/rotation/rotation.bag) | [link](https://github.com/rpng/open_vins/tree/master/ov_data/kaist_vio) | [launch](https://github.com/rpng/open_vins/blob/master/ov_msckf/launch/pgeneva_ros_kaistvio.launch) |
+| rotation_fast | 14.55 | [download](https://urserver.kaist.ac.kr/publicdata/KAIST_VIO_Dataset/rotation/rotation_fast.bag) | [link](https://github.com/rpng/open_vins/tree/master/ov_data/kaist_vio) | [launch](https://github.com/rpng/open_vins/blob/master/ov_msckf/launch/pgeneva_ros_kaistvio.launch) |
+| square | 41.94 | [download](https://urserver.kaist.ac.kr/publicdata/KAIST_VIO_Dataset/square/square.bag) | [link](https://github.com/rpng/open_vins/tree/master/ov_data/kaist_vio) | [launch](https://github.com/rpng/open_vins/blob/master/ov_msckf/launch/pgeneva_ros_kaistvio.launch) |
+| square_fast | 44.07 | [download](https://urserver.kaist.ac.kr/publicdata/KAIST_VIO_Dataset/square/square_fast.bag) | [link](https://github.com/rpng/open_vins/tree/master/ov_data/kaist_vio) | [launch](https://github.com/rpng/open_vins/blob/master/ov_msckf/launch/pgeneva_ros_kaistvio.launch) |
+| square_head | 50.00 | [download](https://urserver.kaist.ac.kr/publicdata/KAIST_VIO_Dataset/square/square_head.bag) | [link](https://github.com/rpng/open_vins/tree/master/ov_data/kaist_vio) | [launch](https://github.com/rpng/open_vins/blob/master/ov_msckf/launch/pgeneva_ros_kaistvio.launch) |
+@m_enddiv
+
+
+
+