Add single and multiple marker board detection nodes

aruco_ros/CMakeLists.txt

@@ -53,11 +53,21 @@ add_executable(marker_publisher src/marker_publish.cpp
 add_dependencies(marker_publisher ${PROJECT_NAME}_gencfg ${catkin_EXPORTED_TARGETS})
 target_link_libraries(marker_publisher ${catkin_LIBRARIES} ${OpenCV_LIBRARIES})
 
+add_executable(single_board src/single_board.cpp
+  src/aruco_ros_utils.cpp)
+add_dependencies(single_board ${PROJECT_NAME}_gencfg)
+target_link_libraries(single_board ${catkin_LIBRARIES} ${OpenCV_LIBRARIES})
+
+add_executable(multi_board src/multi_board.cpp
+  src/aruco_ros_utils.cpp)
+add_dependencies(multi_board ${PROJECT_NAME}_gencfg)
+target_link_libraries(multi_board ${catkin_LIBRARIES} ${OpenCV_LIBRARIES})
+
 #############
 ## Install ##
 #############
 
-install(TARGETS single double marker_publisher aruco_ros_utils
+install(TARGETS single double marker_publisher single_board multi_board aruco_ros_utils
   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}

aruco_ros/include/aruco_ros/aruco_ros_utils.h

@@ -21,5 +21,7 @@ namespace aruco_ros
   //FIXME: make parameter const as soon as the used function is also const
   tf::Transform arucoMarker2Tf(const aruco::Marker& marker, bool rotate_marker_axis=true);
 
+  tf::Transform arucoBoard2Tf(const aruco::Board& board, bool rotate_board_axis=true);
+
 }
 #endif // ARUCO_ROS_UTILS_H

aruco_ros/launch/multi_board.launch

@@ -0,0 +1,23 @@
+<launch>
+
+    <arg name="markerSize"      default="0.16"/>    <!-- in m -->
+    <arg name="eye"             default="left"/>
+    <arg name="board_config"    default="$(find aruco_ros)/samples/multi_board.yml" />
+    <arg name="board_dir"       default="$(find aruco_ros)/samples/" />
+    <arg name="ref_frame"       default=""/>  <!-- leave empty and the pose will be published wrt param parent_name -->
+    <arg name="corner_refinement" default="LINES" /> <!-- NONE, HARRIS, LINES, SUBPIX -->
+
+    <node pkg="aruco_ros" type="multi_board" name="aruco_multi_board">
+        <remap from="/camera_info" to="/stereo/$(arg eye)/camera_info" />
+        <remap from="/image" to="/stereo/$(arg eye)/image_rect_color" />
+        <param name="image_is_rectified" value="True"/>
+        <param name="marker_size"        value="$(arg markerSize)"/>
+        <param name="reference_frame"    value="$(arg ref_frame)"/>   <!-- frame in which the marker pose will be refered -->
+        <param name="camera_frame"       value="stereo_gazebo_$(arg eye)_camera_optical_frame"/>
+        <param name="board_config"       value="$(arg board_config)" />
+        <param name="board_dir"          value="$(arg board_dir)" />
+        <param name="corner_refinement"  value="$(arg corner_refinement)" />
+        <param name="draw_markers"       value="True" />
+    </node>
+
+</launch>

aruco_ros/launch/single_board.launch

@@ -0,0 +1,23 @@
+<launch>
+
+    <arg name="markerSize"      default="0.16"/>    <!-- in m -->
+    <arg name="eye"             default="left"/>
+    <arg name="board_config"    default="$(find aruco_ros)/samples/board_1.yml" />
+    <arg name="board_frame"     default="aruco_board_frame"/>
+    <arg name="ref_frame"       default=""/>  <!-- leave empty and the pose will be published wrt param parent_name -->
+    <arg name="corner_refinement" default="LINES" /> <!-- NONE, HARRIS, LINES, SUBPIX -->
+
+    <node pkg="aruco_ros" type="single_board" name="aruco_single_board">
+        <remap from="/camera_info" to="/stereo/$(arg eye)/camera_info" />
+        <remap from="/image" to="/stereo/$(arg eye)/image_rect_color" />
+        <param name="image_is_rectified" value="True"/>
+        <param name="marker_size"        value="$(arg markerSize)"/>
+        <param name="reference_frame"    value="$(arg ref_frame)"/>   <!-- frame in which the marker pose will be refered -->
+        <param name="camera_frame"       value="stereo_gazebo_$(arg eye)_camera_optical_frame"/>
+        <param name="board_frame"        value="$(arg board_frame)" />
+        <param name="board_config"       value="$(arg board_config)" />
+        <param name="corner_refinement"  value="$(arg corner_refinement)" />
+        <param name="draw_markers"       value="True" />
+    </node>
+
+</launch>

aruco_ros/samples/board_1.yml

@@ -0,0 +1,8 @@
+%YAML:1.0
+aruco_bc_nmarkers: 4
+aruco_bc_mInfoType: 1
+aruco_bc_markers:
+   - { id: 000, corners:[ [ -0.17, -0.17, 0. ], [ -0.01, -0.17, 0. ], [ -0.01, -0.01, 0. ], [ -0.17, -0.01, 0. ] ] }
+   - { id: 001, corners:[ [ 0.01, -0.17, 0. ], [ 0.17, -0.17, 0. ], [ 0.17, -0.01, 0. ], [ 0.01, -0.01, 0. ] ] }
+   - { id: 002, corners:[ [ -0.17, 0.01, 0. ], [ -0.01, 0.01, 0. ], [ -0.01, 0.17, 0. ], [ -0.17, 0.17, 0. ] ] }
+   - { id: 003, corners:[ [ 0.01, 0.01, 0. ], [ 0.17, 0.01, 0. ], [ 0.17, 0.17, 0. ], [ 0.01, 0.17, 0. ] ] }

aruco_ros/samples/board_2.yml

@@ -0,0 +1,8 @@
+%YAML:1.0
+aruco_bc_nmarkers: 4
+aruco_bc_mInfoType: 1
+aruco_bc_markers:
+   - { id: 004, corners:[ [ -0.17, -0.17, 0. ], [ -0.01, -0.17, 0. ], [ -0.01, -0.01, 0. ], [ -0.17, -0.01, 0. ] ] }
+   - { id: 005, corners:[ [ 0.01, -0.17, 0. ], [ 0.17, -0.17, 0. ], [ 0.17, -0.01, 0. ], [ 0.01, -0.01, 0. ] ] }
+   - { id: 006, corners:[ [ -0.17, 0.01, 0. ], [ -0.01, 0.01, 0. ], [ -0.01, 0.17, 0. ], [ -0.17, 0.17, 0. ] ] }
+   - { id: 007, corners:[ [ 0.01, 0.01, 0. ], [ 0.17, 0.01, 0. ], [ 0.17, 0.17, 0. ], [ 0.01, 0.17, 0. ] ] }

aruco_ros/samples/multi_board.yml

@@ -0,0 +1,4 @@
+%YAML:1.0
+aruco_boards:
+   - { frame_id: "/board1", filename: "board_1.yml"}
+   - { frame_id: "/board2", filename: "board_2.yml"}

aruco_ros/src/aruco_ros_utils.cpp

@@ -78,3 +78,39 @@ tf::Transform aruco_ros::arucoMarker2Tf(const aruco::Marker &marker, bool rotate
 
     return tf::Transform(tf_rot, tf_orig);
 }
+
+tf::Transform aruco_ros::arucoBoard2Tf(const aruco::Board &board, bool rotate_board_axis)
+{
+    cv::Mat rot(3, 3, CV_64FC1);
+    cv::Mat Rvec64;
+    board.Rvec.convertTo(Rvec64, CV_64FC1);
+    cv::Rodrigues(Rvec64, rot);
+    cv::Mat tran64;
+    board.Tvec.convertTo(tran64, CV_64FC1);
+
+    // Rotate axis direction as to fit ROS (?)
+    if (rotate_board_axis)
+    {
+      cv::Mat rotate_to_ros(3, 3, CV_64FC1);
+      // -1 0 0
+      // 0 0 1
+      // 0 1 0
+      rotate_to_ros.at<double>(0, 0) = -1.0;
+      rotate_to_ros.at<double>(0, 1) = 0.0;
+      rotate_to_ros.at<double>(0, 2) = 0.0;
+      rotate_to_ros.at<double>(1, 0) = 0.0;
+      rotate_to_ros.at<double>(1, 1) = 0.0;
+      rotate_to_ros.at<double>(1, 2) = 1.0;
+      rotate_to_ros.at<double>(2, 0) = 0.0;
+      rotate_to_ros.at<double>(2, 1) = 1.0;
+      rotate_to_ros.at<double>(2, 2) = 0.0;
+      rot = rot * rotate_to_ros.t();
+    }
+    tf::Matrix3x3 tf_rot(rot.at<double>(0, 0), rot.at<double>(0, 1), rot.at<double>(0, 2),
+                         rot.at<double>(1, 0), rot.at<double>(1, 1), rot.at<double>(1, 2),
+                         rot.at<double>(2, 0), rot.at<double>(2, 1), rot.at<double>(2, 2));
+
+    tf::Vector3 tf_orig(tran64.at<double>(0, 0), tran64.at<double>(1, 0), tran64.at<double>(2, 0));
+
+    return tf::Transform(tf_rot, tf_orig);
+}