aruco_pose: Prevent OpenCV from crashing (#238)

* aruco_pose: Add tests that crash OpenCV

* aruco_pose: Don't try to interpolate single points

aruco_pose/CMakeLists.txt

@@ -25,7 +25,7 @@ find_package(catkin REQUIRED COMPONENTS
 )
 
 find_package(OpenCV 3 REQUIRED COMPONENTS core imgproc calib3d)
-if ("${OpenCV_VERSION_MINOR}" LESS "3")
+if ("${OpenCV_VERSION_MINOR}" LESS "9")
   message(STATUS "OpenCV version too low, using vendored ArUco package")
   include(vendor/VendorOpenCV.cmake)
 else()
@@ -229,4 +229,5 @@ if (CATKIN_ENABLE_TESTING)
   add_rostest(test/test_parser_empty_map.test)
   add_rostest(test/test_node_failure.test)
   add_rostest(test/largemap.test)
+  add_rostest(test/crash_opencv.test)
 endif()

aruco_pose/test/crash_opencv.py

@@ -0,0 +1,18 @@
+import rospy
+import pytest
+
+from visualization_msgs.msg import MarkerArray as VisMarkerArray
+
+
+@pytest.fixture
+def node():
+    return rospy.init_node('aruco_pose_opencv_crash', anonymous=True)
+
+def test_opencv_crashes_img01(node):
+    rospy.wait_for_message('aruco_detect_01/visualization', VisMarkerArray, timeout=5)
+
+def test_opencv_crashes_img02(node):
+    rospy.wait_for_message('aruco_detect_02/visualization', VisMarkerArray, timeout=5)
+
+def test_opencv_crashes_img03(node):
+    rospy.wait_for_message('aruco_detect_03/visualization', VisMarkerArray, timeout=5)

aruco_pose/test/crash_opencv.test

@@ -0,0 +1,51 @@
+<launch>
+    <arg name="corner_method" default="2"/>
+
+    <node pkg="image_publisher" type="image_publisher" name="imgpub_01" args="$(find aruco_pose)/test/crash_image_01.png">
+        <param name="frame_id" value="main_camera_optical"/>
+        <param name="publish_rate" value="10"/>
+        <param name="camera_info_url" value="file://$(find aruco_pose)/test/camera_info.yaml" />
+    </node>
+
+    <node pkg="image_publisher" type="image_publisher" name="imgpub_02" args="$(find aruco_pose)/test/crash_image_02.png">
+        <param name="frame_id" value="main_camera_optical"/>
+        <param name="publish_rate" value="10"/>
+        <param name="camera_info_url" value="file://$(find aruco_pose)/test/camera_info.yaml" />
+    </node>
+
+    <node pkg="image_publisher" type="image_publisher" name="imgpub_03" args="$(find aruco_pose)/test/crash_image_03.png">
+        <param name="frame_id" value="main_camera_optical"/>
+        <param name="publish_rate" value="10"/>
+        <param name="camera_info_url" value="file://$(find aruco_pose)/test/camera_info.yaml" />
+    </node>
+
+    <node pkg="nodelet" type="nodelet" name="nodelet_manager_01" args="manager"/>
+
+    <node pkg="nodelet" clear_params="true" type="nodelet" name="aruco_detect_01" args="load aruco_pose/aruco_detect nodelet_manager_01">
+        <remap from="image_raw" to="imgpub_01/image_raw"/>
+        <remap from="camera_info" to="imgpub_01/camera_info"/>
+        <param name="length" value="0.33"/>
+        <param name="cornerRefinementMethod" value="$(arg corner_method)"/>
+    </node>
+
+    <node pkg="nodelet" type="nodelet" name="nodelet_manager_02" args="manager"/>
+
+    <node pkg="nodelet" clear_params="true" type="nodelet" name="aruco_detect_02" args="load aruco_pose/aruco_detect nodelet_manager_02">
+        <remap from="image_raw" to="imgpub_02/image_raw"/>
+        <remap from="camera_info" to="imgpub_02/camera_info"/>
+        <param name="length" value="0.33"/>
+        <param name="cornerRefinementMethod" value="$(arg corner_method)"/>
+    </node>
+
+    <node pkg="nodelet" type="nodelet" name="nodelet_manager_03" args="manager"/>
+
+    <node pkg="nodelet" clear_params="true" type="nodelet" name="aruco_detect_03" args="load aruco_pose/aruco_detect nodelet_manager_03">
+        <remap from="image_raw" to="imgpub_03/image_raw"/>
+        <remap from="camera_info" to="imgpub_03/camera_info"/>
+        <param name="length" value="0.33"/>
+        <param name="cornerRefinementMethod" value="$(arg corner_method)"/>
+    </node>
+
+    <param name="test_module" value="$(find aruco_pose)/test/crash_opencv.py"/>
+    <test test-name="crash_opencv" pkg="ros_pytest" type="ros_pytest_runner"/>
+</launch>

aruco_pose/vendor/aruco/src/aruco.cpp

@@ -924,6 +924,8 @@ static void _refineCandidateLines(std::vector<Point>& nContours, std::vector<Poi
 	// calculate the line :: who passes through the grouped points
 	Point3f lines[4];
 	for(int i=0; i<4; i++){
+		// Don't try to "interpolate" single points
+		if (cntPts[i].size() < 2) return;
 		lines[i]=_interpolate2Dline(cntPts[i]);
 	}