MarkerDetection: added automatic marker's length estimation when depth image is provided (and when Aruco/MarkerLength is 0).

corelib/include/rtabmap/core/MarkerDetector.h

@@ -43,7 +43,7 @@ public:
 	MarkerDetector(const ParametersMap & parameters = ParametersMap());
 	virtual ~MarkerDetector();
 	void parseParameters(const ParametersMap & parameters);
-	std::map<int, Transform> detect(const cv::Mat & image, const CameraModel & model, cv::Mat * imageWithDetections = 0);
+	std::map<int, Transform> detect(const cv::Mat & image, const CameraModel & model, const cv::Mat & depth = cv::Mat(), float * estimatedMarkerLength = 0, cv::Mat * imageWithDetections = 0);
 
 private:
 #ifdef HAVE_OPENCV_ARUCO

corelib/include/rtabmap/core/Parameters.h

@@ -713,7 +713,7 @@ class RTABMAP_EXP Parameters
     RTABMAP_PARAM(Aruco, Dictionary,             int,   0,     "Dictionary to use: DICT_4X4_50=0, DICT_4X4_100=1, DICT_4X4_250=2, DICT_4X4_1000=3, DICT_5X5_50=4, DICT_5X5_100=5, DICT_5X5_250=6, DICT_5X5_1000=7, DICT_6X6_50=8, DICT_6X6_100=9, DICT_6X6_250=10, DICT_6X6_1000=11, DICT_7X7_50=12, DICT_7X7_100=13, DICT_7X7_250=14, DICT_7X7_1000=15, DICT_ARUCO_ORIGINAL = 16, DICT_APRILTAG_16h5=17, DICT_APRILTAG_25h9=18, DICT_APRILTAG_36h10=19, DICT_APRILTAG_36h11=20");
     RTABMAP_PARAM(Aruco, MarkerLength,           float, 0.1,   "The length (m) of the markers' side.");
     RTABMAP_PARAM(Aruco, VarianceLinear,         float, 0.001, "Linear variance to set on marker detections.");
-    RTABMAP_PARAM(Aruco, VarianceAngular,        float, 0.001, "Angular variance to set on marker detections. Set to >=9999 to use only position (xyz) constraint in graph optimization.");
+    RTABMAP_PARAM(Aruco, VarianceAngular,        float, 0.01,  "Angular variance to set on marker detections. Set to >=9999 to use only position (xyz) constraint in graph optimization.");
     RTABMAP_PARAM(Aruco, CornerRefinementMethod, int,   0,     "Corner refinement method (0: None, 1: Subpixel, 2:contour, 3: AprilTag 2). For OpenCV <3.3.0, this is \"doCornerRefinement\" parameter: set 0 for false and 1 for true.");
 
 public:

corelib/src/MarkerDetector.cpp

@@ -26,6 +26,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
 #include <rtabmap/core/MarkerDetector.h>
+#include <rtabmap/core/util2d.h>
 #include <rtabmap/utilite/ULogger.h>
 
 namespace rtabmap {
@@ -104,7 +105,7 @@ void MarkerDetector::parseParameters(const ParametersMap & parameters)
 #endif
 }
 
-std::map<int, Transform> MarkerDetector::detect(const cv::Mat & image, const CameraModel & model, cv::Mat * imageWithDetections)
+std::map<int, Transform> MarkerDetector::detect(const cv::Mat & image, const CameraModel & model, const cv::Mat & depth, float * markerLengthOut, cv::Mat * imageWithDetections)
 {
 	std::map<int, Transform> detections;
 
@@ -123,9 +124,69 @@ std::map<int, Transform> MarkerDetector::detect(const cv::Mat & image, const Cam
 	UDEBUG("Markers detected=%d rejected=%d", (int)ids.size(), (int)rejected.size());
 	if(ids.size() > 0)
 	{
-		cv::aruco::estimatePoseSingleMarkers(corners, markerLength_, model.K(), model.D(), rvecs, tvecs);
+		float rgbToDepthFactorX = 1.0f;
+		float rgbToDepthFactorY = 1.0f;
+		if(markerLength_ == 0)
+		{
+			if(depth.empty())
+			{
+				UERROR("Depth image is empty, please set %s parameter to non-null.", Parameters::kArucoMarkerLength().c_str());
+				return detections;
+			}
+			rgbToDepthFactorX = 1.0f/(model.imageWidth()>0?model.imageWidth()/depth.cols:1);
+			rgbToDepthFactorY = 1.0f/(model.imageHeight()>0?model.imageHeight()/depth.rows:1);
+		}
+
+		cv::aruco::estimatePoseSingleMarkers(corners, markerLength_==0?1.0f:markerLength_, model.K(), model.D(), rvecs, tvecs);
+		std::vector<float> scales;
 		for(size_t i=0; i<ids.size(); ++i)
 		{
+			if(markerLength_ == 0)
+			{
+				float d1 = util2d::getDepth(depth, corners[i][0].x*rgbToDepthFactorX, corners[i][0].y*rgbToDepthFactorY, true, 0.02f, true);
+				float d2 = util2d::getDepth(depth, corners[i][1].x*rgbToDepthFactorX, corners[i][1].y*rgbToDepthFactorY, true, 0.02f, true);
+				float d3 = util2d::getDepth(depth, corners[i][2].x*rgbToDepthFactorX, corners[i][2].y*rgbToDepthFactorY, true, 0.02f, true);
+				float d4 = util2d::getDepth(depth, corners[i][3].x*rgbToDepthFactorX, corners[i][3].y*rgbToDepthFactorY, true, 0.02f, true);
+				// Accept measurement only if all 4 depth values are valid and
+				// they are at the same depth (camera should be perpendicular to marker for
+				// best depth estimation)
+				if(d1>0 && d2>0 && d3>0 && d4>0)
+				{
+					if( fabs(d1-d2) < 0.01f &&
+						fabs(d1-d3) < 0.01f &&
+						fabs(d1-d4) < 0.01f)
+					{
+						float depth = (d1+d2+d3+d4)/4.0f;
+						scales.push_back(depth/tvecs[i].val[2]);
+						tvecs[i] *= scales.back();
+						UWARN("Automatic marker length estimation: id=%d depth=%fm length=%fm", ids[i], depth, scales.back());
+					}
+					else
+					{
+						UWARN("The four marker's corners should be "
+							  "perpendicular to camera to estimate correctly "
+							  "the marker's length. Errors: %f, %f, %f > 0.01m."
+							  "Parameter %s can be set to non-null to skip automatic "
+							  "marker length estimation. Detections are ignored.",
+							  fabs(d1-d2), fabs(d1-d3), fabs(d1-d4),
+								Parameters::kArucoMarkerLength().c_str());
+						detections.clear();
+						return detections;
+					}
+				}
+				else
+				{
+					UWARN("Some depth values (%f,%f,%f,%f) cannot be detected on the "
+						  "marker's corners, cannot initialize marker length. "
+						  "Parameter %s can be set to non-null to skip automatic "
+						  "marker length estimation. Detections are ignored.",
+						  d1,d2,d3,d4,
+						  Parameters::kArucoMarkerLength().c_str());
+					detections.clear();
+					return detections;
+				}
+			}
+
 			cv::Mat R;
 			cv::Rodrigues(rvecs[i], R);
 			Transform t(R.at<double>(0,0), R.at<double>(0,1), R.at<double>(0,2), tvecs[i].val[0],
@@ -136,6 +197,41 @@ std::map<int, Transform> MarkerDetector::detect(const cv::Mat & image, const Cam
 			detections.insert(std::make_pair(ids[i], pose));
 			UDEBUG("Marker %d detected at %s (%s)", ids[i], pose.prettyPrint().c_str(), t.prettyPrint().c_str());
 		}
+		if(markerLength_ == 0)
+		{
+			float sum = 0.0f;
+			float maxError = 0.0f;
+			for(size_t i=0; i<scales.size(); ++i)
+			{
+				if(i>0)
+				{
+					float error = fabs(scales[i]-scales[0]);
+					if(error > 0.001f)
+					{
+						UWARN("The marker's length detected between 2 of the "
+							  "markers doesn't match (%d->%fm vs %d->%fm)."
+							  "Parameter %s can be set to non-null to skip automatic "
+							  "marker length estimation. Detections are ignored.",
+							  ids[i], scales[i], ids[0], scales[0],
+							  Parameters::kArucoMarkerLength().c_str());
+						detections.clear();
+						return detections;
+					}
+					if(error > maxError)
+					{
+						maxError = error;
+					}
+				}
+				sum += scales[i];
+			}
+			markerLength_ = sum/float(scales.size());
+			UWARN("Final marker length estimated = %fm, max error=%fm (used for subsequent detections)", markerLength_, maxError);
+		}
+	}
+
+	if(markerLengthOut)
+	{
+		*markerLengthOut = markerLength_;
 	}
 
 	if(imageWithDetections)

corelib/src/Memory.cpp

@@ -4604,11 +4604,14 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 					static bool warned = false;
 					if(!warned)
 					{
-						UWARN("Detecting markers in multi-camera setup is not yet implemented, detecting only in first camera. This message is only printed once.");
+						UWARN("Detecting markers in multi-camera setup is not yet implemented, aborting marker detection. This message is only printed once.");
 					}
 					warned = true;
 				}
-				markers = _markerDetector->detect(data.imageRaw(), data.cameraModels()[0]);
+				else
+				{
+					markers = _markerDetector->detect(data.imageRaw(), data.cameraModels()[0], data.depthRaw());
+				}
 			}
 			else if(data.stereoCameraModel().isValidForProjection())
 			{

guilib/src/MainWindow.cpp

@@ -2656,7 +2656,7 @@ void MainWindow::updateMapCloud(
 		for(std::map<int, Transform>::const_iterator iter=posesIn.begin(); iter!=posesIn.end() && iter->first<0; ++iter)
 		{
 #if PCL_VERSION_COMPARE(>=, 1, 7, 2)
-			_cloudViewer->addOrUpdateCoordinate(uFormat("landmark_%d", -iter->first), iter->second, _preferencesDialog->getMarkerLength()/2.0, false);
+			_cloudViewer->addOrUpdateCoordinate(uFormat("landmark_%d", -iter->first), iter->second, _preferencesDialog->getMarkerLength()<=0?0.1:_preferencesDialog->getMarkerLength()/2.0, false);
 #endif
 			if(_preferencesDialog->isLabelsShown())
 			{
@@ -4361,11 +4361,11 @@ void MainWindow::drawLandmarks(cv::Mat & image, const Signature & signature)
 			t.rotationMatrix().convertTo(R, CV_64F);
 			cv::Rodrigues(R, rvec);
 
-			//cv::aruco::drawAxis(image, model.K(), model.D(), rvec, tvec, _preferencesDialog->getMarkerLength() * 0.5f);
+			//cv::aruco::drawAxis(image, model.K(), model.D(), rvec, tvec, _preferencesDialog->getMarkerLength()<=0?0.1:_preferencesDialog->getMarkerLength() * 0.5f);
 
 			// project axis points
 			std::vector< cv::Point3f > axisPoints;
-			float length = _preferencesDialog->getMarkerLength() * 0.5f;
+			float length = _preferencesDialog->getMarkerLength()<=0?0.1:_preferencesDialog->getMarkerLength() * 0.5f;
 			axisPoints.push_back(cv::Point3f(0, 0, 0));
 			axisPoints.push_back(cv::Point3f(length, 0, 0));
 			axisPoints.push_back(cv::Point3f(0, length, 0));

guilib/src/ui/preferencesDialog.ui

@@ -96,7 +96,7 @@
             <x>0</x>
             <y>0</y>
             <width>680</width>
-            <height>2986</height>
+            <height>3008</height>
            </rect>
           </property>
           <layout class="QVBoxLayout" name="verticalLayout_16">
@@ -11207,7 +11207,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                       <item row="1" column="1">
                        <widget class="QLabel" name="label_space2_7">
                         <property name="text">
-                         <string>Marker length. The length (m) of the markers' side.</string>
+                         <string>Marker length. The length (m) of the markers' side. 0 means automatic marker length estimation using the depth image (the camera should look at the marker perpendicularly for initialization).</string>
                         </property>
                         <property name="wordWrap">
                          <bool>true</bool>
@@ -11248,7 +11248,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                          <number>4</number>
                         </property>
                         <property name="minimum">
-                         <double>0.000100000000000</double>
+                         <double>0.000000000000000</double>
                         </property>
                         <property name="singleStep">
                          <double>0.010000000000000</double>