Small fixes

.github/workflows/build_ros2.yml

@@ -49,3 +49,24 @@ jobs:
       - name: Run OpenVINS Simulation!
         run: |
           docker run -t --mount type=bind,source=$GITHUB_WORKSPACE,target=/catkin_ws openvins /bin/bash -c "cd /catkin_ws && source install/setup.bash && ros2 run ov_msckf run_simulation src/open_vins/config/rpng_sim/estimator_config.yaml"
+  build_2204:
+    name: "ROS2 Ubuntu 22.04"
+    runs-on: ubuntu-latest
+    steps:
+      - name: Code Checkout
+        uses: actions/checkout@v2
+      - name: Create Workspace and Docker Image
+        run: |
+          export REPO=$(basename $GITHUB_REPOSITORY) &&
+          cd $GITHUB_WORKSPACE/.. && mkdir src/ &&
+          mv $REPO/ src/ && mkdir $REPO/ && mv src/ $REPO/ && cd $REPO/ &&
+          docker build -t openvins -f $GITHUB_WORKSPACE/src/$REPO/Dockerfile_ros2_22_04 .
+      - name: Echo Enviroment
+        run: |
+          docker run -t --mount type=bind,source=$GITHUB_WORKSPACE,target=/catkin_ws openvins /bin/bash -c "echo $ROS_DISTRO && echo $ROS_VERSION"
+      - name: Run Build in Docker
+        run: |
+          docker run -t --mount type=bind,source=$GITHUB_WORKSPACE,target=/catkin_ws openvins /bin/bash -c "cd /catkin_ws && colcon build"
+#      - name: Run OpenVINS Simulation!
+#        run: |
+#          docker run -t --mount type=bind,source=$GITHUB_WORKSPACE,target=/catkin_ws openvins /bin/bash -c "cd /catkin_ws && source install/setup.bash && ros2 run ov_msckf run_simulation src/open_vins/config/rpng_sim/estimator_config.yaml"

Dockerfile_ros2_22_04

@@ -0,0 +1,47 @@
+FROM osrf/ros:humble-desktop
+
+# =========================================================
+# =========================================================
+
+# 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 utilities for fast in terminal edits (nano etc)
+RUN apt-get update && apt-get install -y libeigen3-dev nano git
+
+# Ceres solver install and setup
+RUN sudo apt-get install -y cmake libgoogle-glog-dev libgflags-dev libatlas-base-dev libeigen3-dev libsuitesparse-dev libceres-dev
+# ENV CERES_VERSION="2.0.0"
+# RUN git clone https://ceres-solver.googlesource.com/ceres-solver && \
+#     cd ceres-solver && \
+#     git checkout tags/${CERES_VERSION} && \
+#     mkdir build && cd build && \
+#     cmake .. && \
+#     make -j$(nproc) install && \
+#     rm -rf ../../ceres-solver
+
+# Seems this has Python 3.10 installed on it...
+RUN apt-get update && apt-get install -y python3-dev python3-matplotlib python3-numpy python3-psutil python3-tk
+
+# Install deps needed for clion remote debugging
+# https://blog.jetbrains.com/clion/2020/01/using-docker-with-clion/
+# RUN sed -i '6i\source "/catkin_ws/install/setup.bash"\' /ros_entrypoint.sh
+RUN apt-get update && apt-get install -y ssh build-essential gcc g++ \
+    gdb clang cmake rsync tar python3 && 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"]
+

config/euroc_mav/kalibr_imu_chain.yaml

@@ -18,7 +18,7 @@ imu0:
   model: "kalibr"
   # how to get from Kalibr imu.yaml result file:
   #   - Tw is imu0:gyroscopes:M:
-  #   - R_IMUtoGYRO: is imu0:gyroscopes:M:
+  #   - R_IMUtoGYRO: is imu0:gyroscopes:C_gyro_i:
   #   - Ta is imu0:accelerometers:M:
   #   - R_IMUtoACC not used by Kalibr
   #   - Tg is imu0:gyroscopes:A:

config/kaist/kalibr_imu_chain.yaml

@@ -26,7 +26,7 @@ imu0:
   model: "kalibr"
   # how to get from Kalibr imu.yaml result file:
   #   - Tw is imu0:gyroscopes:M:
-  #   - R_IMUtoGYRO: is imu0:gyroscopes:M:
+  #   - R_IMUtoGYRO: is imu0:gyroscopes:C_gyro_i:
   #   - Ta is imu0:accelerometers:M:
   #   - R_IMUtoACC not used by Kalibr
   #   - Tg is imu0:gyroscopes:A:

config/kaist_vio/kalibr_imu_chain.yaml

@@ -22,7 +22,7 @@ imu0:
   model: "kalibr"
   # how to get from Kalibr imu.yaml result file:
   #   - Tw is imu0:gyroscopes:M:
-  #   - R_IMUtoGYRO: is imu0:gyroscopes:M:
+  #   - R_IMUtoGYRO: is imu0:gyroscopes:C_gyro_i:
   #   - Ta is imu0:accelerometers:M:
   #   - R_IMUtoACC not used by Kalibr
   #   - Tg is imu0:gyroscopes:A:

config/rpng_aruco/kalibr_imu_chain.yaml

@@ -18,7 +18,7 @@ imu0:
   model: "kalibr"
   # how to get from Kalibr imu.yaml result file:
   #   - Tw is imu0:gyroscopes:M:
-  #   - R_IMUtoGYRO: is imu0:gyroscopes:M:
+  #   - R_IMUtoGYRO: is imu0:gyroscopes:C_gyro_i:
   #   - Ta is imu0:accelerometers:M:
   #   - R_IMUtoACC not used by Kalibr
   #   - Tg is imu0:gyroscopes:A:

config/rpng_ironsides/kalibr_imu_chain.yaml

@@ -18,7 +18,7 @@ imu0:
   model: "kalibr"
   # how to get from Kalibr imu.yaml result file:
   #   - Tw is imu0:gyroscopes:M:
-  #   - R_IMUtoGYRO: is imu0:gyroscopes:M:
+  #   - R_IMUtoGYRO: is imu0:gyroscopes:C_gyro_i:
   #   - Ta is imu0:accelerometers:M:
   #   - R_IMUtoACC not used by Kalibr
   #   - Tg is imu0:gyroscopes:A:

config/rpng_plane/kalibr_imu_chain.yaml

@@ -18,7 +18,7 @@ imu0:
   model: "kalibr"
   # how to get from Kalibr imu.yaml result file:
   #   - Tw is imu0:gyroscopes:M:
-  #   - R_IMUtoGYRO: is imu0:gyroscopes:M:
+  #   - R_IMUtoGYRO: is imu0:gyroscopes:C_gyro_i:
   #   - Ta is imu0:accelerometers:M:
   #   - R_IMUtoACC not used by Kalibr
   #   - Tg is imu0:gyroscopes:A:

config/rpng_sim/kalibr_imu_chain.yaml

@@ -18,7 +18,7 @@ imu0:
   model: "kalibr"
   # how to get from Kalibr imu.yaml result file:
   #   - Tw is imu0:gyroscopes:M:
-  #   - R_IMUtoGYRO: is imu0:gyroscopes:M:
+  #   - R_IMUtoGYRO: is imu0:gyroscopes:C_gyro_i:
   #   - Ta is imu0:accelerometers:M:
   #   - R_IMUtoACC not used by Kalibr
   #   - Tg is imu0:gyroscopes:A:

config/rs_d455/kalibr_imu_chain.yaml

@@ -27,7 +27,7 @@ imu0:
   model: "kalibr"
   # how to get from Kalibr imu.yaml result file:
   #   - Tw is imu0:gyroscopes:M:
-  #   - R_IMUtoGYRO: is imu0:gyroscopes:M:
+  #   - R_IMUtoGYRO: is imu0:gyroscopes:C_gyro_i:
   #   - Ta is imu0:accelerometers:M:
   #   - R_IMUtoACC not used by Kalibr
   #   - Tg is imu0:gyroscopes:A:

config/rs_t265/kalibr_imu_chain.yaml

@@ -22,7 +22,7 @@ imu0:
   model: "kalibr"
   # how to get from Kalibr imu.yaml result file:
   #   - Tw is imu0:gyroscopes:M:
-  #   - R_IMUtoGYRO: is imu0:gyroscopes:M:
+  #   - R_IMUtoGYRO: is imu0:gyroscopes:C_gyro_i:
   #   - Ta is imu0:accelerometers:M:
   #   - R_IMUtoACC not used by Kalibr
   #   - Tg is imu0:gyroscopes:A:

config/tum_vi/kalibr_imu_chain.yaml

@@ -18,7 +18,7 @@ imu0:
   model: "kalibr"
   # how to get from Kalibr imu.yaml result file:
   #   - Tw is imu0:gyroscopes:M:
-  #   - R_IMUtoGYRO: is imu0:gyroscopes:M:
+  #   - R_IMUtoGYRO: is imu0:gyroscopes:C_gyro_i:
   #   - Ta is imu0:accelerometers:M:
   #   - R_IMUtoACC not used by Kalibr
   #   - Tg is imu0:gyroscopes:A:

config/uzhfpv_indoor/kalibr_imu_chain.yaml

@@ -22,7 +22,7 @@ imu0:
   model: "kalibr"
   # how to get from Kalibr imu.yaml result file:
   #   - Tw is imu0:gyroscopes:M:
-  #   - R_IMUtoGYRO: is imu0:gyroscopes:M:
+  #   - R_IMUtoGYRO: is imu0:gyroscopes:C_gyro_i:
   #   - Ta is imu0:accelerometers:M:
   #   - R_IMUtoACC not used by Kalibr
   #   - Tg is imu0:gyroscopes:A:

config/uzhfpv_indoor_45/kalibr_imu_chain.yaml

@@ -22,7 +22,7 @@ imu0:
   model: "kalibr"
   # how to get from Kalibr imu.yaml result file:
   #   - Tw is imu0:gyroscopes:M:
-  #   - R_IMUtoGYRO: is imu0:gyroscopes:M:
+  #   - R_IMUtoGYRO: is imu0:gyroscopes:C_gyro_i:
   #   - Ta is imu0:accelerometers:M:
   #   - R_IMUtoACC not used by Kalibr
   #   - Tg is imu0:gyroscopes:A:

config/uzhfpv_outdoor/kalibr_imu_chain.yaml

@@ -22,7 +22,7 @@ imu0:
   model: "kalibr"
   # how to get from Kalibr imu.yaml result file:
   #   - Tw is imu0:gyroscopes:M:
-  #   - R_IMUtoGYRO: is imu0:gyroscopes:M:
+  #   - R_IMUtoGYRO: is imu0:gyroscopes:C_gyro_i:
   #   - Ta is imu0:accelerometers:M:
   #   - R_IMUtoACC not used by Kalibr
   #   - Tg is imu0:gyroscopes:A:

config/uzhfpv_outdoor_45/kalibr_imu_chain.yaml

@@ -22,7 +22,7 @@ imu0:
   model: "kalibr"
   # how to get from Kalibr imu.yaml result file:
   #   - Tw is imu0:gyroscopes:M:
-  #   - R_IMUtoGYRO: is imu0:gyroscopes:M:
+  #   - R_IMUtoGYRO: is imu0:gyroscopes:C_gyro_i:
   #   - Ta is imu0:accelerometers:M:
   #   - R_IMUtoACC not used by Kalibr
   #   - Tg is imu0:gyroscopes:A:

docs/gs-tutorial.dox

@@ -79,7 +79,7 @@ For RVIZ, one can open the `ov_msckf/launch/display.rviz` configuration file.
 You should see the system publishing features and a state estimate.
 
 @code{.shell-session}
-rviz # term 2
+rviz -d ov_msckf/launch/display.rviz # term 2
 rosbag play V1_01_easy.bag # term 3
 @endcode
 

ov_core/src/test_tracking.cpp

@@ -97,7 +97,7 @@ int main(int argc, char **argv) {
   // Location of the ROS bag we want to read in
   std::string path_to_bag;
   nh->param<std::string>("path_bag", path_to_bag, "/home/patrick/datasets/euroc_mav/V1_01_easy.bag");
-  // nh->param<std::string>("path_bag", path_to_bag, "/home/patrick/datasets/open_vins/aruco_room_01.bag");
+  // nh->param<std::string>("path_bag", path_to_bag, "/home/patrick/datasets/rpng_aruco/aruco_room_01.bag");
   PRINT_INFO("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

ov_core/src/track/TrackAruco.cpp

@@ -94,7 +94,11 @@ void TrackAruco::perform_tracking(double timestamp, const cv::Mat &imgin, size_t
   //===================================================================================
 
   // Perform extraction
+#if CV_MAJOR_VERSION > 4 || ( CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION >= 7)
+  aruco_detector.detectMarkers(img0, corners[cam_id], ids_aruco[cam_id], rejects[cam_id]);
+#else
   cv::aruco::detectMarkers(img0, aruco_dict, corners[cam_id], ids_aruco[cam_id], aruco_params, rejects[cam_id]);
+#endif
   rT2 = boost::posix_time::microsec_clock::local_time();
 
   //===================================================================================

ov_core/src/track/TrackAruco.h

@@ -55,10 +55,16 @@ class TrackAruco : public TrackBase {
                       bool downsize)
       : TrackBase(cameras, 0, numaruco, stereo, histmethod), max_tag_id(numaruco), do_downsizing(downsize) {
 #if ENABLE_ARUCO_TAGS
+#if CV_MAJOR_VERSION > 4 || ( CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION >= 7)
+    aruco_dict = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_6X6_1000);
+    aruco_params.cornerRefinementMethod = cv::aruco::CORNER_REFINE_SUBPIX;
+    aruco_detector = cv::aruco::ArucoDetector(aruco_dict, aruco_params);
+#else
     aruco_dict = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_6X6_1000);
     aruco_params = cv::aruco::DetectorParameters::create();
     // NOTE: people with newer opencv might fail here
     // aruco_params->cornerRefinementMethod = cv::aruco::CornerRefineMethod::CORNER_REFINE_SUBPIX;
+#endif
 #else
     PRINT_ERROR(RED "[ERROR]: you have not compiled with aruco tag support!!!\n" RESET);
     std::exit(EXIT_FAILURE);
@@ -101,11 +107,20 @@ class TrackAruco : public TrackBase {
   bool do_downsizing;
 
 #if ENABLE_ARUCO_TAGS
+#if CV_MAJOR_VERSION > 4 || ( CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION >= 7)
+  // Our dictionary that we will extract aruco tags with
+  cv::aruco::Dictionary aruco_dict;
+  // Parameters the opencv extractor uses
+  cv::aruco::DetectorParameters aruco_params;
+  // Actual detector class
+  cv::aruco::ArucoDetector aruco_detector;
+#else
   // Our dictionary that we will extract aruco tags with
   cv::Ptr<cv::aruco::Dictionary> aruco_dict;
-
   // Parameters the opencv extractor uses
   cv::Ptr<cv::aruco::DetectorParameters> aruco_params;
+#endif
+
 
   // Our tag IDs and corner we will get from the extractor
   std::unordered_map<size_t, std::vector<int>> ids_aruco;