easy_aruco包代码逻辑梳理

easy_aruco包代码逻辑梳理

easy_aruco源码

OpenCV tutorial

请配合源码阅读该文章

ROS 节点的配置

  <node pkg="tf2_ros" type="static_transform_publisher" name="board_mid"
    args="0.1 0.0 0.0 0.0 0.0 0.0 1.0 board board_mid">
  </node>
  
  <node pkg="easy_aruco" type="easy_aruco_node" name="easy_aruco_node" output="screen" >
    <param name="object_type" value="charuco_board" />
    <param name="camera_namespace" value="/usb_cam" />
    <param name="dictionary" value="DICT_4X4_250"/>
    <param name="camera_frame" value="camera_frame"/>
    <param name="reference_frame" value="base_link"/>
    <param name="marker_size" value="0.03"/>
    <param name="square_size" value="0.04"/>
    <param name="square_number_x" value="5"/>
    <param name="square_number_y" value="7"/>
  </node>

这里面设置了easy_aruco_node的一些参数，例如类型、相机的命名空间、参照系等等，这里是他的用法，接下来进入逻辑的分析

easy_aruco_node.cpp

在easy_aruco/src下存在该文件，根据上面的launch文件的配置，设置了object_type，并且根据object_type选择合适的detector，完成选择后调用 detector->start()，该函数存在于 Detector.cpp中

Detector.cpp

包含三个函数

start()

1、根据launch文件，设置基本参数，包括 camera_namespace、camera_frame等

2、创建broadcaster与listener，用于发布和订阅坐标变换(transformation)相关的消息

3、创建debugImagePublisher和base2boardPublisher用于发布调试图像信息(“/easy_aruco_node/debug_image”)以及位姿变换信息(“/easy_aruco_node/base2board_pose”)

4、获取相机信息并调用onCameraInfo()方法，完成对相机参数的传递，以及坐标变换关系的设置

5、调用/easy_aruco/src/ArucoMarker或CharucoBoardDetector.cpp的startImpl()方法，完成对检测器(Detector)的初始化

6、订阅图像话题，获取图像信息，并调用onImage()方法，准备进行标记检测与位姿估计

onCameraInfo()

注：以下智能指针的声明在/easy_aruco/include/Detector.h

1、传递相机参数，内参、畸变(默认为0)、尺寸，保存到 newParams

2、将本地变量 newParams 转换为 全局的智能指针 cameraParameters

3、获取referenceFrame 与 cameraFrame之间的转换关系，并将其保存到 cameraToReferenceTransform

4、将本地变量 cameraToReferenceTransform 转换为全局智能指针 cameraToReference

5、获取board_mid与board之间的转换关系，将其保存在BoardToBoardmidTransform

​ 在实际应用时需要自己设置，如上面launch文件

6、将本地变量BoardToBoardmidTransform转换为全局智能指针 BoardToBoardmid

7、完成参数的获取与保存后停止订阅话题

onImage()

如果cameraParameters或cameraToReference有一个为空，跳出，否则执行/easy_aruco/src/Arucomarker或CharucoBoardDetector.cpp的onImageImpl()

ArucoMarker或CharucoBoardDetector.cpp

startImpl()

获取标记尺寸(marker_size)、字典、并实例化detectorParams，用于存放检测后的信息

onImageImpl()

1、把从传感器中读到的ROS消息数据通过cv_bridge::toCvShare转换成openCV的格式并保存到img中

2、调用aruco::detectMarkers()，传入之前的参数，进行标记检测

void cv::aruco::detectMarkers	(	InputArray 	image,
const Ptr< Dictionary > & 			dictionary,
OutputArrayOfArrays 				corners,
OutputArray 						ids,
const Ptr< DetectorParameters > & 	parameters = makePtr< DetectorParameters >(),
OutputArrayOfArrays 				rejectedImgPoints = noArray() 
)	

3、调用aruco::estimatePoseSingleMarkers或aruco::estimatePoseCharucoBoard进行位姿估计

bool cv::aruco::estimatePoseCharucoBoard	(	InputArray 	charucoCorners,
InputArray 										charucoIds,
const Ptr< CharucoBoard > & 					board,
InputArray 										cameraMatrix,
InputArray 										distCoeffs,
InputOutputArray 								rvec,
InputOutputArray 								tvec,
bool 											useExtrinsicGuess = false 
)	

4、发布tf消息

5、发布debug图像