diff --git a/.gitignore b/.gitignore
index 526ab120f..a505dbc3e 100644
--- a/.gitignore
+++ b/.gitignore
@@ -1,4 +1,4 @@
-cmake-build-debug
+cmake-build-*
build
Build
*.*~
diff --git a/Dockerfile b/Dockerfile
new file mode 100644
index 000000000..e69633365
--- /dev/null
+++ b/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"]
+
diff --git a/ReadMe.md b/ReadMe.md
index 65e8fceb1..ee8f5e956 100644
--- a/ReadMe.md
+++ b/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..)
diff --git a/docs/bib/extra.bib b/docs/bib/extra.bib
index 121d92590..783aa7566 100644
--- a/docs/bib/extra.bib
+++ b/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}
+}
+
+
diff --git a/docs/dev-docker.dox b/docs/dev-docker.dox
new file mode 100644
index 000000000..33d50b437
--- /dev/null
+++ b/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.
+
+*/
\ No newline at end of file
diff --git a/docs/dev-welcome.dox b/docs/dev-welcome.dox
index f5b6995c6..cc9bc2fa3 100644
--- a/docs/dev-welcome.dox
+++ b/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
-
*/
\ No newline at end of file
diff --git a/docs/gs-datasets.dox b/docs/gs-datasets.dox
index e3ab4a278..951d21586 100644
--- a/docs/gs-datasets.dox
+++ b/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
+
+
+
+
diff --git a/docs/gs-installing.dox b/docs/gs-installing.dox
index 1c34b8641..7fe56c797 100644
--- a/docs/gs-installing.dox
+++ b/docs/gs-installing.dox
@@ -13,9 +13,9 @@ See the [opencv_contrib](https://github.com/opencv/opencv_contrib) readme on how
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)
+- [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}
@@ -23,14 +23,14 @@ Please see the official instructions to install ROS:
@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 ROS is not found on the system, one can use command line options to run the simulation without any visualization or `cmake -DENABLE_CATKIN_ROS=OFF ..`.
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
-export ROS_DISTRO=melodic # kinetic=16.04, melodic=18.04, noetic=20.04
+export ROS_DISTRO=noetic # 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
@@ -55,6 +55,9 @@ make -j8
sudo make install
@endcode
+If you do not want to build the modules, you should also be able to do this (while it is not as well tested).
+The ArucoTag tracker depends on a non-free module in the contrib repository, thus this will need to be disabled.
+You can disable this with `catkin build -DENABLE_ARUCO_TAGS=OFF` or `cmake -DENABLE_ARUCO_TAGS=OFF ..` in your build folder.
@section gs-install-openvins Cloning the OpenVINS Project
diff --git a/ov_core/CMakeLists.txt b/ov_core/CMakeLists.txt
index 3440827e1..7fa4e6735 100644
--- a/ov_core/CMakeLists.txt
+++ b/ov_core/CMakeLists.txt
@@ -14,13 +14,22 @@ if (NOT OpenCV_FOUND)
endif()
find_package(Boost REQUIRED COMPONENTS system filesystem thread date_time)
-# display message to user
-#message(WARNING "EIGEN VERSION: " ${EIGEN3_VERSION})
+# Display message to user
message(WARNING "OPENCV VERSION: " ${OpenCV_VERSION})
message(WARNING "BOOST VERSION: " ${Boost_VERSION})
+# If we will compile with aruco support
+option(ENABLE_ARUCO_TAGS "Enable or disable aruco tag (disable if no contrib modules)" ON)
+if (NOT ENABLE_ARUCO_TAGS)
+ add_definitions(-DENABLE_ARUCO_TAGS=0)
+ message(WARNING "DISABLING ARUCOTAG TRACKING!")
+else()
+ add_definitions(-DENABLE_ARUCO_TAGS=1)
+endif()
+
# Describe catkin project
-if (catkin_FOUND)
+option(ENABLE_CATKIN_ROS "Enable or disable building with ROS (if it is found)" ON)
+if (catkin_FOUND AND ENABLE_CATKIN_ROS)
add_definitions(-DROS_AVAILABLE=1)
catkin_package(
CATKIN_DEPENDS roscpp rosbag tf std_msgs geometry_msgs sensor_msgs nav_msgs visualization_msgs cv_bridge
@@ -28,7 +37,8 @@ if (catkin_FOUND)
LIBRARIES ov_core_lib
)
else()
- message(WARNING "CATKIN NOT FOUND BUILDING WITHOUT ROS!")
+ add_definitions(-DROS_AVAILABLE=0)
+ message(WARNING "BUILDING WITHOUT ROS!")
endif()
@@ -89,12 +99,11 @@ target_include_directories(ov_core_lib PUBLIC src)
##################################################
# Make binary files!
##################################################
-if (catkin_FOUND)
+if (catkin_FOUND AND ENABLE_CATKIN_ROS)
add_executable(test_tracking src/test_tracking.cpp)
target_link_libraries(test_tracking ov_core_lib ${thirdparty_libraries})
endif()
-
add_executable(test_webcam src/test_webcam.cpp)
target_link_libraries(test_webcam ov_core_lib ${thirdparty_libraries})
diff --git a/ov_core/src/cam/CamBase.h b/ov_core/src/cam/CamBase.h
new file mode 100644
index 000000000..4b50d0467
--- /dev/null
+++ b/ov_core/src/cam/CamBase.h
@@ -0,0 +1,179 @@
+/*
+ * OpenVINS: An Open Platform for Visual-Inertial Research
+ * Copyright (C) 2021 Patrick Geneva
+ * Copyright (C) 2021 Guoquan Huang
+ * Copyright (C) 2021 OpenVINS Contributors
+ * Copyright (C) 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 .
+ */
+
+#ifndef OV_CORE_CAM_BASE_H
+#define OV_CORE_CAM_BASE_H
+
+#include
+#include
+
+#include
+
+namespace ov_core {
+
+/**
+ * @brief Base pinhole camera model class
+ *
+ * This is the base class for all our camera models.
+ * All these models are pinhole cameras, thus just have standard reprojection logic.
+ *
+ * See each base class for detailed examples on each model:
+ * - @ref ov_core::CamEqui
+ * - @ref ov_core::CamRadtan
+ */
+class CamBase {
+
+public:
+ /**
+ * @brief This will set and update the camera calibration values.
+ * This should be called on startup for each camera and after update!
+ * @param calib Camera calibration information (f_x & f_y & c_x & c_y & k_1 & k_2 & k_3 & k_4)
+ */
+ virtual void set_value(const Eigen::MatrixXd &calib) {
+
+ // Assert we are of size eight
+ assert(calib.rows() == 8);
+ camera_values = calib;
+
+ // Camera matrix
+ cv::Matx33d tempK;
+ tempK(0, 0) = calib(0);
+ tempK(0, 1) = 0;
+ tempK(0, 2) = calib(2);
+ tempK(1, 0) = 0;
+ tempK(1, 1) = calib(1);
+ tempK(1, 2) = calib(3);
+ tempK(2, 0) = 0;
+ tempK(2, 1) = 0;
+ tempK(2, 2) = 1;
+ camera_k_OPENCV = tempK;
+
+ // Distortion parameters
+ cv::Vec4d tempD;
+ tempD(0) = calib(4);
+ tempD(1) = calib(5);
+ tempD(2) = calib(6);
+ tempD(3) = calib(7);
+ camera_d_OPENCV = tempD;
+ }
+
+ /**
+ * @brief Given a raw uv point, this will undistort it based on the camera matrices into normalized camera coords.
+ * @param uv_dist Raw uv coordinate we wish to undistort
+ * @return 2d vector of normalized coordinates
+ */
+ virtual Eigen::Vector2f undistort_f(const Eigen::Vector2f &uv_dist) = 0;
+
+ /**
+ * @brief Given a raw uv point, this will undistort it based on the camera matrices into normalized camera coords.
+ * @param uv_dist Raw uv coordinate we wish to undistort
+ * @return 2d vector of normalized coordinates
+ */
+ Eigen::Vector2d undistort_d(const Eigen::Vector2d &uv_dist) {
+ Eigen::Vector2f ept1, ept2;
+ ept1 = uv_dist.cast();
+ ept2 = undistort_f(ept1);
+ return ept2.cast();
+ }
+
+ /**
+ * @brief Given a raw uv point, this will undistort it based on the camera matrices into normalized camera coords.
+ * @param uv_dist Raw uv coordinate we wish to undistort
+ * @return 2d vector of normalized coordinates
+ */
+ cv::Point2f undistort_cv(const cv::Point2f &uv_dist) {
+ Eigen::Vector2f ept1, ept2;
+ ept1 << uv_dist.x, uv_dist.y;
+ ept2 = undistort_f(ept1);
+ cv::Point2f pt_out;
+ pt_out.x = ept2(0);
+ pt_out.y = ept2(1);
+ return pt_out;
+ }
+
+ /**
+ * @brief Given a normalized uv coordinate this will distort it to the raw image plane
+ * @param uv_norm Normalized coordinates we wish to distort
+ * @return 2d vector of raw uv coordinate
+ */
+ virtual Eigen::Vector2f distort_f(const Eigen::Vector2f &uv_norm) = 0;
+
+ /**
+ * @brief Given a normalized uv coordinate this will distort it to the raw image plane
+ * @param uv_norm Normalized coordinates we wish to distort
+ * @return 2d vector of raw uv coordinate
+ */
+ Eigen::Vector2d distort_d(const Eigen::Vector2d &uv_norm) {
+ Eigen::Vector2f ept1, ept2;
+ ept1 = uv_norm.cast();
+ ept2 = distort_f(ept1);
+ return ept2.cast();
+ }
+
+ /**
+ * @brief Given a normalized uv coordinate this will distort it to the raw image plane
+ * @param uv_norm Normalized coordinates we wish to distort
+ * @return 2d vector of raw uv coordinate
+ */
+ cv::Point2f distort_cv(const cv::Point2f &uv_norm) {
+ Eigen::Vector2f ept1, ept2;
+ ept1 << uv_norm.x, uv_norm.y;
+ ept2 = distort_f(ept1);
+ cv::Point2f pt_out;
+ pt_out.x = ept2(0);
+ pt_out.y = ept2(1);
+ return pt_out;
+ }
+
+ /**
+ * @brief Computes the derivative of raw distorted to normalized coordinate.
+ * @param uv_norm Normalized coordinates we wish to distort
+ * @param H_dz_dzn Derivative of measurement z in respect to normalized
+ * @param H_dz_dzeta Derivative of measurement z in respect to intrinic parameters
+ */
+ virtual void compute_distort_jacobian(const Eigen::Vector2d &uv_norm, Eigen::MatrixXd &H_dz_dzn, Eigen::MatrixXd &H_dz_dzeta) = 0;
+
+ /// Gets the complete intrinsic vector
+ Eigen::MatrixXd get_value() { return camera_values; }
+
+ /// Gets the camera matrix
+ cv::Matx33d get_K() { return camera_k_OPENCV; }
+
+ /// Gets the camera distortion
+ cv::Vec4d get_D() { return camera_d_OPENCV; }
+
+protected:
+ // Cannot construct the base camera class, needs a distortion model
+ CamBase() = default;
+
+ /// Raw set of camera intrinic values (f_x & f_y & c_x & c_y & k_1 & k_2 & k_3 & k_4)
+ Eigen::MatrixXd camera_values;
+
+ /// Camera intrinsics in OpenCV format
+ cv::Matx33d camera_k_OPENCV;
+
+ /// Camera distortion in OpenCV format
+ cv::Vec4d camera_d_OPENCV;
+};
+
+} // namespace ov_core
+
+#endif /* OV_CORE_CAM_BASE_H */
\ No newline at end of file
diff --git a/ov_core/src/cam/CamEqui.h b/ov_core/src/cam/CamEqui.h
new file mode 100644
index 000000000..abc44eaf6
--- /dev/null
+++ b/ov_core/src/cam/CamEqui.h
@@ -0,0 +1,229 @@
+/*
+ * OpenVINS: An Open Platform for Visual-Inertial Research
+ * Copyright (C) 2021 Patrick Geneva
+ * Copyright (C) 2021 Guoquan Huang
+ * Copyright (C) 2021 OpenVINS Contributors
+ * Copyright (C) 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 .
+ */
+
+#ifndef OV_CORE_CAM_EQUI_H
+#define OV_CORE_CAM_EQUI_H
+
+#include "CamBase.h"
+
+namespace ov_core {
+
+/**
+ * @brief Fisheye / equadistant model pinhole camera model class
+ *
+ * As fisheye or wide-angle lenses are widely used in practice, we here provide mathematical derivations
+ * of such distortion model as in [OpenCV fisheye](https://docs.opencv.org/3.4/db/d58/group__calib3d__fisheye.html#details).
+ *
+ * \f{align*}{
+ * \begin{bmatrix} u \\ v \end{bmatrix}:= \mathbf{z}_k &= \mathbf h_d(\mathbf{z}_{n,k}, ~\boldsymbol\zeta)
+ * = \begin{bmatrix} f_x * x + c_x \\
+ * f_y * y + c_y \end{bmatrix}\\[1em]
+ * \empty
+ * {\rm where}~~
+ * x &= \frac{x_n}{r} * \theta_d \\
+ * y &= \frac{y_n}{r} * \theta_d \\
+ * \theta_d &= \theta (1 + k_1 \theta^2 + k_2 \theta^4 + k_3 \theta^6 + k_4 \theta^8) \\
+ * \quad r^2 &= x_n^2 + y_n^2 \\
+ * \theta &= atan(r)
+ * \f}
+ *
+ * where \f$ \mathbf{z}_{n,k} = [ x_n ~ y_n ]^\top\f$ are the normalized coordinates of the 3D feature
+ * and u and v are the distorted image coordinates on the image plane.
+ * Clearly, the following distortion intrinsic parameters are used in the above model:
+ *
+ * \f{align*}{
+ * \boldsymbol\zeta = \begin{bmatrix} f_x & f_y & c_x & c_y & k_1 & k_2 & k_3 & k_4 \end{bmatrix}^\top
+ * \f}
+ *
+ * In analogy to the previous radial distortion (see @ref ov_core::CamRadtan) case, the following Jacobian for these
+ * parameters is needed for intrinsic calibration:
+ * \f{align*}{
+ * \frac{\partial \mathbf h_d (\cdot)}{\partial \boldsymbol\zeta} =
+ * \begin{bmatrix}
+ * x_n & 0 & 1 & 0 & f_x*(\frac{x_n}{r}\theta^3) & f_x*(\frac{x_n}{r}\theta^5) & f_x*(\frac{x_n}{r}\theta^7) & f_x*(\frac{x_n}{r}\theta^9)
+ * \\[5pt] 0 & y_n & 0 & 1 & f_y*(\frac{y_n}{r}\theta^3) & f_y*(\frac{y_n}{r}\theta^5) & f_y*(\frac{y_n}{r}\theta^7) &
+ * f_y*(\frac{y_n}{r}\theta^9) \end{bmatrix} \f}
+ *
+ * Similarly, with the chain rule of differentiation,
+ * we can compute the following Jacobian with respect to the normalized coordinates:
+ *
+ * \f{align*}{
+ * \frac{\partial \mathbf h_d(\cdot)}{\partial \mathbf{z}_{n,k}}
+ * &=
+ * \frac{\partial uv}{\partial xy}\frac{\partial xy}{\partial x_ny_n}+
+ * \frac{\partial uv}{\partial xy}\frac{\partial xy}{\partial r}\frac{\partial r}{\partial x_ny_n}+
+ * \frac{\partial uv}{\partial xy}\frac{\partial xy}{\partial \theta_d}\frac{\partial \theta_d}{\partial \theta}\frac{\partial
+ * \theta}{\partial r}\frac{\partial r}{\partial x_ny_n} \\[1em] \empty
+ * {\rm where}~~~~
+ * \frac{\partial uv}{\partial xy} &= \begin{bmatrix} f_x & 0 \\ 0 & f_y \end{bmatrix} \\
+ * \empty
+ * \frac{\partial xy}{\partial x_ny_n} &= \begin{bmatrix} \theta_d/r & 0 \\ 0 & \theta_d/r \end{bmatrix} \\
+ * \empty
+ * \frac{\partial xy}{\partial r} &= \begin{bmatrix} -\frac{x_n}{r^2}\theta_d \\ -\frac{y_n}{r^2}\theta_d \end{bmatrix} \\
+ * \empty
+ * \frac{\partial r}{\partial x_ny_n} &= \begin{bmatrix} \frac{x_n}{r} & \frac{y_n}{r} \end{bmatrix} \\
+ * \empty
+ * \frac{\partial xy}{\partial \theta_d} &= \begin{bmatrix} \frac{x_n}{r} \\ \frac{y_n}{r} \end{bmatrix} \\
+ * \empty
+ * \frac{\partial \theta_d}{\partial \theta} &= \begin{bmatrix} 1 + 3k_1 \theta^2 + 5k_2 \theta^4 + 7k_3 \theta^6 + 9k_4
+ * \theta^8\end{bmatrix} \\ \empty \frac{\partial \theta}{\partial r} &= \begin{bmatrix} \frac{1}{r^2+1} \end{bmatrix} \f}
+ *
+ * To equate this to one of Kalibr's models, this is what you would use for `pinhole-equi`.
+ */
+class CamEqui : public CamBase {
+
+public:
+ // constructor
+ CamEqui() {}
+
+ /**
+ * @brief Given a raw uv point, this will undistort it based on the camera matrices into normalized camera coords.
+ * @param uv_dist Raw uv coordinate we wish to undistort
+ * @return 2d vector of normalized coordinates
+ */
+ Eigen::Vector2f undistort_f(const Eigen::Vector2f &uv_dist) override {
+
+ // Determine what camera parameters we should use
+ cv::Matx33d camK = camera_k_OPENCV;
+ cv::Vec4d camD = camera_d_OPENCV;
+
+ // Convert point to opencv format
+ cv::Mat mat(1, 2, CV_32F);
+ mat.at(0, 0) = uv_dist(0);
+ mat.at(0, 1) = uv_dist(1);
+ mat = mat.reshape(2); // Nx1, 2-channel
+
+ // Undistort it!
+ cv::fisheye::undistortPoints(mat, mat, camK, camD);
+
+ // Construct our return vector
+ Eigen::Vector2f pt_out;
+ mat = mat.reshape(1); // Nx2, 1-channel
+ pt_out(0) = mat.at(0, 0);
+ pt_out(1) = mat.at(0, 1);
+ return pt_out;
+ }
+
+ /**
+ * @brief Given a normalized uv coordinate this will distort it to the raw image plane
+ * @param uv_norm Normalized coordinates we wish to distort
+ * @return 2d vector of raw uv coordinate
+ */
+ Eigen::Vector2f distort_f(const Eigen::Vector2f &uv_norm) override {
+
+ // Get our camera parameters
+ Eigen::MatrixXd cam_d = camera_values;
+
+ // Calculate distorted coordinates for fisheye
+ double r = std::sqrt(uv_norm(0) * uv_norm(0) + uv_norm(1) * uv_norm(1));
+ double theta = std::atan(r);
+ double theta_d = theta + cam_d(4) * std::pow(theta, 3) + cam_d(5) * std::pow(theta, 5) + cam_d(6) * std::pow(theta, 7) +
+ cam_d(7) * std::pow(theta, 9);
+
+ // Handle when r is small (meaning our xy is near the camera center)
+ double inv_r = (r > 1e-8) ? 1.0 / r : 1.0;
+ double cdist = (r > 1e-8) ? theta_d * inv_r : 1.0;
+
+ // Calculate distorted coordinates for fisheye
+ Eigen::Vector2f uv_dist;
+ double x1 = uv_norm(0) * cdist;
+ double y1 = uv_norm(1) * cdist;
+ uv_dist(0) = (float)(cam_d(0) * x1 + cam_d(2));
+ uv_dist(1) = (float)(cam_d(1) * y1 + cam_d(3));
+ return uv_dist;
+ }
+
+ /**
+ * @brief Computes the derivative of raw distorted to normalized coordinate.
+ * @param uv_norm Normalized coordinates we wish to distort
+ * @param H_dz_dzn Derivative of measurement z in respect to normalized
+ * @param H_dz_dzeta Derivative of measurement z in respect to intrinic parameters
+ */
+ void compute_distort_jacobian(const Eigen::Vector2d &uv_norm, Eigen::MatrixXd &H_dz_dzn, Eigen::MatrixXd &H_dz_dzeta) override {
+
+ // Get our camera parameters
+ Eigen::MatrixXd cam_d = camera_values;
+
+ // Calculate distorted coordinates for fisheye
+ double r = std::sqrt(uv_norm(0) * uv_norm(0) + uv_norm(1) * uv_norm(1));
+ double theta = std::atan(r);
+ double theta_d = theta + cam_d(4) * std::pow(theta, 3) + cam_d(5) * std::pow(theta, 5) + cam_d(6) * std::pow(theta, 7) +
+ cam_d(7) * std::pow(theta, 9);
+
+ // Handle when r is small (meaning our xy is near the camera center)
+ double inv_r = (r > 1e-8) ? 1.0 / r : 1.0;
+ double cdist = (r > 1e-8) ? theta_d * inv_r : 1.0;
+
+ // Jacobian of distorted pixel to "normalized" pixel
+ Eigen::Matrix duv_dxy = Eigen::Matrix::Zero();
+ duv_dxy << cam_d(0), 0, 0, cam_d(1);
+
+ // Jacobian of "normalized" pixel to normalized pixel
+ Eigen::Matrix dxy_dxyn = Eigen::Matrix::Zero();
+ dxy_dxyn << theta_d * inv_r, 0, 0, theta_d * inv_r;
+
+ // Jacobian of "normalized" pixel to r
+ Eigen::Matrix dxy_dr = Eigen::Matrix::Zero();
+ dxy_dr << -uv_norm(0) * theta_d * inv_r * inv_r, -uv_norm(1) * theta_d * inv_r * inv_r;
+
+ // Jacobian of r pixel to normalized xy
+ Eigen::Matrix dr_dxyn = Eigen::Matrix::Zero();
+ dr_dxyn << uv_norm(0) * inv_r, uv_norm(1) * inv_r;
+
+ // Jacobian of "normalized" pixel to theta_d
+ Eigen::Matrix dxy_dthd = Eigen::Matrix::Zero();
+ dxy_dthd << uv_norm(0) * inv_r, uv_norm(1) * inv_r;
+
+ // Jacobian of theta_d to theta
+ double dthd_dth = 1 + 3 * cam_d(4) * std::pow(theta, 2) + 5 * cam_d(5) * std::pow(theta, 4) + 7 * cam_d(6) * std::pow(theta, 6) +
+ 9 * cam_d(7) * std::pow(theta, 8);
+
+ // Jacobian of theta to r
+ double dth_dr = 1 / (r * r + 1);
+
+ // Total Jacobian wrt normalized pixel coordinates
+ H_dz_dzn = Eigen::MatrixXd::Zero(2, 2);
+ H_dz_dzn = duv_dxy * (dxy_dxyn + (dxy_dr + dxy_dthd * dthd_dth * dth_dr) * dr_dxyn);
+
+ // Calculate distorted coordinates for fisheye
+ double x1 = uv_norm(0) * cdist;
+ double y1 = uv_norm(1) * cdist;
+
+ // Compute the Jacobian in respect to the intrinsics
+ H_dz_dzeta = Eigen::MatrixXd::Zero(2, 8);
+ H_dz_dzeta(0, 0) = x1;
+ H_dz_dzeta(0, 2) = 1;
+ H_dz_dzeta(0, 4) = cam_d(0) * uv_norm(0) * inv_r * std::pow(theta, 3);
+ H_dz_dzeta(0, 5) = cam_d(0) * uv_norm(0) * inv_r * std::pow(theta, 5);
+ H_dz_dzeta(0, 6) = cam_d(0) * uv_norm(0) * inv_r * std::pow(theta, 7);
+ H_dz_dzeta(0, 7) = cam_d(0) * uv_norm(0) * inv_r * std::pow(theta, 9);
+ H_dz_dzeta(1, 1) = y1;
+ H_dz_dzeta(1, 3) = 1;
+ H_dz_dzeta(1, 4) = cam_d(1) * uv_norm(1) * inv_r * std::pow(theta, 3);
+ H_dz_dzeta(1, 5) = cam_d(1) * uv_norm(1) * inv_r * std::pow(theta, 5);
+ H_dz_dzeta(1, 6) = cam_d(1) * uv_norm(1) * inv_r * std::pow(theta, 7);
+ H_dz_dzeta(1, 7) = cam_d(1) * uv_norm(1) * inv_r * std::pow(theta, 9);
+ }
+};
+
+} // namespace ov_core
+
+#endif /* OV_CORE_CAM_EQUI_H */
\ No newline at end of file
diff --git a/ov_core/src/cam/CamRadtan.h b/ov_core/src/cam/CamRadtan.h
new file mode 100644
index 000000000..0ee16d101
--- /dev/null
+++ b/ov_core/src/cam/CamRadtan.h
@@ -0,0 +1,199 @@
+/*
+ * OpenVINS: An Open Platform for Visual-Inertial Research
+ * Copyright (C) 2021 Patrick Geneva
+ * Copyright (C) 2021 Guoquan Huang
+ * Copyright (C) 2021 OpenVINS Contributors
+ * Copyright (C) 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 .
+ */
+
+#ifndef OV_CORE_CAM_RADTAN_H
+#define OV_CORE_CAM_RADTAN_H
+
+#include "CamBase.h"
+
+namespace ov_core {
+
+/**
+ * @brief Radial-tangential / Brown–Conrady model pinhole camera model class
+ *
+ * To calibrate camera intrinsics, we need to know how to map our normalized coordinates
+ * into the raw pixel coordinates on the image plane. We first employ the radial distortion
+ * as in [OpenCV model](https://docs.opencv.org/3.4/da/d54/group__imgproc__transform.html#details):
+ *
+ * \f{align*}{
+ * \begin{bmatrix} u \\ v \end{bmatrix}:= \mathbf{z}_k &= \mathbf h_d(\mathbf{z}_{n,k}, ~\boldsymbol\zeta)
+ * = \begin{bmatrix} f_x * x + c_x \\
+ * f_y * y + c_y \end{bmatrix}\\[1em]
+ * \empty
+ * {\rm where}~~
+ * x &= x_n (1 + k_1 r^2 + k_2 r^4) + 2 p_1 x_n y_n + p_2(r^2 + 2 x_n^2) \\\
+ * y &= y_n (1 + k_1 r^2 + k_2 r^4) + p_1 (r^2 + 2 y_n^2) + 2 p_2 x_n y_n \\[1em]
+ * r^2 &= x_n^2 + y_n^2
+ * \f}
+ *
+ * where \f$ \mathbf{z}_{n,k} = [ x_n ~ y_n ]^\top\f$ are the normalized coordinates of the 3D feature and u and v are the distorted image
+ * coordinates on the image plane. The following distortion and camera intrinsic (focal length and image center) parameters are involved in
+ * the above distortion model, which can be estimated online:
+ *
+ * \f{align*}{
+ * \boldsymbol\zeta = \begin{bmatrix} f_x & f_y & c_x & c_y & k_1 & k_2 & p_1 & p_2 \end{bmatrix}^\top
+ * \f}
+ *
+ * Note that we do not estimate the higher order (i.e., higher than fourth order) terms as in most offline calibration
+ * methods such as [Kalibr](https://github.com/ethz-asl/kalibr). To estimate these intrinsic parameters (including the
+ * distortation parameters), the following Jacobian for these parameters is needed:
+ *
+ * \f{align*}{
+ * \frac{\partial \mathbf h_d(\cdot)}{\partial \boldsymbol\zeta} =
+ * \begin{bmatrix}
+ * x & 0 & 1 & 0 & f_x*(x_nr^2) & f_x*(x_nr^4) & f_x*(2x_ny_n) & f_x*(r^2+2x_n^2) \\[5pt]
+ * 0 & y & 0 & 1 & f_y*(y_nr^2) & f_y*(y_nr^4) & f_y*(r^2+2y_n^2) & f_y*(2x_ny_n)
+ * \end{bmatrix}
+ * \f}
+ *
+ * Similarly, the Jacobian with respect to the normalized coordinates can be obtained as follows:
+ *
+ * \f{align*}{
+ * \frac{\partial \mathbf h_d (\cdot)}{\partial \mathbf{z}_{n,k}} =
+ * \begin{bmatrix}
+ * f_x*((1+k_1r^2+k_2r^4)+(2k_1x_n^2+4k_2x_n^2(x_n^2+y_n^2))+2p_1y_n+(2p_2x_n+4p_2x_n)) &
+ * f_x*(2k_1x_ny_n+4k_2x_ny_n(x_n^2+y_n^2)+2p_1x_n+2p_2y_n) \\
+ * f_y*(2k_1x_ny_n+4k_2x_ny_n(x_n^2+y_n^2)+2p_1x_n+2p_2y_n) &
+ * f_y*((1+k_1r^2+k_2r^4)+(2k_1y_n^2+4k_2y_n^2(x_n^2+y_n^2))+(2p_1y_n+4p_1y_n)+2p_2x_n)
+ * \end{bmatrix}
+ * \f}
+ *
+ * To equate this camera class to Kalibr's models, this is what you would use for `pinhole-radtan`.
+ *
+ */
+class CamRadtan : public CamBase {
+
+public:
+ /**
+ * Default constructor.
+ */
+ CamRadtan() {}
+
+ /**
+ * @brief Given a raw uv point, this will undistort it based on the camera matrices into normalized camera coords.
+ * @param uv_dist Raw uv coordinate we wish to undistort
+ * @return 2d vector of normalized coordinates
+ */
+ Eigen::Vector2f undistort_f(const Eigen::Vector2f &uv_dist) override {
+
+ // Determine what camera parameters we should use
+ cv::Matx33d camK = camera_k_OPENCV;
+ cv::Vec4d camD = camera_d_OPENCV;
+
+ // Convert to opencv format
+ cv::Mat mat(1, 2, CV_32F);
+ mat.at(0, 0) = uv_dist(0);
+ mat.at(0, 1) = uv_dist(1);
+ mat = mat.reshape(2); // Nx1, 2-channel
+
+ // Undistort it!
+ cv::undistortPoints(mat, mat, camK, camD);
+
+ // Construct our return vector
+ Eigen::Vector2f pt_out;
+ mat = mat.reshape(1); // Nx2, 1-channel
+ pt_out(0) = mat.at(0, 0);
+ pt_out(1) = mat.at(0, 1);
+ return pt_out;
+ }
+
+ /**
+ * @brief Given a normalized uv coordinate this will distort it to the raw image plane
+ * @param uv_norm Normalized coordinates we wish to distort
+ * @return 2d vector of raw uv coordinate
+ */
+ Eigen::Vector2f distort_f(const Eigen::Vector2f &uv_norm) override {
+
+ // Get our camera parameters
+ Eigen::MatrixXd cam_d = camera_values;
+
+ // Calculate distorted coordinates for radial
+ double r = std::sqrt(uv_norm(0) * uv_norm(0) + uv_norm(1) * uv_norm(1));
+ double r_2 = r * r;
+ double r_4 = r_2 * r_2;
+ double x1 = uv_norm(0) * (1 + cam_d(4) * r_2 + cam_d(5) * r_4) + 2 * cam_d(6) * uv_norm(0) * uv_norm(1) +
+ cam_d(7) * (r_2 + 2 * uv_norm(0) * uv_norm(0));
+ double y1 = uv_norm(1) * (1 + cam_d(4) * r_2 + cam_d(5) * r_4) + cam_d(6) * (r_2 + 2 * uv_norm(1) * uv_norm(1)) +
+ 2 * cam_d(7) * uv_norm(0) * uv_norm(1);
+
+ // Return the distorted point
+ Eigen::Vector2f uv_dist;
+ uv_dist(0) = (float)(cam_d(0) * x1 + cam_d(2));
+ uv_dist(1) = (float)(cam_d(1) * y1 + cam_d(3));
+ return uv_dist;
+ }
+
+ /**
+ * @brief Computes the derivative of raw distorted to normalized coordinate.
+ * @param uv_norm Normalized coordinates we wish to distort
+ * @param H_dz_dzn Derivative of measurement z in respect to normalized
+ * @param H_dz_dzeta Derivative of measurement z in respect to intrinic parameters
+ */
+ void compute_distort_jacobian(const Eigen::Vector2d &uv_norm, Eigen::MatrixXd &H_dz_dzn, Eigen::MatrixXd &H_dz_dzeta) override {
+
+ // Get our camera parameters
+ Eigen::MatrixXd cam_d = camera_values;
+
+ // Calculate distorted coordinates for radial
+ double r = std::sqrt(uv_norm(0) * uv_norm(0) + uv_norm(1) * uv_norm(1));
+ double r_2 = r * r;
+ double r_4 = r_2 * r_2;
+
+ // Jacobian of distorted pixel to normalized pixel
+ H_dz_dzn = Eigen::MatrixXd::Zero(2, 2);
+ double x = uv_norm(0);
+ double y = uv_norm(1);
+ double x_2 = uv_norm(0) * uv_norm(0);
+ double y_2 = uv_norm(1) * uv_norm(1);
+ double x_y = uv_norm(0) * uv_norm(1);
+ H_dz_dzn(0, 0) = cam_d(0) * ((1 + cam_d(4) * r_2 + cam_d(5) * r_4) + (2 * cam_d(4) * x_2 + 4 * cam_d(5) * x_2 * r_2) +
+ 2 * cam_d(6) * y + (2 * cam_d(7) * x + 4 * cam_d(7) * x));
+ H_dz_dzn(0, 1) = cam_d(0) * (2 * cam_d(4) * x_y + 4 * cam_d(5) * x_y * r_2 + 2 * cam_d(6) * x + 2 * cam_d(7) * y);
+ H_dz_dzn(1, 0) = cam_d(1) * (2 * cam_d(4) * x_y + 4 * cam_d(5) * x_y * r_2 + 2 * cam_d(6) * x + 2 * cam_d(7) * y);
+ H_dz_dzn(1, 1) = cam_d(1) * ((1 + cam_d(4) * r_2 + cam_d(5) * r_4) + (2 * cam_d(4) * y_2 + 4 * cam_d(5) * y_2 * r_2) +
+ 2 * cam_d(7) * x + (2 * cam_d(6) * y + 4 * cam_d(6) * y));
+
+ // Calculate distorted coordinates for radtan
+ double x1 = uv_norm(0) * (1 + cam_d(4) * r_2 + cam_d(5) * r_4) + 2 * cam_d(6) * uv_norm(0) * uv_norm(1) +
+ cam_d(7) * (r_2 + 2 * uv_norm(0) * uv_norm(0));
+ double y1 = uv_norm(1) * (1 + cam_d(4) * r_2 + cam_d(5) * r_4) + cam_d(6) * (r_2 + 2 * uv_norm(1) * uv_norm(1)) +
+ 2 * cam_d(7) * uv_norm(0) * uv_norm(1);
+
+ // Compute the Jacobian in respect to the intrinsics
+ H_dz_dzeta = Eigen::MatrixXd::Zero(2, 8);
+ H_dz_dzeta(0, 0) = x1;
+ H_dz_dzeta(0, 2) = 1;
+ H_dz_dzeta(0, 4) = cam_d(0) * uv_norm(0) * r_2;
+ H_dz_dzeta(0, 5) = cam_d(0) * uv_norm(0) * r_4;
+ H_dz_dzeta(0, 6) = 2 * cam_d(0) * uv_norm(0) * uv_norm(1);
+ H_dz_dzeta(0, 7) = cam_d(0) * (r_2 + 2 * uv_norm(0) * uv_norm(0));
+ H_dz_dzeta(1, 1) = y1;
+ H_dz_dzeta(1, 3) = 1;
+ H_dz_dzeta(1, 4) = cam_d(1) * uv_norm(1) * r_2;
+ H_dz_dzeta(1, 5) = cam_d(1) * uv_norm(1) * r_4;
+ H_dz_dzeta(1, 6) = cam_d(1) * (r_2 + 2 * uv_norm(1) * uv_norm(1));
+ H_dz_dzeta(1, 7) = 2 * cam_d(1) * uv_norm(0) * uv_norm(1);
+ }
+};
+
+} // namespace ov_core
+
+#endif /* OV_CORE_CAM_RADTAN_H */
\ No newline at end of file
diff --git a/ov_core/src/dummy.cpp b/ov_core/src/dummy.cpp
index 7fc9c1974..850e49e5f 100644
--- a/ov_core/src/dummy.cpp
+++ b/ov_core/src/dummy.cpp
@@ -19,8 +19,6 @@
* along with this program. If not, see .
*/
-
-
/**
* @namespace ov_core
*
diff --git a/ov_core/src/feat/Feature.cpp b/ov_core/src/feat/Feature.cpp
index bca4b2c7b..a1f6958fe 100644
--- a/ov_core/src/feat/Feature.cpp
+++ b/ov_core/src/feat/Feature.cpp
@@ -19,7 +19,6 @@
* along with this program. If not, see .
*/
-
#include "Feature.h"
using namespace ov_core;
@@ -110,4 +109,3 @@ void Feature::clean_older_measurements(double timestamp) {
}
}
}
-
diff --git a/ov_core/src/feat/Feature.h b/ov_core/src/feat/Feature.h
index 6d8416169..98b3ea253 100644
--- a/ov_core/src/feat/Feature.h
+++ b/ov_core/src/feat/Feature.h
@@ -19,7 +19,6 @@
* along with this program. If not, see .
*/
-
#ifndef OV_CORE_FEATURE_H
#define OV_CORE_FEATURE_H
diff --git a/ov_core/src/feat/FeatureDatabase.h b/ov_core/src/feat/FeatureDatabase.h
index cad404e31..d272dc997 100644
--- a/ov_core/src/feat/FeatureDatabase.h
+++ b/ov_core/src/feat/FeatureDatabase.h
@@ -19,7 +19,6 @@
* along with this program. If not, see .
*/
-
#ifndef OV_CORE_FEATURE_DATABASE_H
#define OV_CORE_FEATURE_DATABASE_H
@@ -139,11 +138,11 @@ class FeatureDatabase {
continue;
}
// Loop through each camera
+ // If we have a measurement greater-than or equal to the specified, this measurement is find
bool has_newer_measurement = false;
for (auto const &pair : (*it).second->timestamps) {
- // If we have a measurement greater-than or equal to the specified, this measurement is find
- if (!pair.second.empty() && pair.second.at(pair.second.size() - 1) >= timestamp) {
- has_newer_measurement = true;
+ has_newer_measurement = (!pair.second.empty() && pair.second.at(pair.second.size() - 1) >= timestamp);
+ if (has_newer_measurement) {
break;
}
}
@@ -186,10 +185,11 @@ class FeatureDatabase {
continue;
}
// Loop through each camera
+ // Check if we have at least one time older then the requested
bool found_containing_older = false;
for (auto const &pair : (*it).second->timestamps) {
- if (!pair.second.empty() && pair.second.at(0) < timestamp) {
- found_containing_older = true;
+ found_containing_older = (!pair.second.empty() && pair.second.at(0) < timestamp);
+ if (found_containing_older) {
break;
}
}
@@ -232,16 +232,10 @@ class FeatureDatabase {
continue;
}
// Boolean if it has the timestamp
+ // Break out if we found a single timestamp that is equal to the specified time
bool has_timestamp = false;
for (auto const &pair : (*it).second->timestamps) {
- // Loop through all timestamps, and see if it has it
- for (auto &timefeat : pair.second) {
- if (timefeat == timestamp) {
- has_timestamp = true;
- break;
- }
- }
- // Break out if we found a single timestamp that is equal to the specified time
+ has_timestamp = (std::find(pair.second.begin(), pair.second.end(), timestamp) != pair.second.end());
if (has_timestamp) {
break;
}
@@ -272,21 +266,17 @@ class FeatureDatabase {
* If a feature was unable to be used, it will still remain since it will not have a delete flag set
*/
void cleanup() {
- // Debug
- // int sizebefore = (int)features_idlookup.size();
// Loop through all features
+ // int sizebefore = (int)features_idlookup.size();
std::unique_lock lck(mtx);
for (auto it = features_idlookup.begin(); it != features_idlookup.end();) {
// If delete flag is set, then delete it
- // NOTE: if we are using a shared pointer, then no need to do this!
if ((*it).second->to_delete) {
- // delete (*it).second;
features_idlookup.erase(it++);
} else {
it++;
}
}
- // Debug
// std::cout << "feat db = " << sizebefore << " -> " << (int)features_idlookup.size() << std::endl;
}
@@ -301,12 +291,10 @@ class FeatureDatabase {
// Count how many measurements
int ct_meas = 0;
for (const auto &pair : (*it).second->timestamps) {
- ct_meas += (*it).second->timestamps.at(pair.first).size();
+ ct_meas += (int)(pair.second.size());
}
// If delete flag is set, then delete it
- // NOTE: if we are using a shared pointer, then no need to do this!
if (ct_meas < 1) {
- // delete (*it).second;
features_idlookup.erase(it++);
} else {
it++;
@@ -326,12 +314,10 @@ class FeatureDatabase {
// Count how many measurements
int ct_meas = 0;
for (const auto &pair : (*it).second->timestamps) {
- ct_meas += (*it).second->timestamps.at(pair.first).size();
+ ct_meas += (int)(pair.second.size());
}
// If delete flag is set, then delete it
- // NOTE: if we are using a shared pointer, then no need to do this!
if (ct_meas < 1) {
- // delete (*it).second;
features_idlookup.erase(it++);
} else {
it++;
diff --git a/ov_core/src/feat/FeatureInitializer.cpp b/ov_core/src/feat/FeatureInitializer.cpp
index a78043b77..f176c5c98 100644
--- a/ov_core/src/feat/FeatureInitializer.cpp
+++ b/ov_core/src/feat/FeatureInitializer.cpp
@@ -19,7 +19,6 @@
* along with this program. If not, see .
*/
-
#include "FeatureInitializer.h"
using namespace ov_core;
@@ -89,6 +88,7 @@ bool FeatureInitializer::single_triangulation(Feature *feat, std::unordered_map<
singularValues.resize(svd.singularValues().rows(), 1);
singularValues = svd.singularValues();
double condA = singularValues(0, 0) / singularValues(singularValues.rows() - 1, 0);
+ // std::cout << feat->featid << " - cond " << std::abs(condA) << " - z " << p_f(2, 0) << std::endl;
// If we have a bad condition number, or it is too close
// Then set the flag for bad (i.e. set z-axis to nan)
@@ -345,6 +345,7 @@ bool FeatureInitializer::single_gaussnewton(Feature *feat, std::unordered_mapfeatid << " - max base " << (feat->p_FinA.norm() / base_line_max) << " - z " << feat->p_FinA(2) << std::endl;
// Check if this feature is bad or not
// 1. If the feature is too close
diff --git a/ov_core/src/feat/FeatureInitializer.h b/ov_core/src/feat/FeatureInitializer.h
index bd9e10a22..4c8312377 100644
--- a/ov_core/src/feat/FeatureInitializer.h
+++ b/ov_core/src/feat/FeatureInitializer.h
@@ -19,7 +19,6 @@
* along with this program. If not, see .
*/
-
#ifndef OPEN_VINS_FEATUREINITIALIZER_H
#define OPEN_VINS_FEATUREINITIALIZER_H
diff --git a/ov_core/src/feat/FeatureInitializerOptions.h b/ov_core/src/feat/FeatureInitializerOptions.h
index c252c7d7c..bb83f04a7 100644
--- a/ov_core/src/feat/FeatureInitializerOptions.h
+++ b/ov_core/src/feat/FeatureInitializerOptions.h
@@ -19,7 +19,6 @@
* along with this program. If not, see .
*/
-
#ifndef OV_CORE_INITIALIZEROPTIONS_H
#define OV_CORE_INITIALIZEROPTIONS_H
diff --git a/ov_core/src/init/InertialInitializer.cpp b/ov_core/src/init/InertialInitializer.cpp
index c3032bf0c..e7e471510 100644
--- a/ov_core/src/init/InertialInitializer.cpp
+++ b/ov_core/src/init/InertialInitializer.cpp
@@ -19,7 +19,6 @@
* along with this program. If not, see .
*/
-
#include "InertialInitializer.h"
using namespace ov_core;
@@ -50,7 +49,7 @@ bool InertialInitializer::initialize_with_imu(double &time0, Eigen::Matrix _imu_excite_threshold || a_var_2to1 > _imu_excite_threshold) && !wait_for_jerk) {
- printf(YELLOW "[INIT-IMU]: to much IMU excitation, above threshold %.4f,%.4f > %.4f\n" RESET, a_var_1to0, a_var_2to1, _imu_excite_threshold);
+ printf(YELLOW "[INIT-IMU]: to much IMU excitation, above threshold %.4f,%.4f > %.4f\n" RESET, a_var_1to0, a_var_2to1,
+ _imu_excite_threshold);
return false;
}
diff --git a/ov_core/src/init/InertialInitializer.h b/ov_core/src/init/InertialInitializer.h
index 441003209..1ee8ccd42 100644
--- a/ov_core/src/init/InertialInitializer.h
+++ b/ov_core/src/init/InertialInitializer.h
@@ -19,7 +19,6 @@
* along with this program. If not, see .
*/
-
#ifndef OV_CORE_INERTIALINITIALIZER_H
#define OV_CORE_INERTIALINITIALIZER_H
diff --git a/ov_core/src/sim/BsplineSE3.cpp b/ov_core/src/sim/BsplineSE3.cpp
index 9e1e35641..9069bbb8d 100644
--- a/ov_core/src/sim/BsplineSE3.cpp
+++ b/ov_core/src/sim/BsplineSE3.cpp
@@ -19,7 +19,6 @@
* along with this program. If not, see .
*/
-
#include "BsplineSE3.h"
using namespace ov_core;
diff --git a/ov_core/src/sim/BsplineSE3.h b/ov_core/src/sim/BsplineSE3.h
index cfdff9643..8d3e68fca 100644
--- a/ov_core/src/sim/BsplineSE3.h
+++ b/ov_core/src/sim/BsplineSE3.h
@@ -19,7 +19,6 @@
* along with this program. If not, see .
*/
-
#ifndef OV_CORE_BSPLINESE3_H
#define OV_CORE_BSPLINESE3_H
diff --git a/ov_core/src/test_tracking.cpp b/ov_core/src/test_tracking.cpp
index 21ca492f2..fc3f8b26f 100644
--- a/ov_core/src/test_tracking.cpp
+++ b/ov_core/src/test_tracking.cpp
@@ -19,11 +19,8 @@
* along with this program. If not, see .
*/
-
#include
#include
-#include
-#include
#include
#include
#include
@@ -37,9 +34,7 @@
#include
#include
-#include
-#include
-
+#include "cam/CamRadtan.h"
#include "track/TrackAruco.h"
#include "track/TrackDescriptor.h"
#include "track/TrackKLT.h"
@@ -60,7 +55,7 @@ std::deque clonetimes;
ros::Time time_start;
// Our master function for tracking
-void handle_stereo(double time0, double time1, cv::Mat img0, cv::Mat img1, bool use_stereo);
+void handle_stereo(double time0, double time1, cv::Mat img0, cv::Mat img1);
// Main function
int main(int argc, char **argv) {
@@ -76,7 +71,7 @@ int main(int argc, char **argv) {
// Location of the ROS bag we want to read in
std::string path_to_bag;
nh.param("path_bag", path_to_bag, "/home/patrick/datasets/eth/V1_01_easy.bag");
- // nh.param("path_bag", path_to_bag, "/home/patrick/datasets/eth/V2_03_difficult.bag");
+ // nh.param("path_bag", path_to_bag, "/home/patrick/datasets/open_vins/aruco_room_01.bag");
printf("ros bag path is: %s\n", path_to_bag.c_str());
// Get our start location and how much of the bag we want to play
@@ -90,17 +85,18 @@ int main(int argc, char **argv) {
//===================================================================================
// Parameters for our extractor
+ ov_core::TrackBase::HistogramMethod method = ov_core::TrackBase::HistogramMethod::NONE;
int num_pts, num_aruco, fast_threshold, grid_x, grid_y, min_px_dist;
double knn_ratio;
bool do_downsizing, use_stereo;
- nh.param("num_pts", num_pts, 800);
+ nh.param("num_pts", num_pts, 400);
nh.param("num_aruco", num_aruco, 1024);
nh.param("clone_states", clone_states, 11);
nh.param("fast_threshold", fast_threshold, 10);
nh.param("grid_x", grid_x, 9);
nh.param("grid_y", grid_y, 7);
- nh.param("min_px_dist", min_px_dist, 3);
- nh.param("knn_ratio", knn_ratio, 0.85);
+ nh.param("min_px_dist", min_px_dist, 10);
+ nh.param("knn_ratio", knn_ratio, 0.70);
nh.param("downsize_aruco", do_downsizing, false);
nh.param("use_stereo", use_stereo, false);
@@ -114,22 +110,20 @@ int main(int argc, char **argv) {
printf("downsize aruco image: %d\n", do_downsizing);
// Fake camera info (we don't need this, as we are not using the normalized coordinates for anything)
- Eigen::Matrix cam0_calib;
- cam0_calib << 1, 1, 0, 0, 0, 0, 0, 0;
-
- // Create our n-camera vectors
- std::map camera_fisheye;
- std::map camera_calibration;
- camera_fisheye.insert({0, false});
- camera_calibration.insert({0, cam0_calib});
- camera_fisheye.insert({1, false});
- camera_calibration.insert({1, cam0_calib});
+ std::unordered_map> cameras;
+ for (int i = 0; i < 2; i++) {
+ Eigen::Matrix cam0_calib;
+ cam0_calib << 1, 1, 0, 0, 0, 0, 0, 0;
+ std::shared_ptr camera_calib = std::make_shared();
+ camera_calib->set_value(cam0_calib);
+ cameras.insert({i, camera_calib});
+ }
// Lets make a feature extractor
- extractor = new TrackKLT(num_pts, num_aruco, fast_threshold, grid_x, grid_y, min_px_dist);
- // extractor = new TrackDescriptor(num_pts,num_aruco,true,fast_threshold,grid_x,grid_y,knn_ratio);
- // extractor = new TrackAruco(num_aruco,true,do_downsizing);
- extractor->set_calibration(camera_calibration, camera_fisheye);
+ //extractor = new TrackKLT(cameras, num_pts, num_aruco, !use_stereo, method, fast_threshold, grid_x, grid_y, min_px_dist);
+ // extractor = new TrackDescriptor(cameras, num_pts, num_aruco, !use_stereo, method, fast_threshold, grid_x, grid_y, min_px_dist,
+ // knn_ratio);
+ extractor = new TrackAruco(cameras, num_aruco, !use_stereo, method, do_downsizing);
//===================================================================================
//===================================================================================
@@ -217,7 +211,7 @@ int main(int argc, char **argv) {
// If we have both left and right, then process
if (has_left && has_right) {
// process
- handle_stereo(time0, time1, img0, img1, use_stereo);
+ handle_stereo(time0, time1, img0, img1);
// reset bools
has_left = false;
has_right = false;
@@ -231,20 +225,41 @@ int main(int argc, char **argv) {
/**
* This function will process the new stereo pair with the extractor!
*/
-void handle_stereo(double time0, double time1, cv::Mat img0, cv::Mat img1, bool use_stereo) {
+void handle_stereo(double time0, double time1, cv::Mat img0, cv::Mat img1) {
+
+ // Animate our dynamic mask moving
+ // Very simple ball bounding around the screen example
+ cv::Mat mask = cv::Mat::zeros(cv::Size(img0.cols, img0.rows), CV_8UC1);
+ static cv::Point2f ball_center;
+ static cv::Point2f ball_velocity;
+ if (ball_velocity.x == 0 || ball_velocity.y == 0) {
+ ball_center.x = (float)img0.cols / 2.0f;
+ ball_center.y = (float)img0.rows / 2.0f;
+ ball_velocity.x = 2.5;
+ ball_velocity.y = 2.5;
+ }
+ ball_center += ball_velocity;
+ if (ball_center.x < 0 || (int)ball_center.x > img0.cols)
+ ball_velocity.x *= -1;
+ if (ball_center.y < 0 || (int)ball_center.y > img0.rows)
+ ball_velocity.y *= -1;
+ cv::circle(mask, ball_center, 100, cv::Scalar(255), cv::FILLED);
// Process this new image
- if (use_stereo) {
- extractor->feed_stereo(time0, img0, img1, 0, 1);
- } else {
- extractor->feed_monocular(time0, img0, 0);
- extractor->feed_monocular(time0, img1, 1);
- }
+ ov_core::CameraData message;
+ message.timestamp = time0;
+ message.sensor_ids.push_back(0);
+ message.sensor_ids.push_back(1);
+ message.images.push_back(img0);
+ message.images.push_back(img1);
+ message.masks.push_back(mask);
+ message.masks.push_back(mask);
+ extractor->feed_new_camera(message);
// Display the resulting tracks
cv::Mat img_active, img_history;
extractor->display_active(img_active, 255, 0, 0, 0, 0, 255);
- extractor->display_history(img_history, 0, 255, 255, 255, 255, 255);
+ extractor->display_history(img_history, 255, 255, 0, 255, 255, 255);
// Show our image!
cv::imshow("Active Tracks", img_active);
diff --git a/ov_core/src/test_webcam.cpp b/ov_core/src/test_webcam.cpp
index 23a0aa6bc..3f5032c11 100644
--- a/ov_core/src/test_webcam.cpp
+++ b/ov_core/src/test_webcam.cpp
@@ -19,7 +19,6 @@
* along with this program. If not, see .
*/
-
#include
#include
#include
@@ -34,6 +33,7 @@
#include
#include
+#include "cam/CamRadtan.h"
#include "track/TrackAruco.h"
#include "track/TrackDescriptor.h"
#include "track/TrackKLT.h"
@@ -56,10 +56,12 @@ int main(int argc, char **argv) {
int clone_states = 20;
int fast_threshold = 10;
int grid_x = 5;
- int grid_y = 3;
- int min_px_dist = 10;
+ int grid_y = 4;
+ int min_px_dist = 20;
double knn_ratio = 0.85;
bool do_downsizing = false;
+ bool use_stereo = false;
+ ov_core::TrackBase::HistogramMethod method = ov_core::TrackBase::HistogramMethod::NONE;
// Parameters for our extractor
app.add_option("--num_pts", num_pts, "Number of feature tracks");
@@ -89,22 +91,19 @@ int main(int argc, char **argv) {
printf("downsize aruco image: %d\n", do_downsizing);
// Fake camera info (we don't need this, as we are not using the normalized coordinates for anything)
- Eigen::Matrix cam0_calib;
- cam0_calib << 1, 1, 0, 0, 0, 0, 0, 0;
-
- // Create our n-camera vectors
- std::map camera_fisheye;
- std::map camera_calibration;
- camera_fisheye.insert({0, false});
- camera_calibration.insert({0, cam0_calib});
- camera_fisheye.insert({1, false});
- camera_calibration.insert({1, cam0_calib});
+ std::unordered_map> cameras;
+ for (int i = 0; i < 2; i++) {
+ Eigen::Matrix cam0_calib;
+ cam0_calib << 1, 1, 0, 0, 0, 0, 0, 0;
+ std::shared_ptr camera_calib = std::make_shared();
+ camera_calib->set_value(cam0_calib);
+ cameras.insert({i, camera_calib});
+ }
// Lets make a feature extractor
- extractor = new TrackKLT(num_pts, num_aruco, fast_threshold, grid_x, grid_y, min_px_dist);
- // extractor = new TrackDescriptor(num_pts,num_aruco,true,fast_threshold,grid_x,grid_y,knn_ratio);
- // extractor = new TrackAruco(num_aruco,true,do_downsizing);
- extractor->set_calibration(camera_calibration, camera_fisheye);
+ extractor = new TrackKLT(cameras, num_pts, num_aruco, !use_stereo, method, fast_threshold, grid_x, grid_y, min_px_dist);
+ // extractor = new TrackDescriptor(cameras, num_pts, num_aruco, !use_stereo, method, fast_threshold, grid_x, grid_y, min_px_dist,
+ // knn_ratio); extractor = new TrackAruco(cameras, num_aruco, !use_stereo, method, do_downsizing);
//===================================================================================
//===================================================================================
@@ -141,15 +140,20 @@ int main(int argc, char **argv) {
// Convert to grayscale if not
if (frame.channels() != 1)
- cv::cvtColor(frame, frame, cv::COLOR_GRAY2RGB);
+ cv::cvtColor(frame, frame, cv::COLOR_RGB2GRAY);
// Else lets track this image
- extractor->feed_monocular(current_time, frame, 0);
+ ov_core::CameraData message;
+ message.timestamp = current_time;
+ message.sensor_ids.push_back(0);
+ message.images.push_back(frame);
+ message.masks.push_back(cv::Mat::zeros(cv::Size(frame.cols, frame.rows), CV_8UC1));
+ extractor->feed_new_camera(message);
// Display the resulting tracks
cv::Mat img_active, img_history;
extractor->display_active(img_active, 255, 0, 0, 0, 0, 255);
- extractor->display_history(img_history, 0, 255, 255, 255, 255, 255);
+ extractor->display_history(img_history, 255, 255, 0, 255, 255, 255);
// Show our image!
cv::imshow("Active Tracks", img_active);
diff --git a/ov_core/src/track/Grider_DOG.h b/ov_core/src/track/Grider_DOG.h
deleted file mode 100644
index 753c101ca..000000000
--- a/ov_core/src/track/Grider_DOG.h
+++ /dev/null
@@ -1,170 +0,0 @@
-/*
- * OpenVINS: An Open Platform for Visual-Inertial Research
- * Copyright (C) 2021 Patrick Geneva
- * Copyright (C) 2021 Guoquan Huang
- * Copyright (C) 2021 OpenVINS Contributors
- * Copyright (C) 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 .
- */
-
-
-#ifndef OV_CORE_GRIDER_DOG_H
-#define OV_CORE_GRIDER_DOG_H
-
-#include
-#include
-#include
-
-#include
-#include
-
-namespace ov_core {
-
-/**
- * @brief Does Difference of Gaussian (DoG) in a grid pattern.
- *
- * This does "Difference of Gaussian" detection in a grid pattern to try to get good features.
- * We then pick the top features in each grid, and return the top features collected over the entire image.
- * This class hasn't been tested that much, as we normally use the Grider_FAST class instead.
- *
- * @m_class{m-block m-danger}
- *
- * @par Improve and Test This!
- * If you are interested in using this grider, then please give it a try and test if for us.
- * Please open a pull request with any impromements, or an issue if you have had any success.
- * We still recommend the Grider_FAST class since that is what we normally use.
- *
- */
-class Grider_DOG {
-
-public:
- /**
- * @brief Compare keypoints based on their response value.
- * @param first First keypoint
- * @param second Second keypoint
- *
- * We want to have the keypoints with the highest values!
- * See: https://stackoverflow.com/a/10910921
- */
- static bool compare_response(cv::KeyPoint first, cv::KeyPoint second) { return first.response > second.response; }
-
- /**
- * @brief For a given small image region this will do the Difference of Gaussian (DOG) detection
- *
- * Will return the vector of keypoints with the averaged response for that given UV.
- * See: https://github.com/jrdi/opencv-examples/blob/master/dog/main.cpp
- */
- static void detect(const cv::Mat &img, std::vector &pts, int ksize, float sigma_small, float sigma_big, float threshold) {
-
- // Calculate the two kernels we will use
- cv::Mat gauBig = cv::getGaussianKernel(ksize, sigma_big, CV_32F);
- cv::Mat gauSmall = cv::getGaussianKernel(ksize, sigma_small, CV_32F);
- cv::Mat DoG = gauSmall - gauBig;
-
- // Apply the kernel to our image
- cv::Mat img_filtered;
- cv::sepFilter2D(img, img_filtered, CV_32F, DoG.t(), DoG);
- img_filtered = cv::abs(img_filtered);
-
- // Assert we are a grey scaled image
- assert(img_filtered.channels() == 1);
-
- // Loop through and record all points above our threshold
- for (int j = 0; j < img_filtered.rows; j++) {
- for (int i = 0; i < img_filtered.cols; i++) {
- // Calculate the response at this u,v coordinate
- float response = img_filtered.at(j, i);
- // Check to see if it is able our specified threshold
- if (response >= threshold) {
- cv::KeyPoint pt;
- pt.pt.x = i;
- pt.pt.y = j;
- pt.response = response;
- pts.push_back(pt);
- }
- }
- }
- }
-
- /**
- * @brief This function will perform grid extraction using Difference of Gaussian (DOG)
- * @param img Image we will do FAST extraction on
- * @param pts vector of extracted points we will return
- * @param num_features max number of features we want to extract
- * @param grid_x size of grid in the x-direction / u-direction
- * @param grid_y size of grid in the y-direction / v-direction
- * @param ksize kernel size
- * @param sigma_small small gaussian sigma
- * @param sigma_big big gaussian sigma
- * @param threshold response threshold
- *
- * Given a specified grid size, this will try to extract fast features from each grid.
- * It will then return the best from each grid in the return vector.
- */
- static void perform_griding(const cv::Mat &img, std::vector &pts, int num_features, int grid_x, int grid_y, int ksize,
- float sigma_small, float sigma_big, float threshold) {
-
- // Calculate the size our extraction boxes should be
- int size_x = img.cols / grid_x;
- int size_y = img.rows / grid_y;
-
- // Make sure our sizes are not zero
- assert(size_x > 0);
- assert(size_y > 0);
-
- // We want to have equally distributed features
- auto num_features_grid = (int)(num_features / (grid_x * grid_y)) + 1;
-
- // Lets loop through each grid and extract features
- for (int x = 0; x < img.cols; x += size_x) {
- for (int y = 0; y < img.rows; y += size_y) {
-
- // Skip if we are out of bounds
- if (x + size_x > img.cols || y + size_y > img.rows)
- continue;
-
- // Calculate where we should be extracting from
- cv::Rect img_roi = cv::Rect(x, y, size_x, size_y);
-
- // Extract DOG features for this part of the image
- std::vector pts_new;
- Grider_DOG::detect(img(img_roi), pts_new, ksize, sigma_small, sigma_big, threshold);
-
- // Now lets get the top number from this
- std::sort(pts_new.begin(), pts_new.end(), Grider_DOG::compare_response);
-
- // Debug print out the response values
- // for (auto pt : pts_new) {
- // std::cout << pt.response << std::endl;
- //}
- // std::cout << "======================" << std::endl;
-
- // Append the "best" ones to our vector
- // Note that we need to "correct" the point u,v since we extracted it in a ROI
- // So we should append the location of that ROI in the image
- for (size_t i = 0; i < (size_t)num_features_grid && i < pts_new.size(); i++) {
- cv::KeyPoint pt_cor = pts_new.at(i);
- pt_cor.pt.x += x;
- pt_cor.pt.y += y;
- pts.push_back(pt_cor);
- }
- }
- }
- }
-};
-
-} // namespace ov_core
-
-#endif /* OV_CORE_GRIDER_DOG_H */
\ No newline at end of file
diff --git a/ov_core/src/track/Grider_FAST.h b/ov_core/src/track/Grider_FAST.h
index 0d02caef4..9abd222e1 100644
--- a/ov_core/src/track/Grider_FAST.h
+++ b/ov_core/src/track/Grider_FAST.h
@@ -19,7 +19,6 @@
* along with this program. If not, see .
*/
-
#ifndef OV_CORE_GRIDER_FAST_H
#define OV_CORE_GRIDER_FAST_H
@@ -60,6 +59,7 @@ class Grider_FAST {
/**
* @brief This function will perform grid extraction using FAST.
* @param img Image we will do FAST extraction on
+ * @param mask Region of the image we do not want to extract features in (255 = do not detect features)
* @param pts vector of extracted points we will return
* @param num_features max number of features we want to extract
* @param grid_x size of grid in the x-direction / u-direction
@@ -70,8 +70,8 @@ class Grider_FAST {
* Given a specified grid size, this will try to extract fast features from each grid.
* It will then return the best from each grid in the return vector.
*/
- static void perform_griding(const cv::Mat &img, std::vector &pts, int num_features, int grid_x, int grid_y, int threshold,
- bool nonmaxSuppression) {
+ static void perform_griding(const cv::Mat &img, const cv::Mat &mask, std::vector &pts, int num_features, int grid_x,
+ int grid_y, int threshold, bool nonmaxSuppression) {
// Calculate the size our extraction boxes should be
int size_x = img.cols / grid_x;
@@ -112,9 +112,20 @@ class Grider_FAST {
// Note that we need to "correct" the point u,v since we extracted it in a ROI
// So we should append the location of that ROI in the image
for (size_t i = 0; i < (size_t)num_features_grid && i < pts_new.size(); i++) {
+
+ // Create keypoint
cv::KeyPoint pt_cor = pts_new.at(i);
pt_cor.pt.x += (float)x;
pt_cor.pt.y += (float)y;
+
+ // Reject if out of bounds (shouldn't be possible...)
+ if ((int)pt_cor.pt.x < 0 || (int)pt_cor.pt.x > img.cols || (int)pt_cor.pt.y < 0 || (int)pt_cor.pt.y > img.rows)
+ continue;
+
+ // Check if it is in the mask region
+ // NOTE: mask has max value of 255 (white) if it should be removed
+ if (mask.at((int)pt_cor.pt.y, (int)pt_cor.pt.x) > 127)
+ continue;
collection.at(r).push_back(pt_cor);
}
}
diff --git a/ov_core/src/track/TrackAruco.cpp b/ov_core/src/track/TrackAruco.cpp
index 5b2958f32..a7285feac 100644
--- a/ov_core/src/track/TrackAruco.cpp
+++ b/ov_core/src/track/TrackAruco.cpp
@@ -19,12 +19,39 @@
* along with this program. If not, see .
*/
-
#include "TrackAruco.h"
using namespace ov_core;
-void TrackAruco::feed_monocular(double timestamp, cv::Mat &imgin, size_t cam_id) {
+void TrackAruco::feed_new_camera(const CameraData &message) {
+
+ // Error check that we have all the data
+ if (message.sensor_ids.empty() || message.sensor_ids.size() != message.images.size() || message.images.size() != message.masks.size()) {
+ printf(RED "[ERROR]: MESSAGE DATA SIZES DO NOT MATCH OR EMPTY!!!\n" RESET);
+ printf(RED "[ERROR]: - message.sensor_ids.size() = %zu\n" RESET, message.sensor_ids.size());
+ printf(RED "[ERROR]: - message.images.size() = %zu\n" RESET, message.images.size());
+ printf(RED "[ERROR]: - message.masks.size() = %zu\n" RESET, message.masks.size());
+ std::exit(EXIT_FAILURE);
+ }
+
+ // There is not such thing as stereo tracking for aruco
+ // Thus here we should just call the monocular version two times
+#if ENABLE_ARUCO_TAGS
+ size_t num_images = message.images.size();
+ parallel_for_(cv::Range(0, (int)num_images), LambdaBody([&](const cv::Range &range) {
+ for (int i = range.start; i < range.end; i++) {
+ perform_tracking(message.timestamp, message.images.at(i), message.sensor_ids.at(i), message.masks.at(i));
+ }
+ }));
+#else
+ printf(RED "[ERROR]: you have not compiled with aruco tag support!!!\n" RESET);
+ std::exit(EXIT_FAILURE);
+#endif
+}
+
+#if ENABLE_ARUCO_TAGS
+
+void TrackAruco::perform_tracking(double timestamp, const cv::Mat &imgin, size_t cam_id, const cv::Mat &maskin) {
// Start timing
rT1 = boost::posix_time::microsec_clock::local_time();
@@ -34,7 +61,16 @@ void TrackAruco::feed_monocular(double timestamp, cv::Mat &imgin, size_t cam_id)
// Histogram equalize
cv::Mat img;
- cv::equalizeHist(imgin, img);
+ if (histogram_method == HistogramMethod::HISTOGRAM) {
+ cv::equalizeHist(imgin, img);
+ } else if (histogram_method == HistogramMethod::CLAHE) {
+ double eq_clip_limit = 10.0;
+ cv::Size eq_win_size = cv::Size(8, 8);
+ cv::Ptr clahe = cv::createCLAHE(eq_clip_limit, eq_win_size);
+ clahe->apply(imgin, img);
+ } else {
+ img = imgin;
+ }
// Clear the old data from the last timestep
ids_aruco[cam_id].clear();
@@ -90,16 +126,23 @@ void TrackAruco::feed_monocular(double timestamp, cv::Mat &imgin, size_t cam_id)
continue;
// Assert we have 4 points (we will only use one of them)
assert(corners[cam_id].at(i).size() == 4);
- // Try to undistort the point
- cv::Point2f npt_l = undistort_point(corners[cam_id].at(i).at(0), cam_id);
- // Append to the ids vector and database
- ids_new.push_back((size_t)ids_aruco[cam_id].at(i));
- database->update_feature((size_t)ids_aruco[cam_id].at(i), timestamp, cam_id, corners[cam_id].at(i).at(0).x,
- corners[cam_id].at(i).at(0).y, npt_l.x, npt_l.y);
+ for (int n = 0; n < 4; n++) {
+ // Check if it is in the mask
+ // NOTE: mask has max value of 255 (white) if it should be
+ if (maskin.at((int)corners[cam_id].at(i).at(n).y, (int)corners[cam_id].at(i).at(n).x) > 127)
+ continue;
+ // Try to undistort the point
+ cv::Point2f npt_l = camera_calib.at(cam_id)->undistort_cv(corners[cam_id].at(i).at(n));
+ // Append to the ids vector and database
+ ids_new.push_back((size_t)ids_aruco[cam_id].at(i) + n * max_tag_id);
+ database->update_feature((size_t)ids_aruco[cam_id].at(i) + n * max_tag_id, timestamp, cam_id, corners[cam_id].at(i).at(n).x,
+ corners[cam_id].at(i).at(n).y, npt_l.x, npt_l.y);
+ }
}
// Move forward in time
- img_last[cam_id] = img.clone();
+ img_last[cam_id] = img;
+ img_mask_last[cam_id] = maskin;
ids_last[cam_id] = ids_new;
rT3 = boost::posix_time::microsec_clock::local_time();
@@ -109,25 +152,16 @@ void TrackAruco::feed_monocular(double timestamp, cv::Mat &imgin, size_t cam_id)
// (int)good_left.size()); printf("[TIME-ARUCO]: %.4f seconds for total\n",(rT3-rT1).total_microseconds() * 1e-6);
}
-void TrackAruco::feed_stereo(double timestamp, cv::Mat &img_leftin, cv::Mat &img_rightin, size_t cam_id_left, size_t cam_id_right) {
-
- // There is not such thing as stereo tracking for aruco
- // Thus here we should just call the monocular version two times
- parallel_for_(cv::Range(0, 2), LambdaBody([&](const cv::Range &range) {
- for (int i = range.start; i < range.end; i++) {
- bool is_left = (i == 0);
- feed_monocular(timestamp, is_left ? img_leftin : img_rightin, is_left ? cam_id_left : cam_id_right);
- }
- }));
-}
-
void TrackAruco::display_active(cv::Mat &img_out, int r1, int g1, int b1, int r2, int g2, int b2) {
// Cache the images to prevent other threads from editing while we viz (which can be slow)
- std::map img_last_cache;
+ std::map img_last_cache, img_mask_last_cache;
for (auto const &pair : img_last) {
img_last_cache.insert({pair.first, pair.second.clone()});
}
+ for (auto const &pair : img_mask_last) {
+ img_mask_last_cache.insert({pair.first, pair.second.clone()});
+ }
// Get the largest width and height
int max_width = -1;
@@ -170,8 +204,14 @@ void TrackAruco::display_active(cv::Mat &img_out, int r1, int g1, int b1, int r2
auto txtpt = (is_small) ? cv::Point(10, 30) : cv::Point(30, 60);
cv::putText(img_temp, "CAM:" + std::to_string((int)pair.first), txtpt, cv::FONT_HERSHEY_COMPLEX_SMALL, (is_small) ? 1.5 : 3.0,
cv::Scalar(0, 255, 0), 3);
+ // Overlay the mask
+ cv::Mat mask = cv::Mat::zeros(img_mask_last_cache[pair.first].rows, img_mask_last_cache[pair.first].cols, CV_8UC3);
+ mask.setTo(cv::Scalar(0, 0, 255), img_mask_last_cache[pair.first]);
+ cv::addWeighted(mask, 0.1, img_temp, 1.0, 0.0, img_temp);
// Replace the output image
img_temp.copyTo(img_out(cv::Rect(max_width * index_cam, 0, img_last_cache[pair.first].cols, img_last_cache[pair.first].rows)));
index_cam++;
}
}
+
+#endif
\ No newline at end of file
diff --git a/ov_core/src/track/TrackAruco.h b/ov_core/src/track/TrackAruco.h
index 486dcfae6..fd99430c7 100644
--- a/ov_core/src/track/TrackAruco.h
+++ b/ov_core/src/track/TrackAruco.h
@@ -19,11 +19,12 @@
* along with this program. If not, see .
*/
-
#ifndef OV_CORE_TRACK_ARUCO_H
#define OV_CORE_TRACK_ARUCO_H
+#if ENABLE_ARUCO_TAGS
#include
+#endif
#include "TrackBase.h"
@@ -39,46 +40,35 @@ namespace ov_core {
class TrackAruco : public TrackBase {
public:
- /**
- * @brief Public default constructor
- */
- TrackAruco() : TrackBase(), max_tag_id(1024), do_downsizing(false) {
- aruco_dict = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_6X6_250);
- aruco_params = cv::aruco::DetectorParameters::create();
- // aruco_params->cornerRefinementMethod = cv::aruco::CornerRefineMethod::CORNER_REFINE_SUBPIX; // people with newer opencv might fail
- // here
- }
-
/**
* @brief Public constructor with configuration variables
+ * @param cameras camera calibration object which has all camera intrinsics in it
* @param numaruco the max id of the arucotags, we don't use any tags greater than this value even if we extract them
- * @param do_downsizing we can scale the image by 1/2 to increase Aruco tag extraction speed
+ * @param binocular if we should do binocular feature tracking or stereo if there are multiple cameras
+ * @param histmethod what type of histogram pre-processing should be done (histogram eq?)
+ * @param downsize we can scale the image by 1/2 to increase Aruco tag extraction speed
*/
- explicit TrackAruco(int numaruco, bool do_downsizing) : TrackBase(0, numaruco), max_tag_id(numaruco), do_downsizing(do_downsizing) {
+ explicit TrackAruco(std::unordered_map> cameras, int numaruco, bool binocular,
+ HistogramMethod histmethod, bool downsize)
+ : TrackBase(cameras, 0, numaruco, binocular, histmethod), max_tag_id(numaruco), do_downsizing(downsize) {
+#if ENABLE_ARUCO_TAGS
aruco_dict = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_6X6_250);
aruco_params = cv::aruco::DetectorParameters::create();
- // aruco_params->cornerRefinementMethod = cv::aruco::CornerRefineMethod::CORNER_REFINE_SUBPIX; // people with newer opencv might fail
- // here
+ // NOTE: people with newer opencv might fail here
+ // aruco_params->cornerRefinementMethod = cv::aruco::CornerRefineMethod::CORNER_REFINE_SUBPIX;
+#else
+ printf(RED "[ERROR]: you have not compiled with aruco tag support!!!\n" RESET);
+ std::exit(EXIT_FAILURE);
+#endif
}
/**
- * @brief Process a new monocular image
- * @param timestamp timestamp the new image occurred at
- * @param img new cv:Mat grayscale image
- * @param cam_id the camera id that this new image corresponds too
- */
- void feed_monocular(double timestamp, cv::Mat &img, size_t cam_id) override;
-
- /**
- * @brief Process new stereo pair of images
- * @param timestamp timestamp this pair occured at (stereo is synchronised)
- * @param img_left first grayscaled image
- * @param img_right second grayscaled image
- * @param cam_id_left first image camera id
- * @param cam_id_right second image camera id
+ * @brief Process a new image
+ * @param message Contains our timestamp, images, and camera ids
*/
- void feed_stereo(double timestamp, cv::Mat &img_left, cv::Mat &img_right, size_t cam_id_left, size_t cam_id_right) override;
+ void feed_new_camera(const CameraData &message);
+#if ENABLE_ARUCO_TAGS
/**
* @brief We override the display equation so we can show the tags we extract.
* @param img_out image to which we will overlayed features on
@@ -86,10 +76,19 @@ class TrackAruco : public TrackBase {
* @param r2,g2,b2 second color to draw in
*/
void display_active(cv::Mat &img_out, int r1, int g1, int b1, int r2, int g2, int b2) override;
+#endif
protected:
- // Timing variables
- boost::posix_time::ptime rT1, rT2, rT3, rT4, rT5, rT6, rT7;
+#if ENABLE_ARUCO_TAGS
+ /**
+ * @brief Process a new monocular image
+ * @param timestamp timestamp the new image occurred at
+ * @param imgin new cv:Mat grayscale image
+ * @param cam_id the camera id that this new image corresponds too
+ * @param maskin tracking mask for the given input image
+ */
+ void perform_tracking(double timestamp, const cv::Mat &imgin, size_t cam_id, const cv::Mat &maskin);
+#endif
// Max tag ID we should extract from (i.e., number of aruco tags starting from zero)
int max_tag_id;
@@ -97,18 +96,18 @@ class TrackAruco : public TrackBase {
// If we should downsize the image
bool do_downsizing;
+#if ENABLE_ARUCO_TAGS
// Our dictionary that we will extract aruco tags with
cv::Ptr aruco_dict;
// Parameters the opencv extractor uses
cv::Ptr aruco_params;
- // Mutex for our ids_aruco, corners, rejects which we use for drawing
- std::mutex mtx_aruco;
-
// Our tag IDs and corner we will get from the extractor
std::unordered_map> ids_aruco;
std::unordered_map>> corners, rejects;
+#endif
+
};
} // namespace ov_core
diff --git a/ov_core/src/track/TrackBase.cpp b/ov_core/src/track/TrackBase.cpp
index e152cf050..77246df1b 100644
--- a/ov_core/src/track/TrackBase.cpp
+++ b/ov_core/src/track/TrackBase.cpp
@@ -19,7 +19,6 @@
* along with this program. If not, see .
*/
-
#include "TrackBase.h"
using namespace ov_core;
@@ -27,10 +26,13 @@ using namespace ov_core;
void TrackBase::display_active(cv::Mat &img_out, int r1, int g1, int b1, int r2, int g2, int b2) {
// Cache the images to prevent other threads from editing while we viz (which can be slow)
- std::map img_last_cache;
+ std::map img_last_cache, img_mask_last_cache;
for (auto const &pair : img_last) {
img_last_cache.insert({pair.first, pair.second.clone()});
}
+ for (auto const &pair : img_mask_last) {
+ img_mask_last_cache.insert({pair.first, pair.second.clone()});
+ }
// Get the largest width and height
int max_width = -1;
@@ -84,6 +86,10 @@ void TrackBase::display_active(cv::Mat &img_out, int r1, int g1, int b1, int r2,
auto txtpt = (is_small) ? cv::Point(10, 30) : cv::Point(30, 60);
cv::putText(img_temp, "CAM:" + std::to_string((int)pair.first), txtpt, cv::FONT_HERSHEY_COMPLEX_SMALL, (is_small) ? 1.5 : 3.0,
cv::Scalar(0, 255, 0), 3);
+ // Overlay the mask
+ cv::Mat mask = cv::Mat::zeros(img_mask_last_cache[pair.first].rows, img_mask_last_cache[pair.first].cols, CV_8UC3);
+ mask.setTo(cv::Scalar(0, 0, 255), img_mask_last_cache[pair.first]);
+ cv::addWeighted(mask, 0.1, img_temp, 1.0, 0.0, img_temp);
// Replace the output image
img_temp.copyTo(img_out(cv::Rect(max_width * index_cam, 0, img_last_cache[pair.first].cols, img_last_cache[pair.first].rows)));
index_cam++;
@@ -93,10 +99,13 @@ void TrackBase::display_active(cv::Mat &img_out, int r1, int g1, int b1, int r2,
void TrackBase::display_history(cv::Mat &img_out, int r1, int g1, int b1, int r2, int g2, int b2, std::vector highlighted) {
// Cache the images to prevent other threads from editing while we viz (which can be slow)
- std::map img_last_cache;
+ std::map img_last_cache, img_mask_last_cache;
for (auto const &pair : img_last) {
img_last_cache.insert({pair.first, pair.second.clone()});
}
+ for (auto const &pair : img_mask_last) {
+ img_mask_last_cache.insert({pair.first, pair.second.clone()});
+ }
// Get the largest width and height
int max_width = -1;
@@ -181,6 +190,10 @@ void TrackBase::display_history(cv::Mat &img_out, int r1, int g1, int b1, int r2
auto txtpt = (is_small) ? cv::Point(10, 30) : cv::Point(30, 60);
cv::putText(img_temp, "CAM:" + std::to_string((int)pair.first), txtpt, cv::FONT_HERSHEY_COMPLEX_SMALL, (is_small) ? 1.5 : 3.0,
cv::Scalar(0, 255, 0), 3);
+ // Overlay the mask
+ cv::Mat mask = cv::Mat::zeros(img_mask_last_cache[pair.first].rows, img_mask_last_cache[pair.first].cols, CV_8UC3);
+ mask.setTo(cv::Scalar(0, 0, 255), img_mask_last_cache[pair.first]);
+ cv::addWeighted(mask, 0.1, img_temp, 1.0, 0.0, img_temp);
// Replace the output image
img_temp.copyTo(img_out(cv::Rect(max_width * index_cam, 0, img_last_cache[pair.first].cols, img_last_cache[pair.first].rows)));
index_cam++;
diff --git a/ov_core/src/track/TrackBase.h b/ov_core/src/track/TrackBase.h
index 77fedf0f9..92c16dbb5 100644
--- a/ov_core/src/track/TrackBase.h
+++ b/ov_core/src/track/TrackBase.h
@@ -19,7 +19,6 @@
* along with this program. If not, see .
*/
-
#ifndef OV_CORE_TRACK_BASE_H
#define OV_CORE_TRACK_BASE_H
@@ -29,16 +28,17 @@
#include
#include
-#include
+#include
#include
#include
#include
-#include "Grider_DOG.h"
#include "Grider_FAST.h"
+#include "cam/CamBase.h"
#include "feat/FeatureDatabase.h"
#include "utils/colors.h"
#include "utils/lambda_body.h"
+#include "utils/sensor_data.h"
namespace ov_core {
@@ -72,152 +72,38 @@ class TrackBase {
public:
/**
- * @brief Public default constructor
+ * @brief Desired pre-processing image method.
*/
- TrackBase() : database(new FeatureDatabase()), num_features(200), currid(0) {}
+ enum HistogramMethod { NONE, HISTOGRAM, CLAHE };
/**
* @brief Public constructor with configuration variables
+ * @param cameras camera calibration object which has all camera intrinsics in it
* @param numfeats number of features we want want to track (i.e. track 200 points from frame to frame)
* @param numaruco the max id of the arucotags, so we ensure that we start our non-auroc features above this value
+ * @param stereo if we should do stereo feature tracking or binocular
+ * @param histmethod what type of histogram pre-processing should be done (histogram eq?)
*/
- TrackBase(int numfeats, int numaruco) : database(new FeatureDatabase()), num_features(numfeats) {
- // Our current feature ID should be larger then the number of aruco tags we have
- currid = (size_t)numaruco + 1;
- }
-
- virtual ~TrackBase() {}
-
- /**
- * @brief Given a the camera intrinsic values, this will set what we should normalize points with.
- * This will also update the feature database with corrected normalized values.
- * Normally this would only be needed if we are optimizing our camera parameters, and thus should re-normalize.
- * @param camera_calib Calibration parameters for all cameras [fx,fy,cx,cy,d1,d2,d3,d4]
- * @param camera_fisheye Map of camera_id => bool if we should do radtan or fisheye distortion model
- * @param correct_active If we should re-undistort active features in our database
- */
- void set_calibration(std::map camera_calib, std::map camera_fisheye, bool correct_active = false) {
-
- // Assert vectors are equal
- assert(camera_calib.size() == camera_fisheye.size());
-
+ TrackBase(std::unordered_map> cameras, int numfeats, int numaruco, bool stereo,
+ HistogramMethod histmethod)
+ : camera_calib(cameras), database(new FeatureDatabase()), num_features(numfeats), use_stereo(stereo), histogram_method(histmethod) {
+ // Our current feature ID should be larger then the number of aruco tags we have (each has 4 corners)
+ currid = 4 * (size_t)numaruco + 1;
// Create our mutex array based on the number of cameras we have
// See https://stackoverflow.com/a/24170141/7718197
if (mtx_feeds.empty() || mtx_feeds.size() != camera_calib.size()) {
std::vector list(camera_calib.size());
mtx_feeds.swap(list);
}
-
- // Initialize our mutex and camera intrinsic values if we are just starting up
- // The number of cameras should not change over time, thus we just need to check if our initial data is empty
- if (camera_k_OPENCV.empty() || camera_d_OPENCV.empty() || this->camera_fisheye.empty()) {
- // Overwrite our fisheye calibration
- this->camera_fisheye = camera_fisheye;
- // Convert values to the OpenCV format
- for (auto const &cam : camera_calib) {
- // Assert we are of size eight
- assert(cam.second.rows() == 8);
- // Camera matrix
- cv::Matx33d tempK;
- tempK(0, 0) = cam.second(0);
- tempK(0, 1) = 0;
- tempK(0, 2) = cam.second(2);
- tempK(1, 0) = 0;
- tempK(1, 1) = cam.second(1);
- tempK(1, 2) = cam.second(3);
- tempK(2, 0) = 0;
- tempK(2, 1) = 0;
- tempK(2, 2) = 1;
- camera_k_OPENCV.insert({cam.first, tempK});
- // Distortion parameters
- cv::Vec4d tempD;
- tempD(0) = cam.second(4);
- tempD(1) = cam.second(5);
- tempD(2) = cam.second(6);
- tempD(3) = cam.second(7);
- camera_d_OPENCV.insert({cam.first, tempD});
- }
- return;
- }
-
- // assert that the number of cameras can not change
- assert(camera_k_OPENCV.size() == camera_calib.size());
- assert(camera_k_OPENCV.size() == camera_fisheye.size());
- assert(camera_k_OPENCV.size() == camera_d_OPENCV.size());
-
- // Convert values to the OpenCV format
- for (auto const &cam : camera_calib) {
- // Lock this image feed
- std::unique_lock lck(mtx_feeds.at(cam.first));
- // Fisheye value
- this->camera_fisheye.at(cam.first) = camera_fisheye.at(cam.first);
- // Assert we are of size eight
- assert(cam.second.rows() == 8);
- // Camera matrix
- cv::Matx33d tempK;
- tempK(0, 0) = cam.second(0);
- tempK(0, 1) = 0;
- tempK(0, 2) = cam.second(2);
- tempK(1, 0) = 0;
- tempK(1, 1) = cam.second(1);
- tempK(1, 2) = cam.second(3);
- tempK(2, 0) = 0;
- tempK(2, 1) = 0;
- tempK(2, 2) = 1;
- camera_k_OPENCV.at(cam.first) = tempK;
- // Distortion parameters
- cv::Vec4d tempD;
- tempD(0) = cam.second(4);
- tempD(1) = cam.second(5);
- tempD(2) = cam.second(6);
- tempD(3) = cam.second(7);
- camera_d_OPENCV.at(cam.first) = tempD;
- }
-
- // If we are calibrating camera intrinsics our normalize coordinates will be stale
- // This is because we appended them to the database with the current best guess *at that timestep*
- // Thus here since we have a change in calibration, re-normalize all the features we have
- if (correct_active) {
-
- // Get all features in this database
- std::unordered_map> features_idlookup = database->get_internal_data();
-
- // Loop through and correct each one
- for (const auto &pair_feat : features_idlookup) {
- // Get our feature
- std::shared_ptr feat = pair_feat.second;
- // Loop through each camera for this feature
- for (auto const &meas_pair : feat->timestamps) {
- size_t camid = meas_pair.first;
- std::unique_lock lck(mtx_feeds.at(camid));
- for (size_t m = 0; m < feat->uvs.at(camid).size(); m++) {
- cv::Point2f pt(feat->uvs.at(camid).at(m)(0), feat->uvs.at(camid).at(m)(1));
- cv::Point2f pt_n = undistort_point(pt, camid);
- feat->uvs_norm.at(camid).at(m)(0) = pt_n.x;
- feat->uvs_norm.at(camid).at(m)(1) = pt_n.y;
- }
- }
- }
- }
}
- /**
- * @brief Process a new monocular image
- * @param timestamp timestamp the new image occurred at
- * @param img new cv:Mat grayscale image
- * @param cam_id the camera id that this new image corresponds too
- */
- virtual void feed_monocular(double timestamp, cv::Mat &img, size_t cam_id) = 0;
+ virtual ~TrackBase() {}
/**
- * @brief Process new stereo pair of images
- * @param timestamp timestamp this pair occured at (stereo is synchronised)
- * @param img_left first grayscaled image
- * @param img_right second grayscaled image
- * @param cam_id_left first image camera id
- * @param cam_id_right second image camera id
+ * @brief Process a new image
+ * @param message Contains our timestamp, images, and camera ids
*/
- virtual void feed_stereo(double timestamp, cv::Mat &img_left, cv::Mat &img_right, size_t cam_id_left, size_t cam_id_right) = 0;
+ virtual void feed_new_camera(const CameraData &message) = 0;
/**
* @brief Shows features extracted in the last image
@@ -232,7 +118,7 @@ class TrackBase {
* @param img_out image to which we will overlayed features on
* @param r1,g1,b1 first color to draw in
* @param r2,g2,b2 second color to draw in
- * @param highlighted unique ids which we wish to highlight
+ * @param highlighted unique ids which we wish to highlight (e.g. slam feats)
*/
virtual void display_history(cv::Mat &img_out, int r1, int g1, int b1, int r2, int g2, int b2, std::vector highlighted = {});
@@ -243,7 +129,11 @@ class TrackBase {
std::shared_ptr get_feature_database() { return database; }
/**
- * @brief Changes the ID of an actively tracked feature to another one
+ * @brief Changes the ID of an actively tracked feature to another one.
+ *
+ * This function can be helpfull if you detect a loop-closure with an old frame.
+ * One could then change the id of an active feature to match the old feature id!
+ *
* @param id_old Old id we want to change
* @param id_new Id we want to change the old id to
*/
@@ -267,72 +157,9 @@ class TrackBase {
}
}
- /**
- * @brief Main function that will undistort/normalize a point.
- * @param pt_in uv 2x1 point that we will undistort
- * @param cam_id id of which camera this point is in
- * @return undistorted 2x1 point
- *
- * Given a uv point, this will undistort it based on the camera matrices.
- * This will call on the model needed, depending on what type of camera it is!
- * So if we have fisheye for camera_1 is true, we will undistort with the fisheye model.
- * In Kalibr's terms, the non-fisheye is `pinhole-radtan` while the fisheye is the `pinhole-equi` model.
- */
- cv::Point2f undistort_point(cv::Point2f pt_in, size_t cam_id) {
- // Determine what camera parameters we should use
- cv::Matx33d camK = this->camera_k_OPENCV.at(cam_id);
- cv::Vec4d camD = this->camera_d_OPENCV.at(cam_id);
- // Call on the fisheye if we should!
- if (this->camera_fisheye.at(cam_id)) {
- return undistort_point_fisheye(pt_in, camK, camD);
- }
- return undistort_point_brown(pt_in, camK, camD);
- }
-
protected:
- /**
- * @brief Undistort function RADTAN/BROWN.
- *
- * Given a uv point, this will undistort it based on the camera matrices.
- * To equate this to Kalibr's models, this is what you would use for `pinhole-radtan`.
- */
- static cv::Point2f undistort_point_brown(cv::Point2f pt_in, cv::Matx33d &camK, cv::Vec4d &camD) {
- // Convert to opencv format
- cv::Mat mat(1, 2, CV_32F);
- mat.at(0, 0) = pt_in.x;
- mat.at(0, 1) = pt_in.y;
- mat = mat.reshape(2); // Nx1, 2-channel
- // Undistort it!
- cv::undistortPoints(mat, mat, camK, camD);
- // Construct our return vector
- cv::Point2f pt_out;
- mat = mat.reshape(1); // Nx2, 1-channel
- pt_out.x = mat.at(0, 0);
- pt_out.y = mat.at(0, 1);
- return pt_out;
- }
-
- /**
- * @brief Undistort function FISHEYE/EQUIDISTANT.
- *
- * Given a uv point, this will undistort it based on the camera matrices.
- * To equate this to Kalibr's models, this is what you would use for `pinhole-equi`.
- */
- static cv::Point2f undistort_point_fisheye(cv::Point2f pt_in, cv::Matx33d &camK, cv::Vec4d &camD) {
- // Convert point to opencv format
- cv::Mat mat(1, 2, CV_32F);
- mat.at(0, 0) = pt_in.x;
- mat.at(0, 1) = pt_in.y;
- mat = mat.reshape(2); // Nx1, 2-channel
- // Undistort it!
- cv::fisheye::undistortPoints(mat, mat, camK, camD);
- // Construct our return vector
- cv::Point2f pt_out;
- mat = mat.reshape(1); // Nx2, 1-channel
- pt_out.x = mat.at(0, 0);
- pt_out.y = mat.at(0, 1);
- return pt_out;
- }
+ /// Camera object which has all calibration in it
+ std::unordered_map> camera_calib;
/// Database with all our current features
std::shared_ptr database;
@@ -340,21 +167,24 @@ class TrackBase {
/// If we are a fisheye model or not
std::map camera_fisheye;
- /// Camera intrinsics in OpenCV format
- std::map camera_k_OPENCV;
-
- /// Camera distortion in OpenCV format
- std::map camera_d_OPENCV;
-
/// Number of features we should try to track frame to frame
int num_features;
+ /// If we should use binocular tracking or stereo tracking for multi-camera
+ bool use_stereo;
+
+ /// What histogram equalization method we should pre-process images with?
+ HistogramMethod histogram_method;
+
/// Mutexs for our last set of image storage (img_last, pts_last, and ids_last)
std::vector mtx_feeds;
/// Last set of images (use map so all trackers render in the same order)
std::map img_last;
+ /// Last set of images (use map so all trackers render in the same order)
+ std::map img_mask_last;
+
/// Last set of tracked points
std::unordered_map> pts_last;
@@ -363,6 +193,9 @@ class TrackBase {
/// Master ID for this tracker (atomic to allow for multi-threading)
std::atomic currid;
+
+ // Timing variables (most children use these...)
+ boost::posix_time::ptime rT1, rT2, rT3, rT4, rT5, rT6, rT7;
};
} // namespace ov_core
diff --git a/ov_core/src/track/TrackDescriptor.cpp b/ov_core/src/track/TrackDescriptor.cpp
index 30e78bd88..890faa23f 100644
--- a/ov_core/src/track/TrackDescriptor.cpp
+++ b/ov_core/src/track/TrackDescriptor.cpp
@@ -19,27 +19,68 @@
* along with this program. If not, see .
*/
-
#include "TrackDescriptor.h"
using namespace ov_core;
-void TrackDescriptor::feed_monocular(double timestamp, cv::Mat &imgin, size_t cam_id) {
+void TrackDescriptor::feed_new_camera(const CameraData &message) {
+
+ // Error check that we have all the data
+ if (message.sensor_ids.empty() || message.sensor_ids.size() != message.images.size() || message.images.size() != message.masks.size()) {
+ printf(RED "[ERROR]: MESSAGE DATA SIZES DO NOT MATCH OR EMPTY!!!\n" RESET);
+ printf(RED "[ERROR]: - message.sensor_ids.size() = %zu\n" RESET, message.sensor_ids.size());
+ printf(RED "[ERROR]: - message.images.size() = %zu\n" RESET, message.images.size());
+ printf(RED "[ERROR]: - message.masks.size() = %zu\n" RESET, message.masks.size());
+ std::exit(EXIT_FAILURE);
+ }
+
+ // Either call our stereo or monocular version
+ // If we are doing binocular tracking, then we should parallize our tracking
+ size_t num_images = message.images.size();
+ if (num_images == 1) {
+ feed_monocular(message, 0);
+ } else if (num_images == 2 && use_stereo) {
+ feed_stereo(message, 0, 1);
+ } else if (!use_stereo) {
+ parallel_for_(cv::Range(0, (int)num_images), LambdaBody([&](const cv::Range &range) {
+ for (int i = range.start; i < range.end; i++) {
+ feed_monocular(message, i);
+ }
+ }));
+ } else {
+ printf(RED "[ERROR]: invalid number of images passed %zu, we only support mono or stereo tracking", num_images);
+ std::exit(EXIT_FAILURE);
+ }
+}
+
+void TrackDescriptor::feed_monocular(const CameraData &message, size_t msg_id) {
// Start timing
rT1 = boost::posix_time::microsec_clock::local_time();
// Lock this data feed for this camera
+ size_t cam_id = message.sensor_ids.at(msg_id);
std::unique_lock lck(mtx_feeds.at(cam_id));
// Histogram equalize
- cv::Mat img;
- cv::equalizeHist(imgin, img);
+ cv::Mat img, mask;
+ if (histogram_method == HistogramMethod::HISTOGRAM) {
+ cv::equalizeHist(message.images.at(msg_id), img);
+ } else if (histogram_method == HistogramMethod::CLAHE) {
+ double eq_clip_limit = 10.0;
+ cv::Size eq_win_size = cv::Size(8, 8);
+ cv::Ptr clahe = cv::createCLAHE(eq_clip_limit, eq_win_size);
+ clahe->apply(message.images.at(msg_id), img);
+ } else {
+ img = message.images.at(msg_id);
+ }
+ mask = message.masks.at(msg_id);
// If we are the first frame (or have lost tracking), initialize our descriptors
if (pts_last.find(cam_id) == pts_last.end() || pts_last[cam_id].empty()) {
- perform_detection_monocular(img, pts_last[cam_id], desc_last[cam_id], ids_last[cam_id]);
- img_last[cam_id] = img.clone();
+ perform_detection_monocular(img, mask, pts_last[cam_id], desc_last[cam_id], ids_last[cam_id]);
+ img_last[cam_id] = img;
+ img_mask_last[cam_id] = mask;
return;
}
@@ -49,12 +90,9 @@ void TrackDescriptor::feed_monocular(double timestamp, cv::Mat &imgin, size_t ca
std::vector ids_new;
// First, extract new descriptors for this new image
- perform_detection_monocular(img, pts_new, desc_new, ids_new);
+ perform_detection_monocular(img, mask, pts_new, desc_new, ids_new);
rT2 = boost::posix_time::microsec_clock::local_time();
- //===================================================================================
- //===================================================================================
-
// Our matches temporally
std::vector matches_ll;
@@ -62,9 +100,6 @@ void TrackDescriptor::feed_monocular(double timestamp, cv::Mat &imgin, size_t ca
robust_match(pts_last[cam_id], pts_new, desc_last[cam_id], desc_new, cam_id, cam_id, matches_ll);
rT3 = boost::posix_time::microsec_clock::local_time();
- //===================================================================================
- //===================================================================================
-
// Get our "good tracks"
std::vector good_left;
std::vector good_ids_left;
@@ -86,37 +121,31 @@ void TrackDescriptor::feed_monocular(double timestamp, cv::Mat &imgin, size_t ca
}
}
- // If we found a good stereo track from left to left, and right to right
- // Then lets replace the current ID with the old ID
- // We also check that we are linked to the same past ID value
+ // Then lets replace the current ID with the old ID if found
+ // Else just append the current feature and its unique ID
+ good_left.push_back(pts_new[i]);
+ good_desc_left.push_back(desc_new.row((int)i));
if (idll != -1) {
- good_left.push_back(pts_new[i]);
- good_desc_left.push_back(desc_new.row((int)i));
good_ids_left.push_back(ids_last[cam_id][idll]);
num_tracklast++;
} else {
- // Else just append the current feature and its unique ID
- good_left.push_back(pts_new[i]);
- good_desc_left.push_back(desc_new.row((int)i));
good_ids_left.push_back(ids_new[i]);
}
}
rT4 = boost::posix_time::microsec_clock::local_time();
- //===================================================================================
- //===================================================================================
-
// Update our feature database, with theses new observations
for (size_t i = 0; i < good_left.size(); i++) {
- cv::Point2f npt_l = undistort_point(good_left.at(i).pt, cam_id);
- database->update_feature(good_ids_left.at(i), timestamp, cam_id, good_left.at(i).pt.x, good_left.at(i).pt.y, npt_l.x, npt_l.y);
+ cv::Point2f npt_l = camera_calib.at(cam_id)->undistort_cv(good_left.at(i).pt);
+ database->update_feature(good_ids_left.at(i), message.timestamp, cam_id, good_left.at(i).pt.x, good_left.at(i).pt.y, npt_l.x, npt_l.y);
}
// Debug info
// printf("LtoL = %d | good = %d | fromlast = %d\n",(int)matches_ll.size(),(int)good_left.size(),num_tracklast);
// Move forward in time
- img_last[cam_id] = img.clone();
+ img_last[cam_id] = img;
+ img_mask_last[cam_id] = mask;
pts_last[cam_id] = good_left;
ids_last[cam_id] = good_ids_left;
desc_last[cam_id] = good_desc_left;
@@ -130,26 +159,44 @@ void TrackDescriptor::feed_monocular(double timestamp, cv::Mat &imgin, size_t ca
// printf("[TIME-DESC]: %.4f seconds for total\n",(rT5-rT1).total_microseconds() * 1e-6);
}
-void TrackDescriptor::feed_stereo(double timestamp, cv::Mat &img_leftin, cv::Mat &img_rightin, size_t cam_id_left, size_t cam_id_right) {
+void TrackDescriptor::feed_stereo(const CameraData &message, size_t msg_id_left, size_t msg_id_right) {
// Start timing
rT1 = boost::posix_time::microsec_clock::local_time();
// Lock this data feed for this camera
+ size_t cam_id_left = message.sensor_ids.at(msg_id_left);
+ size_t cam_id_right = message.sensor_ids.at(msg_id_right);
std::unique_lock lck1(mtx_feeds.at(cam_id_left));
std::unique_lock lck2(mtx_feeds.at(cam_id_right));
- // Histogram equalize
- cv::Mat img_left, img_right;
- cv::equalizeHist(img_leftin, img_left);
- cv::equalizeHist(img_rightin, img_right);
+ // Histogram equalize images
+ cv::Mat img_left, img_right, mask_left, mask_right;
+ if (histogram_method == HistogramMethod::HISTOGRAM) {
+ cv::equalizeHist(message.images.at(msg_id_left), img_left);
+ cv::equalizeHist(message.images.at(msg_id_right), img_right);
+ } else if (histogram_method == HistogramMethod::CLAHE) {
+ double eq_clip_limit = 10.0;
+ cv::Size eq_win_size = cv::Size(8, 8);
+ cv::Ptr clahe = cv::createCLAHE(eq_clip_limit, eq_win_size);
+ clahe->apply(message.images.at(msg_id_left), img_left);
+ clahe->apply(message.images.at(msg_id_right), img_right);
+ } else {
+ img_left = message.images.at(msg_id_left);
+ img_right = message.images.at(msg_id_right);
+ }
+ mask_left = message.masks.at(msg_id_left);
+ mask_right = message.masks.at(msg_id_right);
// If we are the first frame (or have lost tracking), initialize our descriptors
if (pts_last[cam_id_left].empty() || pts_last[cam_id_right].empty()) {
- perform_detection_stereo(img_left, img_right, pts_last[cam_id_left], pts_last[cam_id_right], desc_last[cam_id_left],
- desc_last[cam_id_right], cam_id_left, cam_id_right, ids_last[cam_id_left], ids_last[cam_id_right]);
- img_last[cam_id_left] = img_left.clone();
- img_last[cam_id_right] = img_right.clone();
+ perform_detection_stereo(img_left, img_right, mask_left, mask_right, pts_last[cam_id_left], pts_last[cam_id_right],
+ desc_last[cam_id_left], desc_last[cam_id_right], cam_id_left, cam_id_right, ids_last[cam_id_left],
+ ids_last[cam_id_right]);
+ img_last[cam_id_left] = img_left;
+ img_last[cam_id_right] = img_right;
+ img_mask_last[cam_id_left] = mask_left;
+ img_mask_last[cam_id_right] = mask_right;
return;
}
@@ -159,13 +206,10 @@ void TrackDescriptor::feed_stereo(double timestamp, cv::Mat &img_leftin, cv::Mat
std::vector ids_left_new, ids_right_new;
// First, extract new descriptors for this new image
- perform_detection_stereo(img_left, img_right, pts_left_new, pts_right_new, desc_left_new, desc_right_new, cam_id_left, cam_id_right,
- ids_left_new, ids_right_new);
+ perform_detection_stereo(img_left, img_right, mask_left, mask_right, pts_left_new, pts_right_new, desc_left_new, desc_right_new,
+ cam_id_left, cam_id_right, ids_left_new, ids_right_new);
rT2 = boost::posix_time::microsec_clock::local_time();
- //===================================================================================
- //===================================================================================
-
// Our matches temporally
std::vector matches_ll, matches_rr;
parallel_for_(cv::Range(0, 2), LambdaBody([&](const cv::Range &range) {
@@ -179,9 +223,6 @@ void TrackDescriptor::feed_stereo(double timestamp, cv::Mat &img_leftin, cv::Mat
}));
rT3 = boost::posix_time::microsec_clock::local_time();
- //===================================================================================
- //===================================================================================
-
// Get our "good tracks"
std::vector good_left, good_right;
std::vector good_ids_left, good_ids_right;
@@ -246,11 +287,13 @@ void TrackDescriptor::feed_stereo(double timestamp, cv::Mat &img_leftin, cv::Mat
// Assert that our IDs are the same
assert(good_ids_left.at(i) == good_ids_right.at(i));
// Try to undistort the point
- cv::Point2f npt_l = undistort_point(good_left.at(i).pt, cam_id_left);
- cv::Point2f npt_r = undistort_point(good_right.at(i).pt, cam_id_right);
+ cv::Point2f npt_l = camera_calib.at(cam_id_left)->undistort_cv(good_left.at(i).pt);
+ cv::Point2f npt_r = camera_calib.at(cam_id_right)->undistort_cv(good_right.at(i).pt);
// Append to the database
- database->update_feature(good_ids_left.at(i), timestamp, cam_id_left, good_left.at(i).pt.x, good_left.at(i).pt.y, npt_l.x, npt_l.y);
- database->update_feature(good_ids_left.at(i), timestamp, cam_id_right, good_right.at(i).pt.x, good_right.at(i).pt.y, npt_r.x, npt_r.y);
+ database->update_feature(good_ids_left.at(i), message.timestamp, cam_id_left, good_left.at(i).pt.x, good_left.at(i).pt.y, npt_l.x,
+ npt_l.y);
+ database->update_feature(good_ids_left.at(i), message.timestamp, cam_id_right, good_right.at(i).pt.x, good_right.at(i).pt.y, npt_r.x,
+ npt_r.y);
}
// Debug info
@@ -258,8 +301,10 @@ void TrackDescriptor::feed_stereo(double timestamp, cv::Mat &img_leftin, cv::Mat
// (int)matches_rr.size(),(int)ids_left_new.size(),(int)good_left.size(),num_tracklast);
// Move forward in time
- img_last[cam_id_left] = img_left.clone();
- img_last[cam_id_right] = img_right.clone();
+ img_last[cam_id_left] = img_left;
+ img_last[cam_id_right] = img_right;
+ img_mask_last[cam_id_left] = mask_left;
+ img_mask_last[cam_id_right] = mask_right;
pts_last[cam_id_left] = good_left;
pts_last[cam_id_right] = good_right;
ids_last[cam_id_left] = good_ids_left;
@@ -276,38 +321,58 @@ void TrackDescriptor::feed_stereo(double timestamp, cv::Mat &img_leftin, cv::Mat
// printf("[TIME-DESC]: %.4f seconds for total\n",(rT5-rT1).total_microseconds() * 1e-6);
}
-void TrackDescriptor::perform_detection_monocular(const cv::Mat &img0, std::vector &pts0, cv::Mat &desc0,
- std::vector &ids0) {
+void TrackDescriptor::perform_detection_monocular(const cv::Mat &img0, const cv::Mat &mask0, std::vector &pts0,
+ cv::Mat &desc0, std::vector &ids0) {
// Assert that we need features
assert(pts0.empty());
// Extract our features (use FAST with griding)
std::vector pts0_ext;
- Grider_FAST::perform_griding(img0, pts0_ext, num_features, grid_x, grid_y, threshold, true);
+ Grider_FAST::perform_griding(img0, mask0, pts0_ext, num_features, grid_x, grid_y, threshold, true);
// For all new points, extract their descriptors
cv::Mat desc0_ext;
this->orb0->compute(img0, pts0_ext, desc0_ext);
+ // Create a 2D occupancy grid for this current image
+ // Note that we scale this down, so that each grid point is equal to a set of pixels
+ // This means that we will reject points that less then grid_px_size points away then existing features
+ cv::Size size((int)((float)img0.cols / (float)min_px_dist), (int)((float)img0.rows / (float)min_px_dist));
+ cv::Mat grid_2d = cv::Mat::zeros(size, CV_8UC1);
+
// For all good matches, lets append to our returned vectors
// NOTE: if we multi-thread this atomic can cause some randomness due to multiple thread detecting features
// NOTE: this is due to the fact that we select update features based on feat id
// NOTE: thus the order will matter since we try to select oldest (smallest id) to update with
// NOTE: not sure how to remove... maybe a better way?
for (size_t i = 0; i < pts0_ext.size(); i++) {
- // Append our keypoints and descriptors
+ // Get current left keypoint, check that it is in bounds
+ cv::KeyPoint kpt = pts0_ext.at(i);
+ int x = (int)kpt.pt.x;
+ int y = (int)kpt.pt.y;
+ int x_grid = (int)(kpt.pt.x / (float)min_px_dist);
+ int y_grid = (int)(kpt.pt.y / (float)min_px_dist);
+ if (x_grid < 0 || x_grid >= size.width || y_grid < 0 || y_grid >= size.height || x < 0 || x >= img0.cols || y < 0 || y >= img0.rows) {
+ continue;
+ }
+ // Check if this keypoint is near another point
+ if (grid_2d.at(y_grid, x_grid) > 127)
+ continue;
+ // Else we are good, append our keypoints and descriptors
pts0.push_back(pts0_ext.at(i));
desc0.push_back(desc0_ext.row((int)i));
// Set our IDs to be unique IDs here, will later replace with corrected ones, after temporal matching
size_t temp = ++currid;
ids0.push_back(temp);
+ grid_2d.at(y_grid, x_grid) = 255;
}
}
-void TrackDescriptor::perform_detection_stereo(const cv::Mat &img0, const cv::Mat &img1, std::vector &pts0,
- std::vector &pts1, cv::Mat &desc0, cv::Mat &desc1, size_t cam_id0,
- size_t cam_id1, std::vector &ids0, std::vector &ids1) {
+void TrackDescriptor::perform_detection_stereo(const cv::Mat &img0, const cv::Mat &img1, const cv::Mat &mask0, const cv::Mat &mask1,
+ std::vector &pts0, std::vector &pts1, cv::Mat &desc0,
+ cv::Mat &desc1, size_t cam_id0, size_t cam_id1, std::vector &ids0,
+ std::vector &ids1) {
// Assert that we need features
assert(pts0.empty());
@@ -319,8 +384,8 @@ void TrackDescriptor::perform_detection_stereo(const cv::Mat &img0, const cv::Ma
parallel_for_(cv::Range(0, 2), LambdaBody([&](const cv::Range &range) {
for (int i = range.start; i < range.end; i++) {
bool is_left = (i == 0);
- Grider_FAST::perform_griding(is_left ? img0 : img1, is_left ? pts0_ext : pts1_ext, num_features, grid_x, grid_y,
- threshold, true);
+ Grider_FAST::perform_griding(is_left ? img0 : img1, is_left ? mask0 : mask1, is_left ? pts0_ext : pts1_ext,
+ num_features, grid_x, grid_y, threshold, true);
(is_left ? orb0 : orb1)->compute(is_left ? img0 : img1, is_left ? pts0_ext : pts1_ext, is_left ? desc0_ext : desc1_ext);
}
}));
@@ -329,16 +394,51 @@ void TrackDescriptor::perform_detection_stereo(const cv::Mat &img0, const cv::Ma
std::vector matches;
robust_match(pts0_ext, pts1_ext, desc0_ext, desc1_ext, cam_id0, cam_id1, matches);
+ // Create a 2D occupancy grid for this current image
+ // Note that we scale this down, so that each grid point is equal to a set of pixels
+ // This means that we will reject points that less then grid_px_size points away then existing features
+ cv::Size size0((int)((float)img0.cols / (float)min_px_dist), (int)((float)img0.rows / (float)min_px_dist));
+ cv::Mat grid_2d_0 = cv::Mat::zeros(size0, CV_8UC1);
+ cv::Size size1((int)((float)img1.cols / (float)min_px_dist), (int)((float)img1.rows / (float)min_px_dist));
+ cv::Mat grid_2d_1 = cv::Mat::zeros(size1, CV_8UC1);
+
// For all good matches, lets append to our returned vectors
for (size_t i = 0; i < matches.size(); i++) {
+
// Get our ids
int index_pt0 = matches.at(i).queryIdx;
int index_pt1 = matches.at(i).trainIdx;
+
+ // Get current left/right keypoint, check that it is in bounds
+ cv::KeyPoint kpt0 = pts0_ext.at(index_pt0);
+ cv::KeyPoint kpt1 = pts1_ext.at(index_pt1);
+ int x0 = (int)kpt0.pt.x;
+ int y0 = (int)kpt0.pt.y;
+ int x0_grid = (int)(kpt0.pt.x / (float)min_px_dist);
+ int y0_grid = (int)(kpt0.pt.y / (float)min_px_dist);
+ if (x0_grid < 0 || x0_grid >= size0.width || y0_grid < 0 || y0_grid >= size0.height || x0 < 0 || x0 >= img0.cols || y0 < 0 ||
+ y0 >= img0.rows) {
+ continue;
+ }
+ int x1 = (int)kpt1.pt.x;
+ int y1 = (int)kpt1.pt.y;
+ int x1_grid = (int)(kpt1.pt.x / (float)min_px_dist);
+ int y1_grid = (int)(kpt1.pt.y / (float)min_px_dist);
+ if (x1_grid < 0 || x1_grid >= size1.width || y1_grid < 0 || y1_grid >= size1.height || x1 < 0 || x1 >= img0.cols || y1 < 0 ||
+ y1 >= img0.rows) {
+ continue;
+ }
+
+ // Check if this keypoint is near another point
+ if (grid_2d_0.at(y0_grid, x0_grid) > 127 || grid_2d_1.at(y1_grid, x1_grid) > 127)
+ continue;
+
// Append our keypoints and descriptors
pts0.push_back(pts0_ext[index_pt0]);
pts1.push_back(pts1_ext[index_pt1]);
desc0.push_back(desc0_ext.row(index_pt0));
desc1.push_back(desc1_ext.row(index_pt1));
+
// Set our IDs to be unique IDs here, will later replace with corrected ones, after temporal matching
size_t temp = ++currid;
ids0.push_back(temp);
@@ -383,14 +483,14 @@ void TrackDescriptor::robust_match(std::vector &pts0, std::vector<
// We don't want to do ransac on distorted image uvs since the mapping is nonlinear
std::vector pts0_n, pts1_n;
for (size_t i = 0; i < pts0_rsc.size(); i++) {
- pts0_n.push_back(undistort_point(pts0_rsc.at(i), id0));
- pts1_n.push_back(undistort_point(pts1_rsc.at(i), id1));
+ pts0_n.push_back(camera_calib.at(id0)->undistort_cv(pts0_rsc.at(i)));
+ pts1_n.push_back(camera_calib.at(id1)->undistort_cv(pts1_rsc.at(i)));
}
// Do RANSAC outlier rejection (note since we normalized the max pixel error is now in the normalized cords)
std::vector mask_rsc;
- double max_focallength_img0 = std::max(camera_k_OPENCV.at(id0)(0, 0), camera_k_OPENCV.at(id0)(1, 1));
- double max_focallength_img1 = std::max(camera_k_OPENCV.at(id1)(0, 0), camera_k_OPENCV.at(id1)(1, 1));
+ double max_focallength_img0 = std::max(camera_calib.at(id0)->get_K()(0, 0), camera_calib.at(id0)->get_K()(1, 1));
+ double max_focallength_img1 = std::max(camera_calib.at(id1)->get_K()(0, 0), camera_calib.at(id1)->get_K()(1, 1));
double max_focallength = std::max(max_focallength_img0, max_focallength_img1);
cv::findFundamentalMat(pts0_n, pts1_n, cv::FM_RANSAC, 1 / max_focallength, 0.999, mask_rsc);
@@ -405,43 +505,37 @@ void TrackDescriptor::robust_match(std::vector &pts0, std::vector<
}
void TrackDescriptor::robust_ratio_test(std::vector> &matches) {
-
// Loop through all matches
- for (auto matchIterator = matches.begin(); matchIterator != matches.end(); ++matchIterator) {
+ for (auto &match : matches) {
// If 2 NN has been identified, else remove this feature
- if (matchIterator->size() > 1) {
+ if (match.size() > 1) {
// check distance ratio, remove it if the ratio is larger
- if ((*matchIterator)[0].distance / (*matchIterator)[1].distance > knn_ratio) {
- matchIterator->clear();
+ if (match[0].distance / match[1].distance > knn_ratio) {
+ match.clear();
}
} else {
// does not have 2 neighbours, so remove it
- matchIterator->clear();
+ match.clear();
}
}
}
void TrackDescriptor::robust_symmetry_test(std::vector> &matches1, std::vector> &matches2,
std::vector &good_matches) {
-
// for all matches image 1 -> image 2
- for (auto matchIterator1 = matches1.begin(); matchIterator1 != matches1.end(); ++matchIterator1) {
-
+ for (auto &match1 : matches1) {
// ignore deleted matches
- if (matchIterator1->empty() || matchIterator1->size() < 2)
+ if (match1.empty() || match1.size() < 2)
continue;
-
// for all matches image 2 -> image 1
- for (auto matchIterator2 = matches2.begin(); matchIterator2 != matches2.end(); ++matchIterator2) {
+ for (auto &match2 : matches2) {
// ignore deleted matches
- if (matchIterator2->empty() || matchIterator2->size() < 2)
+ if (match2.empty() || match2.size() < 2)
continue;
-
// Match symmetry test
- if ((*matchIterator1)[0].queryIdx == (*matchIterator2)[0].trainIdx &&
- (*matchIterator2)[0].queryIdx == (*matchIterator1)[0].trainIdx) {
+ if (match1[0].queryIdx == match2[0].trainIdx && match2[0].queryIdx == match1[0].trainIdx) {
// add symmetrical match
- good_matches.emplace_back(cv::DMatch((*matchIterator1)[0].queryIdx, (*matchIterator1)[0].trainIdx, (*matchIterator1)[0].distance));
+ good_matches.emplace_back(cv::DMatch(match1[0].queryIdx, match1[0].trainIdx, match1[0].distance));
// next match in image 1 -> image 2
break;
}
diff --git a/ov_core/src/track/TrackDescriptor.h b/ov_core/src/track/TrackDescriptor.h
index e16304cc0..8b40bb541 100644
--- a/ov_core/src/track/TrackDescriptor.h
+++ b/ov_core/src/track/TrackDescriptor.h
@@ -19,7 +19,6 @@
* along with this program. If not, see .
*/
-
#ifndef OV_CORE_TRACK_DESC_H
#define OV_CORE_TRACK_DESC_H
@@ -40,45 +39,50 @@ namespace ov_core {
class TrackDescriptor : public TrackBase {
public:
- /**
- * @brief Public default constructor
- */
- TrackDescriptor() : TrackBase(), threshold(10), grid_x(8), grid_y(5), knn_ratio(0.75) {}
-
/**
* @brief Public constructor with configuration variables
+ * @param cameras camera calibration object which has all camera intrinsics in it
* @param numfeats number of features we want want to track (i.e. track 200 points from frame to frame)
* @param numaruco the max id of the arucotags, so we ensure that we start our non-auroc features above this value
+ * @param binocular if we should do binocular feature tracking or stereo if there are multiple cameras
+ * @param histmethod what type of histogram pre-processing should be done (histogram eq?)
* @param fast_threshold FAST detection threshold
* @param gridx size of grid in the x-direction / u-direction
* @param gridy size of grid in the y-direction / v-direction
+ * @param minpxdist features need to be at least this number pixels away from each other
* @param knnratio matching ratio needed (smaller value forces top two descriptors during match to be more different)
*/
- explicit TrackDescriptor(int numfeats, int numaruco, int fast_threshold, int gridx, int gridy, double knnratio)
- : TrackBase(numfeats, numaruco), threshold(fast_threshold), grid_x(gridx), grid_y(gridy), knn_ratio(knnratio) {}
+ explicit TrackDescriptor(std::unordered_map> cameras, int numfeats, int numaruco, bool binocular,
+ HistogramMethod histmethod, int fast_threshold, int gridx, int gridy, int minpxdist, double knnratio)
+ : TrackBase(cameras, numfeats, numaruco, binocular, histmethod), threshold(fast_threshold), grid_x(gridx), grid_y(gridy),
+ min_px_dist(minpxdist), knn_ratio(knnratio) {}
+
+ /**
+ * @brief Process a new image
+ * @param message Contains our timestamp, images, and camera ids
+ */
+ void feed_new_camera(const CameraData &message);
+protected:
/**
* @brief Process a new monocular image
- * @param timestamp timestamp the new image occurred at
- * @param img new cv:Mat grayscale image
- * @param cam_id the camera id that this new image corresponds too
+ * @param message Contains our timestamp, images, and camera ids
+ * @param msg_id the camera index in message data vector
*/
- void feed_monocular(double timestamp, cv::Mat &img, size_t cam_id) override;
+ void feed_monocular(const CameraData &message, size_t msg_id);
/**
* @brief Process new stereo pair of images
- * @param timestamp timestamp this pair occured at (stereo is synchronised)
- * @param img_left first grayscaled image
- * @param img_right second grayscaled image
- * @param cam_id_left first image camera id
- * @param cam_id_right second image camera id
+ * @param message Contains our timestamp, images, and camera ids
+ * @param msg_id_left first image index in message data vector
+ * @param msg_id_right second image index in message data vector
*/
- void feed_stereo(double timestamp, cv::Mat &img_left, cv::Mat &img_right, size_t cam_id_left, size_t cam_id_right) override;
+ void feed_stereo(const CameraData &message, size_t msg_id_left, size_t msg_id_right);
-protected:
/**
* @brief Detects new features in the current image
* @param img0 image we will detect features on
+ * @param mask0 mask which has what ROI we do not want features in
* @param pts0 vector of extracted keypoints
* @param desc0 vector of the extracted descriptors
* @param ids0 vector of all new IDs
@@ -88,12 +92,15 @@ class TrackDescriptor : public TrackBase {
* Our vector of IDs will be later overwritten when we match features temporally to the previous frame's features.
* See robust_match() for the matching.
*/
- void perform_detection_monocular(const cv::Mat &img0, std::vector &pts0, cv::Mat &desc0, std::vector &ids0);
+ void perform_detection_monocular(const cv::Mat &img0, const cv::Mat &mask0, std::vector &pts0, cv::Mat &desc0,
+ std::vector &ids0);
/**
* @brief Detects new features in the current stereo pair
* @param img0 left image we will detect features on
* @param img1 right image we will detect features on
+ * @param mask0 mask which has what ROI we do not want features in
+ * @param mask1 mask which has what ROI we do not want features in
* @param pts0 left vector of new keypoints
* @param pts1 right vector of new keypoints
* @param desc0 left vector of extracted descriptors
@@ -108,9 +115,9 @@ class TrackDescriptor : public TrackBase {
* Our vector of IDs will be later overwritten when we match features temporally to the previous frame's features.
* See robust_match() for the matching.
*/
- void perform_detection_stereo(const cv::Mat &img0, const cv::Mat &img1, std::vector &pts0, std::vector &pts1,
- cv::Mat &desc0, cv::Mat &desc1, size_t cam_id0, size_t cam_id1, std::vector &ids0,
- std::vector &ids1);
+ void perform_detection_stereo(const cv::Mat &img0, const cv::Mat &img1, const cv::Mat &mask0, const cv::Mat &mask1,
+ std::vector &pts0, std::vector &pts1, cv::Mat &desc0, cv::Mat &desc1,
+ size_t cam_id0, size_t cam_id1, std::vector &ids0, std::vector &ids1);
/**
* @brief Find matches between two keypoint+descriptor sets.
@@ -152,6 +159,9 @@ class TrackDescriptor : public TrackBase {
int grid_x;
int grid_y;
+ // Minimum pixel distance to be "far away enough" to be a different extracted feature
+ int min_px_dist;
+
// The ratio between two kNN matches, if that ratio is larger then this threshold
// then the two features are too close, so should be considered ambiguous/bad match
double knn_ratio;
diff --git a/ov_core/src/track/TrackKLT.cpp b/ov_core/src/track/TrackKLT.cpp
index 2a61b872c..a262015c9 100644
--- a/ov_core/src/track/TrackKLT.cpp
+++ b/ov_core/src/track/TrackKLT.cpp
@@ -19,23 +19,62 @@
* along with this program. If not, see .
*/
-
#include "TrackKLT.h"
using namespace ov_core;
-void TrackKLT::feed_monocular(double timestamp, cv::Mat &img, size_t cam_id) {
+void TrackKLT::feed_new_camera(const CameraData &message) {
+
+ // Error check that we have all the data
+ if (message.sensor_ids.empty() || message.sensor_ids.size() != message.images.size() || message.images.size() != message.masks.size()) {
+ printf(RED "[ERROR]: MESSAGE DATA SIZES DO NOT MATCH OR EMPTY!!!\n" RESET);
+ printf(RED "[ERROR]: - message.sensor_ids.size() = %zu\n" RESET, message.sensor_ids.size());
+ printf(RED "[ERROR]: - message.images.size() = %zu\n" RESET, message.images.size());
+ printf(RED "[ERROR]: - message.masks.size() = %zu\n" RESET, message.masks.size());
+ std::exit(EXIT_FAILURE);
+ }
+
+ // Either call our stereo or monocular version
+ // If we are doing binocular tracking, then we should parallize our tracking
+ size_t num_images = message.images.size();
+ if (num_images == 1) {
+ feed_monocular(message, 0);
+ } else if (num_images == 2 && use_stereo) {
+ feed_stereo(message, 0, 1);
+ } else if (!use_stereo) {
+ parallel_for_(cv::Range(0, (int)num_images), LambdaBody([&](const cv::Range &range) {
+ for (int i = range.start; i < range.end; i++) {
+ feed_monocular(message, i);
+ }
+ }));
+ } else {
+ printf(RED "[ERROR]: invalid number of images passed %zu, we only support mono or stereo tracking", num_images);
+ std::exit(EXIT_FAILURE);
+ }
+}
+
+void TrackKLT::feed_monocular(const CameraData &message, size_t msg_id) {
// Start timing
rT1 = boost::posix_time::microsec_clock::local_time();
// Lock this data feed for this camera
+ size_t cam_id = message.sensor_ids.at(msg_id);
std::unique_lock lck(mtx_feeds.at(cam_id));
// Histogram equalize
- cv::equalizeHist(img, img);
- // cv::Ptr clahe = cv::createCLAHE(eq_clip_limit, eq_win_size);
- // clahe->apply(img, img);
+ cv::Mat img, mask;
+ if (histogram_method == HistogramMethod::HISTOGRAM) {
+ cv::equalizeHist(message.images.at(msg_id), img);
+ } else if (histogram_method == HistogramMethod::CLAHE) {
+ double eq_clip_limit = 10.0;
+ cv::Size eq_win_size = cv::Size(8, 8);
+ cv::Ptr clahe = cv::createCLAHE(eq_clip_limit, eq_win_size);
+ clahe->apply(message.images.at(msg_id), img);
+ } else {
+ img = message.images.at(msg_id);
+ }
+ mask = message.masks.at(msg_id);
// Extract the new image pyramid
std::vector imgpyr;
@@ -46,40 +85,33 @@ void TrackKLT::feed_monocular(double timestamp, cv::Mat &img, size_t cam_id) {
// This also handles, the tracking initalization on the first call to this extractor
if (pts_last[cam_id].empty()) {
// Detect new features
- perform_detection_monocular(imgpyr, pts_last[cam_id], ids_last[cam_id]);
+ perform_detection_monocular(imgpyr, mask, pts_last[cam_id], ids_last[cam_id]);
// Save the current image and pyramid
- img_last[cam_id] = img.clone();
+ img_last[cam_id] = img;
img_pyramid_last[cam_id] = imgpyr;
+ img_mask_last[cam_id] = mask;
return;
}
// First we should make that the last images have enough features so we can do KLT
// This will "top-off" our number of tracks so always have a constant number
- perform_detection_monocular(img_pyramid_last[cam_id], pts_last[cam_id], ids_last[cam_id]);
+ perform_detection_monocular(img_pyramid_last[cam_id], img_mask_last[cam_id], pts_last[cam_id], ids_last[cam_id]);
rT3 = boost::posix_time::microsec_clock::local_time();
- //===================================================================================
- //===================================================================================
-
- // Debug
- // printf("current points = %d,%d\n",(int)pts_left_last.size(),(int)pts_right_last.size());
-
// Our return success masks, and predicted new features
std::vector mask_ll;
std::vector pts_left_new = pts_last[cam_id];
// Lets track temporally
perform_matching(img_pyramid_last[cam_id], imgpyr, pts_last[cam_id], pts_left_new, cam_id, cam_id, mask_ll);
- assert(pts_left_new.size()==ids_last[cam_id].size());
+ assert(pts_left_new.size() == ids_last[cam_id].size());
rT4 = boost::posix_time::microsec_clock::local_time();
- //===================================================================================
- //===================================================================================
-
// If any of our mask is empty, that means we didn't have enough to do ransac, so just return
if (mask_ll.empty()) {
- img_last[cam_id] = img.clone();
+ img_last[cam_id] = img;
img_pyramid_last[cam_id] = imgpyr;
+ img_mask_last[cam_id] = mask;
pts_last[cam_id].clear();
ids_last[cam_id].clear();
printf(RED "[KLT-EXTRACTOR]: Failed to get enough points to do RANSAC, resetting.....\n" RESET);
@@ -93,8 +125,12 @@ void TrackKLT::feed_monocular(double timestamp, cv::Mat &img, size_t cam_id) {
// Loop through all left points
for (size_t i = 0; i < pts_left_new.size(); i++) {
// Ensure we do not have any bad KLT tracks (i.e., points are negative)
- if (pts_left_new.at(i).pt.x < 0 || pts_left_new.at(i).pt.y < 0 || (int)pts_left_new.at(i).pt.x > img.cols ||
- (int)pts_left_new.at(i).pt.y > img.rows)
+ if (pts_left_new.at(i).pt.x < 0 || pts_left_new.at(i).pt.y < 0 || (int)pts_left_new.at(i).pt.x >= img.cols ||
+ (int)pts_left_new.at(i).pt.y >= img.rows)
+ continue;
+ // Check if it is in the mask
+ // NOTE: mask has max value of 255 (white) if it should be
+ if ((int)message.masks.at(msg_id).at((int)pts_left_new.at(i).pt.y, (int)pts_left_new.at(i).pt.x) > 127)
continue;
// If it is a good track, and also tracked from left to right
if (mask_ll[i]) {
@@ -103,18 +139,16 @@ void TrackKLT::feed_monocular(double timestamp, cv::Mat &img, size_t cam_id) {
}
}
- //===================================================================================
- //===================================================================================
-
// Update our feature database, with theses new observations
for (size_t i = 0; i < good_left.size(); i++) {
- cv::Point2f npt_l = undistort_point(good_left.at(i).pt, cam_id);
- database->update_feature(good_ids_left.at(i), timestamp, cam_id, good_left.at(i).pt.x, good_left.at(i).pt.y, npt_l.x, npt_l.y);
+ cv::Point2f npt_l = camera_calib.at(cam_id)->undistort_cv(good_left.at(i).pt);
+ database->update_feature(good_ids_left.at(i), message.timestamp, cam_id, good_left.at(i).pt.x, good_left.at(i).pt.y, npt_l.x, npt_l.y);
}
// Move forward in time
- img_last[cam_id] = img.clone();
+ img_last[cam_id] = img;
img_pyramid_last[cam_id] = imgpyr;
+ img_mask_last[cam_id] = mask;
pts_last[cam_id] = good_left;
ids_last[cam_id] = good_ids_left;
rT5 = boost::posix_time::microsec_clock::local_time();
@@ -127,27 +161,40 @@ void TrackKLT::feed_monocular(double timestamp, cv::Mat &img, size_t cam_id) {
// printf("[TIME-KLT]: %.4f seconds for total\n",(rT5-rT1).total_microseconds() * 1e-6);
}
-void TrackKLT::feed_stereo(double timestamp, cv::Mat &img_leftin, cv::Mat &img_rightin, size_t cam_id_left, size_t cam_id_right) {
+void TrackKLT::feed_stereo(const CameraData &message, size_t msg_id_left, size_t msg_id_right) {
// Start timing
rT1 = boost::posix_time::microsec_clock::local_time();
// Lock this data feed for this camera
+ size_t cam_id_left = message.sensor_ids.at(msg_id_left);
+ size_t cam_id_right = message.sensor_ids.at(msg_id_right);
std::unique_lock lck1(mtx_feeds.at(cam_id_left));
std::unique_lock lck2(mtx_feeds.at(cam_id_right));
- // Histogram equalize and then
- // Extract image pyramids (boost seems to require us to put all the arguments even if there are defaults....)
- cv::Mat img_left, img_right;
+ // Histogram equalize images
+ cv::Mat img_left, img_right, mask_left, mask_right;
+ if (histogram_method == HistogramMethod::HISTOGRAM) {
+ cv::equalizeHist(message.images.at(msg_id_left), img_left);
+ cv::equalizeHist(message.images.at(msg_id_right), img_right);
+ } else if (histogram_method == HistogramMethod::CLAHE) {
+ double eq_clip_limit = 10.0;
+ cv::Size eq_win_size = cv::Size(8, 8);
+ cv::Ptr clahe = cv::createCLAHE(eq_clip_limit, eq_win_size);
+ clahe->apply(message.images.at(msg_id_left), img_left);
+ clahe->apply(message.images.at(msg_id_right), img_right);
+ } else {
+ img_left = message.images.at(msg_id_left);
+ img_right = message.images.at(msg_id_right);
+ }
+ mask_left = message.masks.at(msg_id_left);
+ mask_right = message.masks.at(msg_id_right);
+
+ // Extract image pyramids
std::vector imgpyr_left, imgpyr_right;
- // cv::Ptr clahe = cv::createCLAHE(eq_clip_limit, eq_win_size);
parallel_for_(cv::Range(0, 2), LambdaBody([&](const cv::Range &range) {
for (int i = range.start; i < range.end; i++) {
bool is_left = (i == 0);
- // Histogram equalize
- cv::equalizeHist(is_left ? img_leftin : img_rightin, is_left ? img_left : img_right);
- // clahe->apply((is_left ? img_leftin : img_rightin), (is_left ? img_left : img_right));
- // Extract image pyramids (boost seems to require us to put all the arguments even if there are defaults....)
cv::buildOpticalFlowPyramid(is_left ? img_left : img_right, is_left ? imgpyr_left : imgpyr_right, win_size, pyr_levels);
}
}));
@@ -155,27 +202,27 @@ void TrackKLT::feed_stereo(double timestamp, cv::Mat &img_leftin, cv::Mat &img_r
// If we didn't have any successful tracks last time, just extract this time
// This also handles, the tracking initalization on the first call to this extractor
- if (pts_last[cam_id_left].empty() || pts_last[cam_id_right].empty()) {
+ if (pts_last[cam_id_left].empty() && pts_last[cam_id_right].empty()) {
// Track into the new image
- perform_detection_stereo(imgpyr_left, imgpyr_right, pts_last[cam_id_left], pts_last[cam_id_right], ids_last[cam_id_left],
- ids_last[cam_id_right]);
+ perform_detection_stereo(imgpyr_left, imgpyr_right, mask_left, mask_right, cam_id_left, cam_id_right, pts_last[cam_id_left],
+ pts_last[cam_id_right], ids_last[cam_id_left], ids_last[cam_id_right]);
// Save the current image and pyramid
img_last[cam_id_left] = img_left;
img_last[cam_id_right] = img_right;
img_pyramid_last[cam_id_left] = imgpyr_left;
img_pyramid_last[cam_id_right] = imgpyr_right;
+ img_mask_last[cam_id_left] = mask_left;
+ img_mask_last[cam_id_right] = mask_right;
return;
}
// First we should make that the last images have enough features so we can do KLT
// This will "top-off" our number of tracks so always have a constant number
- perform_detection_stereo(img_pyramid_last[cam_id_left], img_pyramid_last[cam_id_right], pts_last[cam_id_left], pts_last[cam_id_right],
+ perform_detection_stereo(img_pyramid_last[cam_id_left], img_pyramid_last[cam_id_right], img_mask_last[cam_id_left],
+ img_mask_last[cam_id_right], cam_id_left, cam_id_right, pts_last[cam_id_left], pts_last[cam_id_right],
ids_last[cam_id_left], ids_last[cam_id_right]);
rT3 = boost::posix_time::microsec_clock::local_time();
- //===================================================================================
- //===================================================================================
-
// Our return success masks, and predicted new features
std::vector mask_ll, mask_rr;
std::vector pts_left_new = pts_last[cam_id_left];
@@ -207,16 +254,18 @@ void TrackKLT::feed_stereo(double timestamp, cv::Mat &img_leftin, cv::Mat &img_r
//===================================================================================
// If any of our masks are empty, that means we didn't have enough to do ransac, so just return
- if (mask_ll.empty() || mask_rr.empty()) {
- img_last[cam_id_left] = std::move(img_left);
- img_last[cam_id_right] = std::move(img_right);
- img_pyramid_last[cam_id_left] = std::move(imgpyr_left);
- img_pyramid_last[cam_id_right] = std::move(imgpyr_right);
+ if (mask_ll.empty() && mask_rr.empty()) {
+ img_last[cam_id_left] = img_left;
+ img_last[cam_id_right] = img_right;
+ img_pyramid_last[cam_id_left] = imgpyr_left;
+ img_pyramid_last[cam_id_right] = imgpyr_right;
+ img_mask_last[cam_id_left] = mask_left;
+ img_mask_last[cam_id_right] = mask_right;
pts_last[cam_id_left].clear();
pts_last[cam_id_right].clear();
ids_last[cam_id_left].clear();
ids_last[cam_id_right].clear();
- printf(RED "[KLT-EXTRACTOR]: Failed to get enough points to do RANSAC, resetting....." RESET);
+ printf(RED "[KLT-EXTRACTOR]: Failed to get enough points to do RANSAC, resetting.....\n" RESET);
return;
}
@@ -245,7 +294,7 @@ void TrackKLT::feed_stereo(double timestamp, cv::Mat &img_leftin, cv::Mat &img_r
if (mask_ll[i] && found_right && mask_rr[index_right]) {
// Ensure we do not have any bad KLT tracks (i.e., points are negative)
if (pts_right_new.at(index_right).pt.x < 0 || pts_right_new.at(index_right).pt.y < 0 ||
- (int)pts_right_new.at(index_right).pt.x > img_right.cols || (int)pts_right_new.at(index_right).pt.y > img_right.rows)
+ (int)pts_right_new.at(index_right).pt.x >= img_right.cols || (int)pts_right_new.at(index_right).pt.y >= img_right.rows)
continue;
good_left.push_back(pts_left_new.at(i));
good_right.push_back(pts_right_new.at(index_right));
@@ -262,8 +311,8 @@ void TrackKLT::feed_stereo(double timestamp, cv::Mat &img_leftin, cv::Mat &img_r
// Loop through all right points
for (size_t i = 0; i < pts_right_new.size(); i++) {
// Ensure we do not have any bad KLT tracks (i.e., points are negative)
- if (pts_right_new.at(i).pt.x < 0 || pts_right_new.at(i).pt.y < 0 || (int)pts_right_new.at(i).pt.x > img_right.cols ||
- (int)pts_right_new.at(i).pt.y > img_right.rows)
+ if (pts_right_new.at(i).pt.x < 0 || pts_right_new.at(i).pt.y < 0 || (int)pts_right_new.at(i).pt.x >= img_right.cols ||
+ (int)pts_right_new.at(i).pt.y >= img_right.rows)
continue;
// See if we have the same feature in the right
bool added_already = (std::find(good_ids_right.begin(), good_ids_right.end(), ids_last[cam_id_right].at(i)) != good_ids_right.end());
@@ -275,28 +324,29 @@ void TrackKLT::feed_stereo(double timestamp, cv::Mat &img_leftin, cv::Mat &img_r
}
}
- //===================================================================================
- //===================================================================================
-
// Update our feature database, with theses new observations
for (size_t i = 0; i < good_left.size(); i++) {
- cv::Point2f npt_l = undistort_point(good_left.at(i).pt, cam_id_left);
- database->update_feature(good_ids_left.at(i), timestamp, cam_id_left, good_left.at(i).pt.x, good_left.at(i).pt.y, npt_l.x, npt_l.y);
+ cv::Point2f npt_l = camera_calib.at(cam_id_left)->undistort_cv(good_left.at(i).pt);
+ database->update_feature(good_ids_left.at(i), message.timestamp, cam_id_left, good_left.at(i).pt.x, good_left.at(i).pt.y, npt_l.x,
+ npt_l.y);
}
for (size_t i = 0; i < good_right.size(); i++) {
- cv::Point2f npt_r = undistort_point(good_right.at(i).pt, cam_id_right);
- database->update_feature(good_ids_right.at(i), timestamp, cam_id_right, good_right.at(i).pt.x, good_right.at(i).pt.y, npt_r.x, npt_r.y);
+ cv::Point2f npt_r = camera_calib.at(cam_id_right)->undistort_cv(good_right.at(i).pt);
+ database->update_feature(good_ids_right.at(i), message.timestamp, cam_id_right, good_right.at(i).pt.x, good_right.at(i).pt.y, npt_r.x,
+ npt_r.y);
}
// Move forward in time
- img_last[cam_id_left] = std::move(img_left);
- img_last[cam_id_right] = std::move(img_right);
- img_pyramid_last[cam_id_left] = std::move(imgpyr_left);
- img_pyramid_last[cam_id_right] = std::move(imgpyr_right);
- pts_last[cam_id_left] = std::move(good_left);
- pts_last[cam_id_right] = std::move(good_right);
- ids_last[cam_id_left] = std::move(good_ids_left);
- ids_last[cam_id_right] = std::move(good_ids_right);
+ img_last[cam_id_left] = img_left;
+ img_last[cam_id_right] = img_right;
+ img_pyramid_last[cam_id_left] = imgpyr_left;
+ img_pyramid_last[cam_id_right] = imgpyr_right;
+ img_mask_last[cam_id_left] = mask_left;
+ img_mask_last[cam_id_right] = mask_right;
+ pts_last[cam_id_left] = good_left;
+ pts_last[cam_id_right] = good_right;
+ ids_last[cam_id_left] = good_ids_left;
+ ids_last[cam_id_right] = good_ids_right;
rT6 = boost::posix_time::microsec_clock::local_time();
// Timing information
@@ -308,60 +358,75 @@ void TrackKLT::feed_stereo(double timestamp, cv::Mat &img_leftin, cv::Mat &img_r
// printf("[TIME-KLT]: %.4f seconds for total\n",(rT6-rT1).total_microseconds() * 1e-6);
}
-void TrackKLT::perform_detection_monocular(const std::vector &img0pyr, std::vector &pts0,
+void TrackKLT::perform_detection_monocular(const std::vector &img0pyr, const cv::Mat &mask0, std::vector &pts0,
std::vector &ids0) {
- // First compute how many more features we need to extract from this image
- int num_featsneeded = num_features - (int)pts0.size();
-
- // If we don't need any features, just return
- if (num_featsneeded < 0.2 * num_features)
- return;
-
// Create a 2D occupancy grid for this current image
// Note that we scale this down, so that each grid point is equal to a set of pixels
// This means that we will reject points that less then grid_px_size points away then existing features
- // TODO: figure out why I need to add the windowsize of the klt to handle features that are outside the image bound
- // TODO: I assume this is because klt of features at the corners is not really well defined, thus if it doesn't get a match it will be
- // out-of-bounds
- Eigen::MatrixXi grid_2d =
- Eigen::MatrixXi::Zero((int)(img0pyr.at(0).rows / min_px_dist) + 15, (int)(img0pyr.at(0).cols / min_px_dist) + 15);
+ cv::Size size((int)((float)img0pyr.at(0).cols / (float)min_px_dist), (int)((float)img0pyr.at(0).rows / (float)min_px_dist));
+ cv::Mat grid_2d = cv::Mat::zeros(size, CV_8UC1);
auto it0 = pts0.begin();
- auto it2 = ids0.begin();
+ auto it1 = ids0.begin();
while (it0 != pts0.end()) {
- // Get current left keypoint
+ // Get current left keypoint, check that it is in bounds
cv::KeyPoint kpt = *it0;
+ int x = (int)kpt.pt.x;
+ int y = (int)kpt.pt.y;
+ int x_grid = (int)(kpt.pt.x / (float)min_px_dist);
+ int y_grid = (int)(kpt.pt.y / (float)min_px_dist);
+ if (x_grid < 0 || x_grid >= size.width || y_grid < 0 || y_grid >= size.height || x < 0 || x >= img0pyr.at(0).cols || y < 0 ||
+ y >= img0pyr.at(0).rows) {
+ it0 = pts0.erase(it0);
+ it1 = ids0.erase(it1);
+ continue;
+ }
// Check if this keypoint is near another point
- if (grid_2d((int)(kpt.pt.y / min_px_dist), (int)(kpt.pt.x / min_px_dist)) == 1) {
+ if (grid_2d.at(y_grid, x_grid) > 127) {
it0 = pts0.erase(it0);
- it2 = ids0.erase(it2);
+ it1 = ids0.erase(it1);
+ continue;
+ }
+ // Now check if it is in a mask area or not
+ // NOTE: mask has max value of 255 (white) if it should be
+ if (mask0.at(y, x) > 127) {
+ it0 = pts0.erase(it0);
+ it1 = ids0.erase(it1);
continue;
}
// Else we are good, move forward to the next point
- grid_2d((int)(kpt.pt.y / min_px_dist), (int)(kpt.pt.x / min_px_dist)) = 1;
+ grid_2d.at(y_grid, x_grid) = 255;
it0++;
- it2++;
+ it1++;
}
+ // First compute how many more features we need to extract from this image
+ int num_featsneeded = num_features - (int)pts0.size();
+
+ // If we don't need any features, just return
+ if (num_featsneeded < std::min(75, (int)(0.2 * num_features)))
+ return;
+
// Extract our features (use fast with griding)
std::vector pts0_ext;
- Grider_FAST::perform_griding(img0pyr.at(0), pts0_ext, num_featsneeded, grid_x, grid_y, threshold, true);
+ Grider_FAST::perform_griding(img0pyr.at(0), mask0, pts0_ext, num_featsneeded, grid_x, grid_y, threshold, true);
// Now, reject features that are close a current feature
std::vector kpts0_new;
std::vector pts0_new;
for (auto &kpt : pts0_ext) {
// Check that it is in bounds
- bool outof_bounds_0 = (kpt.pt.x < 0 || kpt.pt.y < 0 || kpt.pt.x >= img0pyr.at(0).cols || kpt.pt.y >= img0pyr.at(0).rows);
- if (outof_bounds_0)
+ int x_grid = (int)(kpt.pt.x / (float)min_px_dist);
+ int y_grid = (int)(kpt.pt.y / (float)min_px_dist);
+ if (x_grid < 0 || x_grid >= size.width || y_grid < 0 || y_grid >= size.height)
continue;
// See if there is a point at this location
- if (grid_2d((int)(kpt.pt.y / min_px_dist), (int)(kpt.pt.x / min_px_dist)) == 1)
+ if (grid_2d.at(y_grid, x_grid) > 127)
continue;
// Else lets add it!
kpts0_new.push_back(kpt);
pts0_new.push_back(kpt.pt);
- grid_2d((int)(kpt.pt.y / min_px_dist), (int)(kpt.pt.x / min_px_dist)) = 1;
+ grid_2d.at(y_grid, x_grid) = 255;
}
// Loop through and record only ones that are valid
@@ -380,49 +445,45 @@ void TrackKLT::perform_detection_monocular(const std::vector &img0pyr,
}
}
-void TrackKLT::perform_detection_stereo(const std::vector &img0pyr, const std::vector &img1pyr,
- std::vector &pts0, std::vector &pts1,
- std::vector &ids0, std::vector &ids1) {
+void TrackKLT::perform_detection_stereo(const std::vector &img0pyr, const std::vector &img1pyr, const cv::Mat &mask0,
+ const cv::Mat &mask1, size_t cam_id_left, size_t cam_id_right, std::vector &pts0,
+ std::vector &pts1, std::vector &ids0, std::vector &ids1) {
// Create a 2D occupancy grid for this current image
// Note that we scale this down, so that each grid point is equal to a set of pixels
// This means that we will reject points that less then grid_px_size points away then existing features
- // TODO: figure out why I need to add the windowsize of the klt to handle features that are outside the image bound
- // TODO: I assume this is because klt of features at the corners is not really well defined, thus if it doesn't get a match it will be
- // out-of-bounds
- Eigen::MatrixXi grid_2d_0 =
- Eigen::MatrixXi::Zero((int)(img0pyr.at(0).rows / min_px_dist) + 15, (int)(img0pyr.at(0).cols / min_px_dist) + 15);
- Eigen::MatrixXi grid_2d_1 =
- Eigen::MatrixXi::Zero((int)(img1pyr.at(0).rows / min_px_dist) + 15, (int)(img1pyr.at(0).cols / min_px_dist) + 15);
+ cv::Size size0((int)((float)img0pyr.at(0).cols / (float)min_px_dist), (int)((float)img0pyr.at(0).rows / (float)min_px_dist));
+ cv::Mat grid_2d_0 = cv::Mat::zeros(size0, CV_8UC1);
auto it0 = pts0.begin();
auto it1 = ids0.begin();
while (it0 != pts0.end()) {
- // Get current left keypoint
+ // Get current left keypoint, check that it is in bounds
cv::KeyPoint kpt = *it0;
- // Check if this keypoint is near another point
- if (grid_2d_0((int)(kpt.pt.y / min_px_dist), (int)(kpt.pt.x / min_px_dist)) == 1) {
+ int x = (int)kpt.pt.x;
+ int y = (int)kpt.pt.y;
+ int x_grid = (int)(kpt.pt.x / (float)min_px_dist);
+ int y_grid = (int)(kpt.pt.y / (float)min_px_dist);
+ if (x_grid < 0 || x_grid >= size0.width || y_grid < 0 || y_grid >= size0.height || x < 0 || x >= img0pyr.at(0).cols || y < 0 ||
+ y >= img0pyr.at(0).rows) {
it0 = pts0.erase(it0);
it1 = ids0.erase(it1);
continue;
}
- // Else we are good, move forward to the next point
- grid_2d_0((int)(kpt.pt.y / min_px_dist), (int)(kpt.pt.x / min_px_dist)) = 1;
- it0++;
- it1++;
- }
- it0 = pts1.begin();
- it1 = ids1.begin();
- while (it0 != pts1.end()) {
- // Get current right keypoint
- cv::KeyPoint kpt = *it0;
// Check if this keypoint is near another point
- if (grid_2d_1((int)(kpt.pt.y / min_px_dist), (int)(kpt.pt.x / min_px_dist)) == 1) {
- it0 = pts1.erase(it0);
- it1 = ids1.erase(it1);
+ if (grid_2d_0.at(y_grid, x_grid) > 127) {
+ it0 = pts0.erase(it0);
+ it1 = ids0.erase(it1);
+ continue;
+ }
+ // Now check if it is in a mask area or not
+ // NOTE: mask has max value of 255 (white) if it should be
+ if (mask0.at(y, x) > 127) {
+ it0 = pts0.erase(it0);
+ it1 = ids0.erase(it1);
continue;
}
// Else we are good, move forward to the next point
- grid_2d_1((int)(kpt.pt.y / min_px_dist), (int)(kpt.pt.x / min_px_dist)) = 1;
+ grid_2d_0.at(y_grid, x_grid) = 255;
it0++;
it1++;
}
@@ -433,39 +494,47 @@ void TrackKLT::perform_detection_stereo(const std::vector &img0pyr, con
// LEFT: if we need features we should extract them in the current frame
// LEFT: we will also try to track them from this frame over to the right frame
// LEFT: in the case that we have two features that are the same, then we should merge them
- if (num_featsneeded_0 > 0.2 * num_features) {
+ if (num_featsneeded_0 > std::min(75, (int)(0.2 * num_features))) {
// Extract our features (use fast with griding)
std::vector pts0_ext;
- Grider_FAST::perform_griding(img0pyr.at(0), pts0_ext, num_featsneeded_0, grid_x, grid_y, threshold, true);
+ Grider_FAST::perform_griding(img0pyr.at(0), mask0, pts0_ext, num_featsneeded_0, grid_x, grid_y, threshold, true);
// Now, reject features that are close a current feature
std::vector kpts0_new;
std::vector pts0_new;
for (auto &kpt : pts0_ext) {
+ // Check that it is in bounds
+ int x_grid = (int)(kpt.pt.x / (float)min_px_dist);
+ int y_grid = (int)(kpt.pt.y / (float)min_px_dist);
+ if (x_grid < 0 || x_grid >= size0.width || y_grid < 0 || y_grid >= size0.height)
+ continue;
// See if there is a point at this location
- if (grid_2d_0((int)(kpt.pt.y / min_px_dist), (int)(kpt.pt.x / min_px_dist)) == 1)
+ if (grid_2d_0.at(y_grid, x_grid) > 127)
continue;
// Else lets add it!
+ grid_2d_0.at(y_grid, x_grid) = 255;
kpts0_new.push_back(kpt);
pts0_new.push_back(kpt.pt);
- grid_2d_0((int)(kpt.pt.y / min_px_dist), (int)(kpt.pt.x / min_px_dist)) = 1;
}
// TODO: Project points from the left frame into the right frame
// TODO: This will not work for large baseline systems.....
+ // TODO: If we had some depth estimates we could do a better projection
+ // TODO: Or project and search along the epipolar line??
std::vector kpts1_new;
std::vector pts1_new;
kpts1_new = kpts0_new;
pts1_new = pts0_new;
- // If we have points, do KLT tracking to get the valid projections
+ // If we have points, do KLT tracking to get the valid projections into the right image
if (!pts0_new.empty()) {
// Do our KLT tracking from the left to the right frame of reference
// Note: we have a pretty big window size here since our projection might be bad
// Note: but this might cause failure in cases of repeated textures (eg. checkerboard)
std::vector mask;
+ // perform_matching(img0pyr, img1pyr, kpts0_new, kpts1_new, cam_id_left, cam_id_right, mask);
std::vector error;
cv::TermCriteria term_crit = cv::TermCriteria(cv::TermCriteria::COUNT + cv::TermCriteria::EPS, 10, 0.01);
cv::calcOpticalFlowPyrLK(img0pyr, img1pyr, pts0_new, pts1_new, mask, error, win_size, pyr_levels, term_crit,
@@ -474,17 +543,22 @@ void TrackKLT::perform_detection_stereo(const std::vector &img0pyr, con
// Loop through and record only ones that are valid
for (size_t i = 0; i < pts0_new.size(); i++) {
- // Check that our tracks are in bounds
- bool outof_bounds_0 = (pts0_new.at(i).x < 0 || pts0_new.at(i).y < 0 || pts0_new.at(i).x >= img0pyr.at(0).cols ||
- pts0_new.at(i).y >= img0pyr.at(0).rows);
- bool outof_bounds_1 = (pts1_new.at(i).x < 0 || pts1_new.at(i).y < 0 || pts1_new.at(i).x >= img1pyr.at(0).cols ||
- pts1_new.at(i).y >= img1pyr.at(0).rows);
+ // Check that it is in bounds
+ if ((int)pts0_new.at(i).x < 0 || (int)pts0_new.at(i).x >= img0pyr.at(0).cols || (int)pts0_new.at(i).y < 0 ||
+ (int)pts0_new.at(i).y >= img0pyr.at(0).rows) {
+ continue;
+ }
+ if ((int)pts1_new.at(i).x < 0 || (int)pts1_new.at(i).x >= img1pyr.at(0).cols || (int)pts1_new.at(i).y < 0 ||
+ (int)pts1_new.at(i).y >= img1pyr.at(0).rows) {
+ continue;
+ }
- // If this is not already in the right image, then we should treat it as a stereo
- // Otherwise we will treat this as just a monocular track of the feature
+ // Check to see if it there is already a feature in the right image at this location
+ // 1) If this is not already in the right image, then we should treat it as a stereo
+ // 2) Otherwise we will treat this as just a monocular track of the feature
// TODO: we should check to see if we can combine this new feature and the one in the right
- if (!outof_bounds_0 && mask[i] == 1 && !outof_bounds_1 &&
- !(grid_2d_1((int)(pts1_new.at(i).y / min_px_dist), (int)(pts1_new.at(i).x / min_px_dist)) == 1)) {
+ // TODO: seems if reject features which overlay with right features already we have very poor tracking perf
+ if (mask[i] == 1) {
// update the uv coordinates
kpts0_new.at(i).pt = pts0_new.at(i);
kpts1_new.at(i).pt = pts1_new.at(i);
@@ -495,7 +569,7 @@ void TrackKLT::perform_detection_stereo(const std::vector &img0pyr, con
size_t temp = ++currid;
ids0.push_back(temp);
ids1.push_back(temp);
- } else if (!outof_bounds_0) {
+ } else {
// update the uv coordinates
kpts0_new.at(i).pt = pts0_new.at(i);
// append the new uv coordinate
@@ -508,25 +582,73 @@ void TrackKLT::perform_detection_stereo(const std::vector &img0pyr, con
}
}
+ // RIGHT: Now summarise the number of tracks in the right image
+ // RIGHT: We will try to extract some monocular features if we have the room
+ // RIGHT: This will also remove features if there are multiple in the same location
+ cv::Size size1((int)((float)img1pyr.at(0).cols / (float)min_px_dist), (int)((float)img1pyr.at(0).rows / (float)min_px_dist));
+ cv::Mat grid_2d_1 = cv::Mat::zeros(size1, CV_8UC1);
+ it0 = pts1.begin();
+ it1 = ids1.begin();
+ while (it0 != pts1.end()) {
+ // Get current left keypoint, check that it is in bounds
+ cv::KeyPoint kpt = *it0;
+ int x = (int)kpt.pt.x;
+ int y = (int)kpt.pt.y;
+ int x_grid = (int)(kpt.pt.x / (float)min_px_dist);
+ int y_grid = (int)(kpt.pt.y / (float)min_px_dist);
+ if (x_grid < 0 || x_grid >= size1.width || y_grid < 0 || y_grid >= size1.height || x < 0 || x >= img1pyr.at(0).cols || y < 0 ||
+ y >= img1pyr.at(0).rows) {
+ it0 = pts1.erase(it0);
+ it1 = ids1.erase(it1);
+ continue;
+ }
+ // Check if this is a stereo point
+ bool is_stereo = (std::find(ids0.begin(), ids0.end(), *it1) != ids0.end());
+ // Check if this keypoint is near another point
+ // NOTE: if it is *not* a stereo point, then we will not delete the feature
+ // NOTE: this means we might have a mono and stereo feature near each other, but that is ok
+ if (grid_2d_1.at(y_grid, x_grid) > 127 && !is_stereo) {
+ it0 = pts1.erase(it0);
+ it1 = ids1.erase(it1);
+ continue;
+ }
+ // Now check if it is in a mask area or not
+ // NOTE: mask has max value of 255 (white) if it should be
+ if (mask1.at(y, x) > 127) {
+ it0 = pts1.erase(it0);
+ it1 = ids1.erase(it1);
+ continue;
+ }
+ // Else we are good, move forward to the next point
+ grid_2d_1.at(y_grid, x_grid) = 255;
+ it0++;
+ it1++;
+ }
+
// RIGHT: if we need features we should extract them in the current frame
// RIGHT: note that we don't track them to the left as we already did left->right tracking above
int num_featsneeded_1 = num_features - (int)pts1.size();
- if (num_featsneeded_1 > 0.2 * num_features) {
+ if (num_featsneeded_1 > std::min(75, (int)(0.2 * num_features))) {
// Extract our features (use fast with griding)
std::vector pts1_ext;
- Grider_FAST::perform_griding(img1pyr.at(0), pts1_ext, num_featsneeded_1, grid_x, grid_y, threshold, true);
+ Grider_FAST::perform_griding(img1pyr.at(0), mask1, pts1_ext, num_featsneeded_1, grid_x, grid_y, threshold, true);
// Now, reject features that are close a current feature
for (auto &kpt : pts1_ext) {
+ // Check that it is in bounds
+ int x_grid = (int)(kpt.pt.x / (float)min_px_dist);
+ int y_grid = (int)(kpt.pt.y / (float)min_px_dist);
+ if (x_grid < 0 || x_grid >= size1.width || y_grid < 0 || y_grid >= size1.height)
+ continue;
// See if there is a point at this location
- if (grid_2d_1((int)(kpt.pt.y / min_px_dist), (int)(kpt.pt.x / min_px_dist)) == 1)
+ if (grid_2d_1.at(y_grid, x_grid) > 127)
continue;
// Else lets add it!
pts1.push_back(kpt);
size_t temp = ++currid;
ids1.push_back(temp);
- grid_2d_1((int)(kpt.pt.y / min_px_dist), (int)(kpt.pt.x / min_px_dist)) = 1;
+ grid_2d_1.at(y_grid, x_grid) = 255;
}
}
}
@@ -559,21 +681,21 @@ void TrackKLT::perform_matching(const std::vector &img0pyr, const std::
// Now do KLT tracking to get the valid new points
std::vector mask_klt;
std::vector error;
- cv::TermCriteria term_crit = cv::TermCriteria(cv::TermCriteria::COUNT + cv::TermCriteria::EPS, 10, 0.01);
+ cv::TermCriteria term_crit = cv::TermCriteria(cv::TermCriteria::COUNT + cv::TermCriteria::EPS, 20, 0.01);
cv::calcOpticalFlowPyrLK(img0pyr, img1pyr, pts0, pts1, mask_klt, error, win_size, pyr_levels, term_crit, cv::OPTFLOW_USE_INITIAL_FLOW);
// Normalize these points, so we can then do ransac
// We don't want to do ransac on distorted image uvs since the mapping is nonlinear
std::vector