Merge branch 'main' of github.com:Hydran00/aruco_camera_pose_estimator

.github/workflows/humble.yml

@@ -0,0 +1,26 @@
+name: Humble CI
+
+on:
+  push:
+    branches:
+      - main
+
+jobs:
+  humble_source:
+    runs-on: ubuntu-22.04
+    steps:
+      - uses: ros-tooling/[email protected]
+        with:
+          required-ros-distributions: humble
+      # OpenCV
+      - run: sudo apt install libopencv-dev -y
+      # CVBridge
+      - run: sudo apt install ros-humble-cv-bridge -y
+      - uses: ros-tooling/[email protected]
+        with:
+          package-name: calibration_srv
+          target-ros2-distro: humble
+      - uses: ros-tooling/[email protected]
+        with:
+          package-name: aruco_camera_pose_estimator_cpp
+          target-ros2-distro: humble

.github/workflows/jazzy.yml

@@ -0,0 +1,27 @@
+name: Jazzy CI
+
+on:
+  push:
+    branches:
+      - main
+
+jobs:
+  humble_source:
+    runs-on: ubuntu-24.04
+    steps:
+      - uses: ros-tooling/[email protected]
+        with:
+          required-ros-distributions: jazzy
+      # OpenCV
+      - run: sudo apt install libopencv-dev -y
+      # CVBridge
+      - run: sudo apt install ros-jazzy-cv-bridge -y
+      - run: source /opt/ros/jazzy/setup.bash
+      - uses: ros-tooling/[email protected]
+        with:
+          package-name: calibration_srv
+          target-ros2-distro: jazzy
+      - uses: ros-tooling/[email protected]
+        with:
+          package-name: aruco_camera_pose_estimator_cpp
+          target-ros2-distro: jazzy

LICENSE

@@ -0,0 +1,21 @@
+MIT License
+
+Copyright (c) [year] [fullname]
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.

README.md

@@ -1 +1,91 @@
 # aruco_camera_pose_estimator
+![Build Humble](https://github.com/Hydran00/aruco_camera_pose_estimator/actions/workflows/humble.yml/badge.svg)  ![Build Jazzy](https://github.com/Hydran00/aruco_camera_pose_estimator/actions/workflows/jazzy.yml/badge.svg)  
+
+This repo contains a ROS2 metapackage that estimates the camera pose with respect to the world frame exploiting Aruco Markers. This package implements a [ROS2 Service](https://docs.ros.org/en/humble/Tutorials/Beginner-CLI-Tools/Understanding-ROS2-Services/Understanding-ROS2-Services.html) that collects a set of $N$ measurements of the aruco position and orientation and computes the average. Then the resulting camera position and orientation is sent as the service response to the calling node. Each required parameter is loaded from a `.yaml` file.
+
+# Installation
+Install dependecies:
+- [ROS2](https://docs.ros.org/en/humble/Installation.html)
+- OpenCV
+  ```
+  sudo apt install libopencv-dev -y
+  ```
+- ROS CVBridge
+  ```
+  sudo apt-get install ros-$ROS_DISTRO-cv-bridge
+  ```
+
+The following code will build the `calibration_srv` interface and the c++ package of the program:
+```
+cd <path/to/your/ros2/workspace>/src
+git clone https://github.com/Hydran00/aruco_camera_pose_estimator.git
+cd .. 
+colcon build --symlink-install
+```
+Then print your aruco using the 4x4 dictionary. You can use this [website](https://chev.me/arucogen/).
+
+# Run the service server
+In a sourced terminal run
+```
+ros2 launch aruco_camera_pose_estimator_cpp aruco_camera_pose_estimator.launch.py
+```
+In another sourced terminal call the service
+```
+ros2 service call /calibration/get_camera_pose calibration_srv/srv/Calibration "req:
+  data: ''"
+```
+### Maths behind this implementation
+The OpenCV Aruco Library let us compute $M_{c}^{a} \in \mathcal{R}^{4x4}$ that express the rototranslation of the Aruco with respect to the camera frame. 
+
+Since we want $M_{w}^{c}$ (the camera with respect to the world frame), we can compute this matrix using homogeneous matrices multiplication as:
+
+$M_{w}^{c} = M_{w}^{a}  M_{a}^{c}$  
+
+where $M_{a}^{c} \in \mathcal{R}^{4x4}$ is the transpose of $M_{c}^{a}$ and $M_{w}^{a} \in \mathcal{R}^{4x4}$ is the rototranslation of the aruco marker with respect to the world frame.
+
+**Note that if you just need the camera pose with respect to the aruco marker you can neglect $M_{w}^{a}$, i.e. setting it to equal to the identity matrix.**
+
+### Parameters
+#### ROS2 Parameters
+  - `input_topic_name`: ROS2 input topic name of the camera image stream
+  - `output_service_name`: ROS2 output service name of the computed camera pose
+  - `aruco_marker_id`: id of the aruco in the dictionary. 
+  - `n_observation`: the number of measurements used for the computation of the mean rototranslation (`timeout` should be set accordingly)
+  - `timeout_ms`: the amount of time in milliseconds before the data collection is considered failed. Higher `n_observation` means more time to wait for the image stream. When the timeout is reached, the service response is sent having `error` in the `frame_id` field.  
+  - `show_img`: wether to show the axis of the aruco when one is found.
+  - `aruco_size`: the aruco edge size in centimeters
+  - `aruco_XYZ_offset_from_baseframe`: XYZ displacement of the aruco with respect to the world frame. This corresponds to the entries (0,3), (1,3), (2,3) of $M_{w}^{a}$
+  `aruco_rot_offset_from_baseframe`: orientation displacement of the aruco with respect to the world frame. It is specified in (w,x,y,z) quaternion forms. This, after the conversion to the rotation matrix, corresponds to the 3x3 matrix starting from (0,0) in $M_{w}^{a}$
+#### Camera calibration parameters
+Camera intrinsic and distortion coefficients can be set editing `calibration_params.yaml` under `config`.
+
+camera intrinsic:
+  - `cx`
+  - `cy`
+  - `fx`
+  - `fy`    
+These parameters are the one in the calibration matrix
+```math
+\begin{pmatrix}
+  fx & 0 & cx \\
+  0 & fy & cy \\
+  0 & 0 & 1 \\
+\end{pmatrix}
+```
+distortion coefficients:
+  - `k1`
+  - `k2`
+  - `k3`
+  - `p1`
+  - `p2`
+
+**Note: The currently loaded parameters in the yaml file belongs to the RS435 @ 1920x1080.**
+
+Each parameters can be set editing `config.yaml`
+
+### Tips: changing aruco dictionary
+If you need to use a different aruco dictionary you just need to update the following line in `image_processor.cpp`
+
+```
+dictionary_ = cv::aruco::getPredefinedDictionary(cv::aruco::<your_dictionary_name>);
+```

CMakeLists.txt → aruco_camera_pose_estimator_cpp/CMakeLists.txt

@@ -1,5 +1,5 @@
 cmake_minimum_required(VERSION 3.8)
-project(aruco_camera_pose_estimator)
+project(aruco_camera_pose_estimator_cpp)
 
 # Default to C++14
 if(NOT CMAKE_CXX_STANDARD)
@@ -14,17 +14,17 @@ find_package(ament_cmake_python REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(sensor_msgs REQUIRED)
 find_package(geometry_msgs REQUIRED)
-find_package( OpenCV REQUIRED )
-find_package( Eigen3 REQUIRED )
+find_package(OpenCV REQUIRED )
+find_package(Eigen3 REQUIRED )
 find_package(cv_bridge REQUIRED)
 find_package(OpenCV REQUIRED)
 find_package(calibration_srv REQUIRED)
 
-add_executable(aruco_camera_pose_estimator
+add_executable(aruco_camera_pose_estimator_cpp
   src/pose_service.cpp
   src/image_processor.cpp
 )
-ament_target_dependencies(aruco_camera_pose_estimator
+ament_target_dependencies(aruco_camera_pose_estimator_cpp
   rclcpp
     sensor_msgs
     geometry_msgs
@@ -35,16 +35,16 @@ include_directories(
   ${OpenCV_INCLUDE_DIRS}
   include
 )
-target_link_libraries(aruco_camera_pose_estimator
+target_link_libraries(aruco_camera_pose_estimator_cpp
   Eigen3::Eigen
   ${OpenCV_LIBS}
 )
 install(TARGETS
-  aruco_camera_pose_estimator
+  aruco_camera_pose_estimator_cpp
   DESTINATION lib/${PROJECT_NAME}
 )
 install(
-  DIRECTORY config
+  DIRECTORY config launch
   DESTINATION share/${PROJECT_NAME}
 )
 

config/calibration_params.yaml → aruco_camera_pose_estimator_cpp/config/calibration_params.yaml

@@ -1,7 +1,13 @@
 pose_service:
   ros__parameters:
-    # rs435 parameters
+    # rs435 intrinsic parameters
     cx: 958.620910644531
     cy: 564.277893066406
     fx: 1376.80395507812
     fy: 1376.80322265625
+    # rs435 distortion parameter
+    k1: 0.0
+    k2: 0.0
+    k3: 0.0
+    p1: 0.0
+    p2: 0.0

config/config.yaml → aruco_camera_pose_estimator_cpp/config/config.yaml

@@ -15,4 +15,16 @@ pose_service:
       - 0.0
       - 0.0
       - 0.7071067811865475
-      - 0.7071067811865475
+      - 0.7071067811865475
+
+# if you just need the pose of the camera, with respect to the aruco marker
+# use this insted:
+#   aruco_XYZ_offset_from_baseframe: # in meters
+#     - 0.0
+#     - 0.0
+#     - 0.0
+#   aruco_rot_offset_from_baseframe: # in quaternion form (w, x, y, z)
+#     - 1.0
+#     - 0.0
+#     - 0.0
+#     - 0.0

include/image_processor.hpp → aruco_camera_pose_estimator_cpp/include/image_processor.hpp

@@ -16,7 +16,8 @@ class ImageProcessor : public rclcpp::Node {
                  Eigen::Vector3d &mean_tvec, Eigen::Quaterniond &mean_quat,
                  bool &mean_computed, bool show_img, uint16_t aruco_marker_id,
                  double arucobase_size, double cx, double cy, double fx,
-                 double fy);
+                 double fy, double k1, double k2, double k3, double p1,
+                 double p2);
   std::string topic_name_;
   uint32_t N_;
   Eigen::Vector3d &mean_tvec_;
@@ -36,8 +37,9 @@ class ImageProcessor : public rclcpp::Node {
   uint16_t aruco_marker_id_;
   double arucobase_size_;
   double cx_, cy_, fx_, fy_;
-  cv::Mat intrinsic_camera_ = cv::Mat_<double>(3, 3);  // << fx, 0, cx, 0, fy, cy, 0, 0, 1);
-  cv::Mat distortion_camera_ = cv::Mat_<double>(1, 5); // << 0, 0, 0, 0, 0);
+  double k1_, k2_, k3_, p1_, p2_;
+  cv::Mat intrinsic_camera_ = cv::Mat_<double>(3, 3);
+  cv::Mat distortion_camera_ = cv::Mat_<double>(1, 5);
 
   bool camera_pose_estimation(cv::Mat &frame, cv::Vec3d &tvec_camera,
                               cv::Vec3d &rvec_camera);

aruco_camera_pose_estimator_cpp/launch/aruco_camera_pose_estimator.launch.py

@@ -0,0 +1,28 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from launch.actions import IncludeLaunchDescription, OpaqueFunction
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch.launch_context import LaunchContext
+from launch_ros.substitutions import FindPackageShare
+from launch_ros.parameter_descriptions import ParameterFile
+from ament_index_python.packages import get_package_share_directory
+
+
+import os
+
+
+def generate_launch_description():
+    ld = LaunchDescription()
+
+    aruco_calibration_node = Node(
+        package="aruco_camera_pose_estimator_cpp",
+        executable="aruco_camera_pose_estimator_cpp",
+        # get parameters from yaml file
+        parameters=[ParameterFile(get_package_share_directory('aruco_camera_pose_estimator') + "/config/config.yaml"),
+                    ParameterFile(get_package_share_directory('aruco_camera_pose_estimator') + "/config/calibration_params.yaml")
+                    ]
+    )
+    ####################################################################
+    ld.add_action(aruco_calibration_node)
+    return ld

aruco_camera_pose_estimator_cpp/package.xml

@@ -0,0 +1,17 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>aruco_camera_pose_estimator_cpp</name>
+  <version>0.0.1</version>
+  <description>ROS2 package for estimating the position and orientation of the camera using Aruco
+    markers</description>
+  <maintainer email="[email protected]">hydran00</maintainer>
+  <license>MIT</license>
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <depend>cv_bridge</depend>
+  <depend>calibration_srv</depend>
+  <depend>rclcpp</depend>
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/image_processor.cpp → aruco_camera_pose_estimator_cpp/src/image_processor.cpp

@@ -10,12 +10,11 @@
 
 using namespace std::chrono_literals;
 
-ImageProcessor::ImageProcessor(const std::string &topic_name, const uint32_t N,
-                               Eigen::Vector3d &mean_tvec,
-                               Eigen::Quaterniond &mean_quat,
-                               bool &mean_computed, bool show_img,
-                               uint16_t aruco_marker_id, double arucobase_size,
-                               double cx, double cy, double fx, double fy)
+ImageProcessor::ImageProcessor(
+    const std::string &topic_name, const uint32_t N, Eigen::Vector3d &mean_tvec,
+    Eigen::Quaterniond &mean_quat, bool &mean_computed, bool show_img,
+    uint16_t aruco_marker_id, double arucobase_size, double cx, double cy,
+    double fx, double fy, double k1, double k2, double k3, double p1, double p2)
     : Node("image_processor"),
       topic_name_(topic_name),
       N_(N),
@@ -28,14 +27,19 @@ ImageProcessor::ImageProcessor(const std::string &topic_name, const uint32_t N,
       cx_(cx),
       cy_(cy),
       fx_(fx),
-      fy_(fy) {
+      fy_(fy),
+      k1_(k1),
+      k2_(k2),
+      k3_(k3),
+      p1_(p1),
+      p2_(p2) {
   idx_ = 0;
   dictionary_ = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_4X4_50);
   detector_params_ = cv::aruco::DetectorParameters::create();
   tvec_cam_list_.resize(N_, cv::Vec3d(0, 0, 0));
   rvec_cam_list_.resize(N_, cv::Vec3d(0, 0, 0));
 
-  distortion_camera_ = (cv::Mat_<double>(1, 5) << 0, 0, 0, 0, 0);
+  distortion_camera_ = (cv::Mat_<double>(1, 5) << k1_, k2_, p1_, p2_, k3_);
   intrinsic_camera_ =
       (cv::Mat_<double>(3, 3) << fx_, 0, cx_, 0, fy_, cy_, 0, 0, 1);
 }
@@ -66,7 +70,7 @@ bool ImageProcessor::camera_pose_estimation(cv::Mat &frame,
           cv::imshow("Aruco Marker", frame);
           cv::waitKey(1);
         }
-      return true;
+        return true;
       }
     }
   }