Skip to content

Commit be2c63a

Browse files
marcoesposito1988marcoesposito1988
committed
First commit: working Aruco and Charuco tracking
1 parent 23b0653 commit be2c63a

18 files changed

+787
-0
lines changed

‎CMakeLists.txt

+43
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,43 @@
1+
cmake_minimum_required(VERSION 2.8.3)
2+
project(easy_aruco)
3+
4+
find_package(catkin REQUIRED COMPONENTS sensor_msgs cv_bridge tf2_ros tf2_geometry_msgs)
5+
6+
find_package(OpenCV REQUIRED)
7+
8+
catkin_package()
9+
10+
include_directories(
11+
include
12+
${catkin_INCLUDE_DIRS}
13+
)
14+
15+
add_executable(${PROJECT_NAME}_node
16+
include/ArucoMarkerDetector.h
17+
include/CharucoBoardDetector.h
18+
include/Detector.h
19+
include/Utils.h
20+
src/ArucoMarkerDetector.cpp
21+
src/CharucoBoardDetector.cpp
22+
src/Detector.cpp
23+
src/Utils.cpp
24+
src/easy_aruco_node.cpp
25+
)
26+
27+
target_link_libraries(${PROJECT_NAME}_node
28+
${catkin_LIBRARIES}
29+
${OpenCV_LIBS}
30+
)
31+
32+
install(PROGRAMS
33+
scripts/create_charuco_board.py
34+
scripts/create_aruco_marker.py
35+
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
36+
)
37+
38+
## Mark executables and/or libraries for installation
39+
install(TARGETS ${PROJECT_NAME}_node
40+
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
41+
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
42+
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
43+
)

‎README.md

+61
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,61 @@
1+
# easy_aruco
2+
3+
This package provides Yet Another Wrapper for the [ArUco library](https://www.uco.es/investiga/grupos/ava/node/26), in particular
4+
for the version [integrated into OpenCV 3.1+](https://docs.opencv.org/3.1.0/d9/d6a/group__aruco.html). It includes:
5+
- scripts to generate ArUco markers or ChArUco boards with given physical dimensions
6+
- a node for tracking individual ArUco markers
7+
- a node for tracking a ChArUco board (usually more stable and reliable than a marker)
8+
9+
The pose of each tracked object is published on tf. It is possible to publish the pose of each object with respect to an arbitrary
10+
reference frame which is parent of the camera frame, as required by common hand-eye calibration setups
11+
(for example see the related package [easy_handeye](https://github.com/IFL-CAMP/easy_handeye)).
12+
13+
ArUco marker | ChArUco board
14+
:-------------------------:|:-------------------------:
15+
![](docs/img/example_marker.png) | ![](docs/img/example_board.png)
16+
17+
## Getting started
18+
19+
- make sure that your camera is calibrated! RGB-D cameras usually provide decent factory calibrations through the respective
20+
ROS driver, but RGB cameras such as webcams will require you to perform
21+
an [intrinsic calibration](http://wiki.ros.org/camera_calibration/Tutorials/MonocularCalibration).
22+
23+
- check out this repository into your catkin workspace:
24+
```bash
25+
# create a new workspace if needed
26+
source /opt/ros/$ROSDISTRO/devel/setup.bash
27+
mkdir -p catkin_ws/src
28+
cd catkin_ws/src
29+
30+
# check out the repository, satisfy the dependencies and build
31+
git clone https://github.com/marcoesposito1988/easy_aruco
32+
cd ..
33+
rosdep install -yir --from-paths src
34+
catkin build
35+
source devel/setup.bash
36+
```
37+
38+
- generate a ChArUco board:
39+
```bash
40+
# example parameters: 5x7 chessboard, squares 40mm and markers 20mm big
41+
rosrun easy_aruco create_charuco_board.py --dictionary DICT_6X6_250 --squares_x 7 --squares_y 9 --square_size 24 --marker_size 16 --output_path ~/charuco_board.pdf
42+
```
43+
44+
- print the PDF making sure that the rescaling option (also called sometimes 'Fit to size' or 'Auto shrink') is DISABLED
45+
46+
- __CHECK WITH A RULER THAT THE PHYSICAL DIMENSION OF THE MARKER CORRESPONDS TO WHAT YOU PROVIDED IN MILLIMETERS AS ARGUMENT TO THE SCRIPT__
47+
48+
- start your ROS camera driver, for example the Orbbec ASTRA:
49+
```bash
50+
roslaunch astra_camera astra.launch
51+
```
52+
53+
- start the marker tracking, specifying the parameters of the marker and the namespace of the topics generated by the ROS camera
54+
driver (in the Orbbec case, `/camera/rgb/camera_info` for the calibration and `/camera/rgb/image_rect_color` for the rectified
55+
color image; for a driver respecting the standard, it suffices to provide the root namespace `/camera/rgb/`):
56+
```bash
57+
roslaunch easy_aruco track_charuco_board.launch camera_namespace:=/camera/rgb dictionary:=DICT_6X6_250 square_number_x:=7 square_number_y:=9 square_size:=0.024 marker_size:=0.016
58+
```
59+
60+
- the pose of the board is now available in tf
61+

‎docs/img/example_board.png

386 KB
Loading

‎docs/img/example_marker.png

330 KB
Loading

‎include/ArucoMarkerDetector.h

+23
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,23 @@
1+
/* Copyright (c) 2012-2019 ImFusion GmbH, Munich, Germany. All rights reserved.
2+
*/
3+
#pragma once
4+
5+
#include "Detector.h"
6+
#include "Utils.h"
7+
8+
#include <opencv2/aruco.hpp>
9+
#include <sensor_msgs/Image.h>
10+
11+
class ArucoMarkerDetector : public Detector {
12+
public:
13+
explicit ArucoMarkerDetector(const ros::NodeHandle &nh)
14+
: Detector(nh) {}
15+
16+
protected:
17+
cv::Ptr<cv::aruco::DetectorParameters> detectorParams;
18+
cv::Ptr<cv::aruco::Dictionary> dictionary;
19+
float markerSideLengthMeters = -1;
20+
21+
void startImpl() override;
22+
void onImageImpl(const sensor_msgs::ImageConstPtr &img) override;
23+
};

‎include/CharucoBoardDetector.h

+25
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,25 @@
1+
/* Copyright (c) 2012-2019 ImFusion GmbH, Munich, Germany. All rights reserved.
2+
*/
3+
#pragma once
4+
5+
#include "Detector.h"
6+
#include <opencv2/aruco/charuco.hpp>
7+
#include <sensor_msgs/Image.h>
8+
9+
class CharucoBoardDetector: public Detector {
10+
public:
11+
explicit CharucoBoardDetector(const ros::NodeHandle &nh)
12+
: Detector(nh) {}
13+
14+
protected:
15+
cv::Ptr<cv::aruco::CharucoBoard> board;
16+
cv::Ptr<cv::aruco::DetectorParameters> detectorParams;
17+
cv::Ptr<cv::aruco::Dictionary> dictionary;
18+
float markerSideLengthMeters = -1;
19+
float squareSideLengthMeters = -1;
20+
int squareNumberX = -1;
21+
int squareNumberY = -1;
22+
23+
void startImpl() override;
24+
void onImageImpl(const sensor_msgs::ImageConstPtr &img) override;
25+
};

‎include/Detector.h

+42
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,42 @@
1+
/* Copyright (c) 2012-2019 ImFusion GmbH, Munich, Germany. All rights reserved.
2+
*/
3+
#pragma once
4+
5+
#include "Utils.h"
6+
#include <ros/ros.h>
7+
#include <sensor_msgs/CameraInfo.h>
8+
#include <sensor_msgs/Image.h>
9+
10+
#include <tf2_ros/transform_broadcaster.h>
11+
#include <tf2_ros/transform_listener.h>
12+
13+
class Detector {
14+
public:
15+
explicit Detector(const ros::NodeHandle &nh)
16+
: nh(nh) {}
17+
18+
void onCameraInfo(const sensor_msgs::CameraInfo &msg);
19+
void onImage(const sensor_msgs::ImageConstPtr &img);
20+
void start();
21+
22+
protected:
23+
virtual void startImpl() = 0;
24+
virtual void onImageImpl(const sensor_msgs::ImageConstPtr &img) = 0;
25+
26+
ros::NodeHandle nh;
27+
std::string cameraInfoTopic;
28+
std::string cameraImageTopic;
29+
std::string cameraNamespace;
30+
std::string cameraFrame;
31+
std::string referenceFrame;
32+
33+
tf2_ros::Buffer buffer;
34+
std::unique_ptr<tf2_ros::TransformListener> listener;
35+
std::unique_ptr<tf2_ros::TransformBroadcaster> broadcaster;
36+
std::unique_ptr<tf2::Transform> cameraToReference;
37+
38+
ros::Subscriber imageSubscriber;
39+
ros::Subscriber cameraInfoSubscriberOnce;
40+
ros::Publisher debugImagePublisher;
41+
std::unique_ptr<CameraParameters> cameraParameters;
42+
};

‎include/Utils.h

+21
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,21 @@
1+
/* Copyright (c) 2012-2019 ImFusion GmbH, Munich, Germany. All rights reserved.
2+
*/
3+
#pragma once
4+
5+
#include <opencv2/aruco/dictionary.hpp>
6+
#include <opencv2/core.hpp>
7+
#include <tf2/LinearMath/Transform.h>
8+
9+
struct CameraParameters {
10+
cv::Mat cameraMatrix = cv::Mat(3, 3, CV_64FC1);
11+
cv::Mat distorsionCoeff = cv::Mat(4, 1, CV_64FC1);
12+
cv::Size size;
13+
};
14+
15+
16+
cv::Vec3d rotationVectorWithROSAxes(const cv::Vec3d &Rvec);
17+
18+
tf2::Transform
19+
rotationAndTranslationVectorsToTransform(const cv::Vec3d &Rvec, const cv::Vec3d &Tvec);
20+
21+
cv::aruco::PREDEFINED_DICTIONARY_NAME arucoDictionaryIdFromString(const std::string& idString);

‎launch/track_aruco_marker.launch

+21
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,21 @@
1+
<launch>
2+
<arg name="camera_namespace" default="/camera" doc="The topic namespace containing the camera's camera_info and image_rect_color topics" />
3+
<arg name="dictionary" doc="The OpenCV ID of the ArUco dictionary used to create the marker (e.g. DICT_6X6_250)" />
4+
5+
<arg name="camera_frame" doc="The tf frame corresponding to the optical frame of the RGB camera" />
6+
<arg name="reference_frame" default="" doc="The tf frame with respect to which the pose will be published" />
7+
8+
<arg name="marker_size" doc="The size of the marker (in meters)" />
9+
10+
<node pkg="easy_aruco" type="easy_aruco_node" name="easy_aruco_node" >
11+
<param name="object_type" value="aruco_marker" />
12+
13+
<param name="camera_namespace" value="$(arg camera_namespace)" />
14+
<param name="dictionary" value="$(arg dictionary)" />
15+
16+
<param name="camera_frame" value="$(arg camera_frame)" />
17+
<param name="reference_frame" value="$(arg reference_frame)" />
18+
19+
<param name="marker_size" value="$(arg marker_size)" />
20+
</node>
21+
</launch>

‎launch/track_charuco_board.launch

+27
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,27 @@
1+
<launch>
2+
<arg name="camera_namespace" default="/camera" doc="The topic namespace containing the camera's camera_info and image_rect_color topics" />
3+
<arg name="dictionary" doc="The OpenCV ID of the ArUco dictionary used to create the markers (e.g. DICT_6X6_250)" />
4+
5+
<arg name="camera_frame" doc="The tf frame corresponding to the optical frame of the RGB camera" />
6+
<arg name="reference_frame" default="" doc="The tf frame with respect to which the pose will be published" />
7+
8+
<arg name="marker_size" doc="The size of the markers placed on the white squares of the chessboard (in meters)" />
9+
<arg name="square_size" doc="The size of the squares of the chessboard (in meters)" />
10+
<arg name="square_number_x" doc="The number of squares in the x direction" />
11+
<arg name="square_number_y" doc="The number of squares in the y direction" />
12+
13+
<node pkg="easy_aruco" type="easy_aruco_node" name="easy_aruco_node" >
14+
<param name="object_type" value="charuco_board" />
15+
16+
<param name="camera_namespace" value="$(arg camera_namespace)" />
17+
<param name="dictionary" value="$(arg dictionary)" />
18+
19+
<param name="camera_frame" value="$(arg camera_frame)" />
20+
<param name="reference_frame" value="$(arg reference_frame)" />
21+
22+
<param name="marker_size" value="$(arg marker_size)" />
23+
<param name="square_size" value="$(arg square_size)" />
24+
<param name="square_number_x" value="$(arg square_number_x)" />
25+
<param name="square_number_y" value="$(arg square_number_y)" />
26+
</node>
27+
</launch>

‎package.xml

+20
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,20 @@
1+
<?xml version="1.0"?>
2+
<package format="2">
3+
<name>easy_aruco</name>
4+
<version>0.0.0</version>
5+
<description>Yet another ArUco tracking package for ROS</description>
6+
7+
<author email="esposito@imfusion.com">Marco Esposito</author>
8+
<maintainer email="esposito@imfusion.com">Marco Esposito</maintainer>
9+
10+
<license>LGPLv3</license>
11+
12+
<buildtool_depend>catkin</buildtool_depend>
13+
<depend>opencv3</depend>
14+
<depend>cv_bridge</depend>
15+
<depend>sensor_msgs</depend>
16+
<depend>tf2_ros</depend>
17+
<depend>tf2_geometry_msgs</depend>
18+
<exec_depend>python-click</exec_depend>
19+
20+
</package>

‎scripts/create_aruco_marker.py

+78
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,78 @@
1+
#!/usr/bin/env python2
2+
3+
import tempfile
4+
from fpdf import FPDF
5+
import cv2
6+
import os
7+
import numpy as np
8+
import click
9+
10+
11+
ARUCO_DICTIONARIES = (
12+
'DICT_4X4_50',
13+
'DICT_4X4_100',
14+
'DICT_4X4_250',
15+
'DICT_4X4_1000',
16+
17+
'DICT_5X5_50',
18+
'DICT_5X5_100',
19+
'DICT_5X5_250',
20+
'DICT_5X5_1000',
21+
22+
'DICT_6X6_50',
23+
'DICT_6X6_100',
24+
'DICT_6X6_250',
25+
'DICT_6X6_1000',
26+
27+
'DICT_ARUCO_ORIGINAL',
28+
)
29+
30+
31+
@click.command()
32+
@click.option('--dictionary', prompt=True, type=click.Choice(ARUCO_DICTIONARIES), help='ArUco dictionary to be used to create the board')
33+
@click.option('--marker_id', prompt='Marker ID', type=click.INT, help='The marker ID')
34+
@click.option('--marker_size', prompt='Marker size (mm)', type=click.FLOAT, help='The size of the marker')
35+
@click.option('--output_path', prompt=True, type=click.Path(writable=True), help='The path in which the PDF with the marker should be saved')
36+
def generate_marker(dictionary, marker_id, marker_size, output_path):
37+
marker_size_m = marker_size / 1000
38+
39+
complete_path = os.path.abspath(os.path.expanduser(output_path))
40+
41+
A4_SIZE_m = (0.297, 0.21)
42+
PAGE_RESOLUTION = (3508, 2480)
43+
PAGE_PIXELS_PER_METER = np.array(PAGE_RESOLUTION)/np.array(A4_SIZE_m)
44+
marker_size_pixels = np.around(PAGE_PIXELS_PER_METER * marker_size_m).astype('int')
45+
46+
if marker_size_m > A4_SIZE_m[0] or marker_size_m > A4_SIZE_m[1]:
47+
raise ValueError('given size exceeds A4')
48+
49+
aruco_dict = cv2.aruco.Dictionary_get(cv2.aruco.DICT_6X6_250)
50+
imboard = cv2.aruco.drawMarker(aruco_dict, marker_id, marker_size_pixels[0])
51+
52+
f = tempfile.NamedTemporaryFile(suffix='.png', delete=False)
53+
54+
cv2.imwrite(f.name, imboard)
55+
56+
marker_size_x_mm = marker_size_m * 1000
57+
marker_size_y_mm = marker_size_m * 1000
58+
59+
pdf = FPDF('P', 'mm', 'A4')
60+
61+
pdf.add_page('P')
62+
63+
pdf.image(f.name, x=(A4_SIZE_m[1]/2*1000)-marker_size_x_mm/2, y=(A4_SIZE_m[0]/2*1000)-marker_size_y_mm/2, w=marker_size_x_mm, h=marker_size_y_mm)
64+
65+
print('Configuration:')
66+
print('Marker size in millimeters: '+str(marker_size_x_mm))
67+
print('Marker ID: ' + str(marker_id))
68+
print('ArUco Dictionary name: ' + dictionary)
69+
70+
print('Saving the marker as an A4 sized PDF in '+os.path.abspath(os.path.expanduser(complete_path)))
71+
72+
pdf.output(complete_path)
73+
74+
print('REMEMBER TO TURN OFF THE AUTOMATIC RESCALING OF THE PRINTER!')
75+
76+
77+
if __name__ == '__main__':
78+
generate_marker()

0 commit comments

Comments
 (0)