fixed some quirks (camera viewer + tag working)

CMakeLists.txt

@@ -1496,28 +1496,33 @@ ENDIF(PCL_COMPILE_OPTIONS)
 MESSAGE(STATUS "")
 MESSAGE(STATUS "Optional dependencies ('*' affects some default parameters) :")
 IF(OpenCV_FOUND)
+  IF(OPENCV_ARUCO_FOUND)
+    set(ARUCO_STR "YES")
+  ELSE()
+    set(ARUCO_STR "NO")
+  ENDIF()
   IF(OpenCV_VERSION_MAJOR EQUAL 2)
     IF(OPENCV_NONFREE_FOUND)
-      MESSAGE(STATUS " *With OpenCV 2 nonfree module (SIFT/SURF) = YES (License: Non commercial)")
+      MESSAGE(STATUS " *With OpenCV 2 nonfree module (SIFT/SURF) = YES, aruco = ${ARUCO_STR} (License: Non commercial)")
     ELSE()
-      MESSAGE(STATUS " *With OpenCV 2 nonfree module (SIFT/SURF) = NO (not found, License: BSD)")
+      MESSAGE(STATUS " *With OpenCV 2 nonfree module (SIFT/SURF) = NO, aruco = ${ARUCO_STR} (not found, License: BSD)")
     ENDIF()
   ELSE()
     IF(OPENCV_XFEATURES2D_FOUND)
       IF(NONFREE STREQUAL "//")
         IF((OpenCV_VERSION_MAJOR LESS 4) OR ((OpenCV_VERSION_MAJOR EQUAL 4) AND (OpenCV_VERSION_MINOR LESS 5)))
-          MESSAGE(STATUS " *With OpenCV ${OpenCV_VERSION} xfeatures2d = YES, nonfree = NO (License: BSD)")
+          MESSAGE(STATUS " *With OpenCV ${OpenCV_VERSION} xfeatures2d = YES, nonfree = NO, aruco = ${ARUCO_STR} (License: BSD)")
         ELSE()
-          MESSAGE(STATUS " *With OpenCV ${OpenCV_VERSION} xfeatures2d = YES, nonfree = NO (License: Apache 2)")
+          MESSAGE(STATUS " *With OpenCV ${OpenCV_VERSION} xfeatures2d = YES, nonfree = NO, aruco = ${ARUCO_STR} (License: Apache 2)")
         ENDIF()
       ELSE()
-        MESSAGE(STATUS " *With OpenCV ${OpenCV_VERSION} xfeatures2d = YES, nonfree = YES (License: Non commercial)")
+        MESSAGE(STATUS " *With OpenCV ${OpenCV_VERSION} xfeatures2d = YES, nonfree = YES, aruco = ${ARUCO_STR} (License: Non commercial)")
       ENDIF()
     ELSE()
       IF((OpenCV_VERSION_MAJOR LESS 4) OR ((OpenCV_VERSION_MAJOR EQUAL 4) AND (OpenCV_VERSION_MINOR LESS 5)))
-        MESSAGE(STATUS " *With OpenCV ${OpenCV_VERSION} xfeatures2d = NO, nonfree = NO (License: BSD)")
+        MESSAGE(STATUS " *With OpenCV ${OpenCV_VERSION} xfeatures2d = NO, nonfree = NO, aruco = ${ARUCO_STR} (License: BSD)")
       ELSE()
-        MESSAGE(STATUS " *With OpenCV ${OpenCV_VERSION} xfeatures2d = NO, nonfree = NO (License: Apache 2)")
+        MESSAGE(STATUS " *With OpenCV ${OpenCV_VERSION} xfeatures2d = NO, nonfree = NO, aruco = ${ARUCO_STR} (License: Apache 2)")
       ENDIF()
     ENDIF()
   ENDIF()

corelib/src/MarkerDetector.cpp

@@ -432,8 +432,8 @@ void MarkerDetector::parseParameters(const ParametersMap & parameters)
 		}
 		apriltagLibFamily_  = createAprilTagPredefinedDictionary(dictFamily);
 		UASSERT_MSG(apriltagLibFamily_ != NULL, uFormat("AprilTag library cannot be used with dictionary type %d", (int)dictionaryId_).c_str());
+		errno = 0;
 		apriltag_detector_add_family(((apriltag_detector_t*)apriltagLibDetector_), (apriltag_family_t*)apriltagLibFamily_);
-
 		if (errno == ENOMEM) {
 			UFATAL("Unable to add family to detector due to insufficient memory to allocate the tag-family decoder with the default maximum hamming value of 2. Try choosing an alternative tag family.");
 		}
@@ -454,6 +454,7 @@ void MarkerDetector::parseParameters(const ParametersMap & parameters)
 // deprecated
 std::map<int, Transform> MarkerDetector::detect(const cv::Mat & image, const CameraModel & model, const cv::Mat & depth, float * markerLengthOut, cv::Mat * imageWithDetections)
 {
+	UDEBUG("");
     std::map<int, Transform> detections;
     std::map<int, MarkerInfo> infos = detect(image, model, depth, std::map<int, float>(), imageWithDetections);
     
@@ -513,8 +514,8 @@ std::map<int, MarkerInfo> MarkerDetector::detect(const cv::Mat & image,
 		}
 		image_u8_t im = {grayImage.cols, grayImage.rows, (int)grayImage.step, grayImage.data};
 
+		errno = 0;
 		zarray_t *apriltagDetections = apriltag_detector_detect(((apriltag_detector_t*)apriltagLibDetector_), &im);
-
 		if (errno == EAGAIN) {
 			UERROR("Unable to create the %d threads requested.", ((apriltag_detector_t*)apriltagLibDetector_)->nthreads);
 			if(apriltagDetections)
@@ -844,7 +845,6 @@ std::map<int, MarkerInfo> MarkerDetector::detect(const cv::Mat & image,
 
 	if(imageWithDetections)
 	{
-#ifdef HAVE_OPENCV_ARUCO
 		if(image.channels()==1)
 		{
 			cv::cvtColor(image, *imageWithDetections, cv::COLOR_GRAY2BGR);
@@ -853,32 +853,38 @@ std::map<int, MarkerInfo> MarkerDetector::detect(const cv::Mat & image,
 		{
 			image.copyTo(*imageWithDetections);
 		}
+
 		if(!ids.empty())
 		{
+#ifdef HAVE_OPENCV_ARUCO
 			cv::aruco::drawDetectedMarkers(*imageWithDetections, corners, ids);
-
+#else
+			UWARN("RTAB-Map is not built with \"aruco\" module from OpenCV. Cannot draw markers on image.");
+#endif
 			for(unsigned int i = 0; i < ids.size(); i++)
 			{
                 std::map<int, MarkerInfo>::iterator iter = detections.find(ids[i]);
                 if(iter!=detections.end())
                 {
 					int cam = cams[i];
+					UASSERT(cam >=0 && cam < (int)models.size());
 					const CameraModel & model = models[cam];
-					cv::Mat subImage(*imageWithDetections, cv::Rect(subRGBWidth*i, 0, subRGBWidth, imageWithDetections->rows));
+					int roix = subRGBWidth*cam;
+					UASSERT(roix >=0 && roix <= imageWithDetections->cols - subRGBWidth);
+					cv::Mat subImage(*imageWithDetections, cv::Rect(roix, 0, subRGBWidth, imageWithDetections->rows));
 					cv::Vec3d rvec;
 					cv::Vec3d tvec(poses[i].x(), poses[i].y(), poses[i].z());
-					cv::Rodrigues(poses[i].rotationMatrix(), rvec);								
+					cv::Mat R;
+					poses[i].rotationMatrix().convertTo(R, CV_64F);
+					cv::Rodrigues(R, rvec);			
 #if CV_MAJOR_VERSION > 4 || (CV_MAJOR_VERSION == 4 && (CV_MINOR_VERSION >1 || (CV_MINOR_VERSION==1 && CV_PATCH_VERSION>=1)))
                     cv::drawFrameAxes(subImage, model.K(), model.D(), rvec, tvec, iter->second.length() * 0.5f);
-#else
+#elif defined(HAVE_OPENCV_ARUCO)
                     cv::aruco::drawAxis(subImage, model.K(), model.D(), rvec, tvec, iter->second.length() * 0.5f);
 #endif
                 }
 			}
 		}
-#else
-		UERROR("RTAB-Map is not built with \"aruco\" module from OpenCV. Cannot draw markers on image.");
-#endif
 	}
 
 	return detections;
@@ -890,6 +896,7 @@ std::map<int, MarkerInfo> MarkerDetector::detect(const cv::Mat & image,
                            const std::map<int, float> & markerLengths,
                            cv::Mat * imageWithDetections)
 {
+	UDEBUG("");
 	if(!image.empty() && image.cols != model.imageWidth())
 	{
 		UERROR("This method cannot handle multi-camera marker detection, use the other function version supporting it.");

guilib/include/rtabmap/gui/CameraViewer.h

@@ -77,6 +77,7 @@ private:
 	QElapsedTimer fpsTimer_;
 	double lastCapturePeriod_;
 	double previousCaptureStamp_;
+	std::map<int, float> _landmarksSize;
 };
 
 } /* namespace rtabmap */

guilib/src/CameraViewer.cpp

@@ -84,7 +84,7 @@ CameraViewer::CameraViewer(QWidget * parent, const ParametersMap & parameters) :
 	showScanCheckbox_->setChecked(true);
 
 	markerCheckbox_ = new QCheckBox("Detect markers", this);
-#ifdef HAVE_OPENCV_ARUCO
+#if defined(HAVE_OPENCV_ARUCO) || defined(RTABMAP_APRILTAG)
 	markerCheckbox_->setEnabled(true);
 	markerDetector_ = new MarkerDetector(parameters);
 #else
@@ -165,12 +165,22 @@ void CameraViewer::showImage(const rtabmap::SensorData & data)
 				}
 			}
 		}
+		else
+		{
+			_landmarksSize.clear();
+		}
 			
 		if(!models.empty() && models[0].isValidForProjection())
 		{
 			cv::Mat imageWithDetections;
-			detections = markerDetector_->detect(left, models, depthOrRight, std::map<int, float>(), &imageWithDetections);
+			detections = markerDetector_->detect(left, models, depthOrRight, _landmarksSize, &imageWithDetections);
 			imageView_->setImage(uCvMat2QImage(imageWithDetections));
+			for(std::map<int, MarkerInfo>::iterator iter=detections.begin(); iter!=detections.end(); ++iter)
+			{
+				if(iter->second.length() > 0.0f) {
+					_landmarksSize.insert(std::make_pair(iter->first, iter->second.length()));
+				}
+			}
 		}
 		else
 		{
@@ -236,7 +246,7 @@ void CameraViewer::showImage(const rtabmap::SensorData & data)
 #if PCL_VERSION_COMPARE(>=, 1, 7, 2)
 					cloudView_->addOrUpdateCoordinate(uFormat("landmark_%d", iter->first), iter->second.pose(), iter->second.length(), false);
 #endif
-					std::string num = uNumber2Str(iter->first);
+					std::string num = uFormat("%d (%.1f cm)", iter->first, iter->second.length()*100.0f);
 					cloudView_->addOrUpdateText(
 							std::string("landmark_str_") + num,
 							num,

guilib/src/PreferencesDialog.cpp

@@ -5640,6 +5640,17 @@ void PreferencesDialog::updateAvailableMarkerDictionaries()
 		{
 			_ui->MarkerDictionary->setCurrentIndex(Parameters::defaultMarkerDictionary());
 		}
+#else
+		if(_ui->MarkerDictionary->currentIndex() >=17 && _ui->MarkerDictionary->currentIndex() <= 20)
+		{
+			// If apriltag is selected, select apriltag refinement by default
+			_ui->OpenCVCornerRefinementMethod->setCurrentIndex(3);
+		}
+		else if(_ui->OpenCVCornerRefinementMethod->currentIndex() == 3)
+		{
+			// If not apriltag dictionary selected, reset refinement to default.
+			_ui->OpenCVCornerRefinementMethod->setCurrentIndex(Parameters::defaultMarkerOpenCVCornerRefinementMethod());
+		}
 #endif
 #if CV_MAJOR_VERSION < 4 || (CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION <8)
 		// disable aruco MPI dictionary	

guilib/src/ui/preferencesDialog.ui

@@ -15556,10 +15556,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                       </item>
                       <item row="3" column="0">
                        <widget class="QDoubleSpinBox" name="MarkerLength">
-                        <property name="toolTip">
-                         <string>If 0, the length is estimated only on the first marker detected, then re-used for all next detections (i.e., this assumes that markers have all the same length). 
-With &lt;0, the length is estimated once for each unique marker, then re-used for next detections with the same marker ID.</string>
-                        </property>
                         <property name="suffix">
                          <string> m</string>
                         </property>
@@ -15627,8 +15623,12 @@ With &lt;0, the length is estimated once for each unique marker, then re-used fo
                       </item>
                       <item row="3" column="1">
                        <widget class="QLabel" name="label_space2_7">
+                        <property name="toolTip">
+                         <string>If 0, the length is estimated only on the first marker detected, then re-used for all next detections (i.e., this assumes that markers have all the same length). 
+With &lt;0, the length is estimated once for each unique marker, then re-used for next detections with the same marker ID.</string>
+                        </property>
                         <property name="text">
-                         <string>Marker length. The length (m) of the markers' side. Value &amp;lt;=0 means automatic marker length estimation using the depth image (the camera should look at the marker perpendicularly for initialization). Mouse over the spinbox to see the difference between negative and 0.</string>
+                         <string>Marker length. The length (m) of the markers' side. Value &amp;lt;=0 means automatic marker length estimation using the depth image (the camera should look at the marker perpendicularly for initialization). Mouse over this label to see the difference between negative and 0.</string>
                         </property>
                         <property name="wordWrap">
                          <bool>true</bool>
@@ -16000,9 +16000,6 @@ With &lt;0, the length is estimated once for each unique marker, then re-used fo
                       </item>
                       <item row="4" column="0">
                        <widget class="QLineEdit" name="MarkerLengths">
-                        <property name="toolTip">
-                         <string>For example, to detect markers 12 and 14 with lengths of 8 and 15 cm respectively, and all markers between 30 and 40 with a length of 10 cm, set &quot;12 0.08|14 0.15|30:40 0.1&quot;</string>
-                        </property>
                         <property name="placeholderText">
                          <string/>
                         </property>
@@ -16010,8 +16007,11 @@ With &lt;0, the length is estimated once for each unique marker, then re-used fo
                       </item>
                       <item row="4" column="1">
                        <widget class="QLabel" name="label_space2_25">
+                        <property name="toolTip">
+                         <string>For example, to detect markers 12 and 14 with lengths of 8 and 15 cm respectively, and all markers between 30 and 40 with a length of 10 cm, set &quot;12 0.08|14 0.15|30:40 0.1&quot;</string>
+                        </property>
                         <property name="text">
-                         <string>List of markers to detect. Format is the marker's ID followed by its length (in meters), markers are separated by a vertical line (&quot;id1 length|id2 length&quot;). We can also define a range of markers with &quot;id1:id2 length&quot; (id2 included). If empty, all markers of the chosen dictionary can be detected and their length is set/estimated based on Marker Length. Mouse over text box for example.</string>
+                         <string>List of markers to detect. Format is the marker's ID followed by its length (in meters), markers are separated by a vertical line (&quot;id1 length|id2 length&quot;). We can also define a range of markers with &quot;id1:id2 length&quot; (id2 included). If empty, all markers of the chosen dictionary can be detected and their length is set/estimated based on Marker Length. Mouse over this label for example.</string>
                         </property>
                         <property name="wordWrap">
                          <bool>true</bool>