Adding support for AprilTag v3 library (#1702)

* Adding support for AprilTag v3 library

* New parameter: Marker/Strategy (default opencv-aruco as before). Optimized multicameras marker detection (do only once with stitched image)

* Exposed all AprilTag parameters. Sharing Marker/Dictionary between opencv and apriltag implementation. Added new Marker/Lengths parameter.

* Support AprilTAg library not built with aruco

* cleanup

* Added marker range support

* Make apriltag detection in same orientation than opencv

* fixed shenanigans when rendering markers with multi cameras

* Don't update odom cache (localization mode) when not moving (detecting loop/landmark or not)

* fixed some quirks (camera viewer + tag working)

* Setting quad decimate to 1 by default

* typo

* Implemented OptimizerG2O::loadGraph()

* added apriltag's aruco support info in the cmake config summary

* fixed warning

* fixed isam2 assert when using landmarks

* Fixed homography on multicam

* Adding marker detection time for convenience

* Added missing MIP 36h12 tag family in UI

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/include/rtabmap/gui/PreferencesDialog.h

@@ -364,6 +364,7 @@ private Q_SLOTS:
 	void updateStereoDisparityVisibility();
 	void updateFeatureMatchingVisibility();
 	void updateGlobalDescriptorVisibility();
+	void updateAvailableMarkerDictionaries();
 	void updateOdometryStackedIndex(int index);
 	void useOdomFeatures();
 	void changeWorkingDirectory();

guilib/src/AboutDialog.cpp

@@ -107,6 +107,13 @@ AboutDialog::AboutDialog(QWidget * parent) :
 	_ui->label_fastcv->setText("No");
 	_ui->label_fastcv_license->setEnabled(false);
 #endif
+#ifdef RTABMAP_APRILTAG
+	_ui->label_apriltag->setText("Yes");
+	_ui->label_apriltag_license->setEnabled(true);
+#else
+	_ui->label_apriltag->setText("No");
+	_ui->label_apriltag_license->setEnabled(false);
+#endif
 #ifdef RTABMAP_PDAL
 	_ui->label_pdal->setText("Yes");
 	_ui->label_pdal_license->setEnabled(true);

guilib/src/CameraViewer.cpp

@@ -36,6 +36,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/gui/CloudViewer.h>
 #include <rtabmap/utilite/UCv2Qt.h>
 #include <rtabmap/utilite/ULogger.h>
+#include <rtabmap/utilite/UTimer.h>
 #include <QtCore/QMetaType>
 #include <QHBoxLayout>
 #include <QVBoxLayout>
@@ -84,7 +85,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
@@ -151,6 +152,7 @@ void CameraViewer::showImage(const rtabmap::SensorData & data)
 
 	imageView_->setVisible(!left.empty() || !left.empty());
 	std::map<int, MarkerInfo> detections;
+	UTimer markerDetectionTime;
 	if(!left.empty())
 	{
 		std::vector<CameraModel> models;
@@ -165,12 +167,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
 		{
@@ -184,6 +196,10 @@ void CameraViewer::showImage(const rtabmap::SensorData & data)
 		sizes.append(QString(" Depth=%1x%2").arg(depthOrRight.cols).arg(depthOrRight.rows));
 	}
 	sizes.append(QString(" FPS capture=%1 render=%2").arg(lastCapturePeriod_>0.0?(int)round(1.0/lastCapturePeriod_):0).arg((int)round(1.0/fpsTimer_.restart()*1000)));
+	if(markerCheckbox_->isEnabled() && markerCheckbox_->isChecked())
+	{
+		sizes.append(QString(" Marker=%1ms").arg(int(markerDetectionTime.ticks()*1000)));
+	}
 	imageSizeLabel_->setText(sizes);
 
 	if(!depthOrRight.empty() &&
@@ -236,7 +252,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/MainWindow.cpp

@@ -5239,6 +5239,7 @@ void MainWindow::drawKeypoints(const std::multimap<int, cv::KeyPoint> & refWords
 
 void MainWindow::drawLandmarks(cv::Mat & image, const Signature & signature)
 {
+	UDEBUG("%ld landmarks", signature.getLandmarks().size());
 	for(std::map<int, Link>::const_iterator iter=signature.getLandmarks().begin(); iter!=signature.getLandmarks().end(); ++iter)
 	{
 		// Project in all cameras in which the landmark is visible
@@ -5295,6 +5296,9 @@ void MainWindow::drawLandmarks(cv::Mat & image, const Signature & signature)
 							{
 								imagePoints[j].x += i*model.imageWidth();
 							}
+							// Make sure the frame origin is visible
+							valid = imagePoints[0].x >= i*model.imageWidth() && imagePoints[0].x < (i+1)*model.imageWidth() &&
+									imagePoints[0].y >= 0 && imagePoints[0].y < image.rows;
 						}
 					}
 					if(valid)

guilib/src/PreferencesDialog.cpp

@@ -475,15 +475,14 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->checkBox_showOdomFrustums->setChecked(false);
 #endif
 
-	//if OpenCV < 3.4.2
-#if CV_MAJOR_VERSION < 3 || (CV_MAJOR_VERSION == 3 && (CV_MINOR_VERSION <4 || (CV_MINOR_VERSION ==4 && CV_SUBMINOR_VERSION<2)))
-	_ui->ArucoDictionary->setItemData(17, 0, Qt::UserRole - 1);
-	_ui->ArucoDictionary->setItemData(18, 0, Qt::UserRole - 1);
-	_ui->ArucoDictionary->setItemData(19, 0, Qt::UserRole - 1);
-	_ui->ArucoDictionary->setItemData(20, 0, Qt::UserRole - 1);
+#if !defined(HAVE_OPENCV_ARUCO) && !defined(RTABMAP_APRILTAG)
+	_ui->label_markerDetection->setText(_ui->label_markerDetection->text()+" This option works only if OpenCV has been built with \"aruco\" module and/or RTAB-Map has been built with AprilTag library support.");
 #endif
 #ifndef HAVE_OPENCV_ARUCO
-	_ui->label_markerDetection->setText(_ui->label_markerDetection->text()+" This option works only if OpenCV has been built with \"aruco\" module.");
+	_ui->MarkerStrategy->setItemData(0, 0, Qt::UserRole - 1);
+#endif
+#ifndef RTABMAP_APRILTAG
+	_ui->MarkerStrategy->setItemData(1, 0, Qt::UserRole - 1);
 #endif
 
 #ifndef RTABMAP_MADGWICK
@@ -1751,18 +1750,30 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->stereosgbm_mode->setObjectName(Parameters::kStereoSGBMMode().c_str());
 
 	// Aruco marker
-	_ui->ArucoDictionary->setObjectName(Parameters::kMarkerDictionary().c_str());
-	_ui->ArucoMarkerLength->setObjectName(Parameters::kMarkerLength().c_str());
-	_ui->ArucoMaxDepthError->setObjectName(Parameters::kMarkerMaxDepthError().c_str());
-	_ui->ArucoVarianceLinear->setObjectName(Parameters::kMarkerVarianceLinear().c_str());
-	_ui->ArucoVarianceAngular->setObjectName(Parameters::kMarkerVarianceAngular().c_str());
-	_ui->ArucoVarianceOrientationIgnored->setObjectName(Parameters::kMarkerVarianceOrientationIgnored().c_str());
-	_ui->ArucoMarkerRangeMin->setObjectName(Parameters::kMarkerMinRange().c_str());
-	_ui->ArucoMarkerRangeMax->setObjectName(Parameters::kMarkerMaxRange().c_str());
-	_ui->ArucoMarkerPriors->setObjectName(Parameters::kMarkerPriors().c_str());
-	_ui->ArucoPriorsVarianceLinear->setObjectName(Parameters::kMarkerPriorsVarianceLinear().c_str());
-	_ui->ArucoPriorsVarianceAngular->setObjectName(Parameters::kMarkerPriorsVarianceAngular().c_str());
-	_ui->ArucoCornerRefinementMethod->setObjectName(Parameters::kMarkerCornerRefinementMethod().c_str());
+	_ui->MarkerStrategy->setObjectName(Parameters::kMarkerStrategy().c_str());
+	connect(_ui->MarkerStrategy, SIGNAL(currentIndexChanged(int)), _ui->stackedWidget_markerStrategy, SLOT(setCurrentIndex(int)));
+	connect(_ui->MarkerStrategy, SIGNAL(currentIndexChanged(int)), this, SLOT(updateAvailableMarkerDictionaries()));
+	_ui->MarkerStrategy->setCurrentIndex(Parameters::defaultMarkerStrategy());
+	updateAvailableMarkerDictionaries();
+	_ui->MarkerDictionary->setObjectName(Parameters::kMarkerDictionary().c_str());
+	_ui->MarkerLength->setObjectName(Parameters::kMarkerLength().c_str());
+	_ui->MarkerLengths->setObjectName(Parameters::kMarkerLengths().c_str());
+	_ui->MarkerMaxDepthError->setObjectName(Parameters::kMarkerMaxDepthError().c_str());
+	_ui->MarkerVarianceLinear->setObjectName(Parameters::kMarkerVarianceLinear().c_str());
+	_ui->MarkerVarianceAngular->setObjectName(Parameters::kMarkerVarianceAngular().c_str());
+	_ui->MarkerVarianceOrientationIgnored->setObjectName(Parameters::kMarkerVarianceOrientationIgnored().c_str());
+	_ui->MarkerRangeMin->setObjectName(Parameters::kMarkerMinRange().c_str());
+	_ui->MarkerRangeMax->setObjectName(Parameters::kMarkerMaxRange().c_str());
+	_ui->MarkerPriors->setObjectName(Parameters::kMarkerPriors().c_str());
+	_ui->MarkerPriorsVarianceLinear->setObjectName(Parameters::kMarkerPriorsVarianceLinear().c_str());
+	_ui->MarkerPriorsVarianceAngular->setObjectName(Parameters::kMarkerPriorsVarianceAngular().c_str());
+	_ui->OpenCVCornerRefinementMethod->setObjectName(Parameters::kMarkerOpenCVCornerRefinementMethod().c_str());
+	_ui->apriltag_nthreads->setObjectName(Parameters::kMarkerAprilTagNThreads().c_str());
+	_ui->apriltag_quad_decimate->setObjectName(Parameters::kMarkerAprilTagQuadDecimate().c_str());
+	_ui->apriltag_quad_sigma->setObjectName(Parameters::kMarkerAprilTagQuadSigma().c_str());
+	_ui->apriltag_refine_edges->setObjectName(Parameters::kMarkerAprilTagRefineEdges().c_str());
+	_ui->apriltag_decode_sharpening->setObjectName(Parameters::kMarkerAprilTagDecodeSharpening().c_str());
+	_ui->apriltag_debug->setObjectName(Parameters::kMarkerAprilTagDebug().c_str());
 
 	// IMU filter
 	_ui->doubleSpinBox_imuFilterMadgwickGain->setObjectName(Parameters::kImuFilterMadgwickGain().c_str());
@@ -3938,15 +3949,36 @@ bool PreferencesDialog::validateForm()
 		_ui->checkbox_odomDisabled->setChecked(false);
 	}
 
-#if CV_MAJOR_VERSION < 3 || (CV_MAJOR_VERSION == 3 && (CV_MINOR_VERSION <4 || (CV_MINOR_VERSION ==4 && CV_SUBMINOR_VERSION<2)))
-	if(_ui->ArucoDictionary->currentIndex()>=17)
-	{
-		QMessageBox::warning(this, tr("Parameter warning"),
-				tr("ArUco dictionary: cannot select AprilTag dictionary, OpenCV version should be at least 3.4.2. Setting back to 0."));
-		_ui->ArucoDictionary->setCurrentIndex(0);
-	}
-#endif
 
+	if(_ui->MarkerStrategy->currentIndex() == 0)
+	{
+#if CV_MAJOR_VERSION < 3 || (CV_MAJOR_VERSION == 3 && (CV_MINOR_VERSION <4 || (CV_MINOR_VERSION ==4 && CV_SUBMINOR_VERSION<2)))
+		if(_ui->MarkerDictionary->currentIndex()>=17)
+		{
+			QMessageBox::warning(this, tr("Parameter warning"),
+					tr("opencv-aruco: cannot use the selected dictionary (%1), OpenCV version should be at least 3.4.2. Setting back to 0.").arg(_ui->MarkerDictionary->currentIndex()));
+			_ui->MarkerDictionary->setCurrentIndex(0);
+		}
+#elif CV_MAJOR_VERSION < 4 || (CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION <8)
+		if(_ui->MarkerDictionary->currentIndex()>=21)
+		{
+			QMessageBox::warning(this, tr("Parameter warning"),
+					tr("Opencv Strategy: cannot use selected dictionary (%1), OpenCV version should be at least 4.8.0. Setting back to 0.").arg(_ui->MarkerDictionary->currentIndex()));
+			_ui->MarkerDictionary->setCurrentIndex(0);
+		}
+#endif
+	}
+	else if(_ui->MarkerStrategy->currentIndex() == 1)
+	{
+#ifndef RTABMAP_APRILTAG_WITH_ARUCO
+		if(_ui->MarkerDictionary->currentIndex() < 17 || _ui->MarkerDictionary->currentIndex() == 21)
+		{
+			QMessageBox::warning(this, tr("Parameter warning"),
+					tr("AprilTag Strategy: cannot use selected dictionary (%1), AprilTag should be built with aruco support. Setting back to 17.").arg(_ui->MarkerDictionary->currentIndex()));
+			_ui->MarkerDictionary->setCurrentIndex(17);
+		}
+#endif
+	}
 	return true;
 }
 
@@ -5573,6 +5605,64 @@ void PreferencesDialog::updateGlobalDescriptorVisibility()
 	_ui->groupBox_pydescriptor->setVisible(_ui->comboBox_globalDescriptorExtractor->currentIndex() == 1);
 }
 
+void PreferencesDialog::updateAvailableMarkerDictionaries()
+{
+	Qt::ItemFlags enableFlags = Qt::ItemIsEnabled | Qt::ItemIsSelectable;
+	for(int i=0;i<_ui->MarkerDictionary->count();++i) {
+		_ui->MarkerDictionary->setItemData(i, QVariant(static_cast<int>(enableFlags)), Qt::UserRole - 1); 
+	}
+
+	if(_ui->MarkerStrategy->currentIndex() == 1) // AprilTag Strategy is selected
+	{
+		// ARUCO_ORIGINAL not available with AprilTag lib
+		_ui->MarkerDictionary->setItemData(16, 0, Qt::UserRole - 1);
+#ifndef RTABMAP_APRILTAG_WITH_ARUCO
+		// disable all aruco dictionaries
+		for(int i=0;i<17;++i) {
+			_ui->MarkerDictionary->setItemData(i, 0, Qt::UserRole - 1); 
+		}
+		_ui->MarkerDictionary->setItemData(21, 0, Qt::UserRole - 1);
+		if(_ui->MarkerDictionary->currentIndex() < 17 || _ui->MarkerDictionary->currentIndex() > 20)
+		{
+			_ui->MarkerDictionary->setCurrentIndex(20); // 36h11 by default
+		}
+#endif
+	}
+	else //if(_ui->MarkerStrategy->currentIndex() == 0) // OpenCV Strategy is selected
+	{
+		//if OpenCV < 3.4.2
+#if CV_MAJOR_VERSION < 3 || (CV_MAJOR_VERSION == 3 && (CV_MINOR_VERSION <4 || (CV_MINOR_VERSION ==4 && CV_SUBMINOR_VERSION<2)))
+		// disable all apriltag dictionaries
+		for(int i=17;i<21;++i) {
+			_ui->MarkerDictionary->setItemData(i, 0, Qt::UserRole - 1); 
+		}
+		if(_ui->MarkerDictionary->currentIndex() >=17 && _ui->MarkerDictionary->currentIndex() <= 20)
+		{
+			_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	
+		_ui->MarkerDictionary->setItemData(21, 0, Qt::UserRole - 1);
+		if(_ui->MarkerDictionary->currentIndex() == 21)
+		{
+			_ui->MarkerDictionary->setCurrentIndex(Parameters::defaultMarkerDictionary());
+		}
+#endif
+	}
+}
+
 void PreferencesDialog::updateOdometryStackedIndex(int index)
 {
 	if(index == 11) // FLOAM -> LOAM
@@ -6237,7 +6327,7 @@ bool PreferencesDialog::isMarkerDetection() const
 }
 double PreferencesDialog::getMarkerLength() const
 {
-	return _ui->ArucoMarkerLength->value();
+	return _ui->MarkerLength->value();
 }
 bool PreferencesDialog::isCloudMeshing() const
 {