Confirmed stereo calibration with fisheye works. Fix camera start/top progress dialog not drawn. Opencv >=4.7 using new opencv's ArucoDetector class.

CMakeLists.txt

@@ -246,7 +246,8 @@ set_property(CACHE RTABMAP_QT_VERSION PROPERTY STRINGS AUTO 4 5 6)
 # find_package(RTABMap) requests the same components this build used.
 SET(RTABMAP_OpenCV_COMPONENTS_5 core imgproc highgui stitching photo video videoio calib geometry)
 SET(RTABMAP_OpenCV_COMPONENTS_4 core imgproc highgui stitching photo video videoio calib3d)
-SET(RTABMAP_OpenCV_OPTIONAL_COMPONENTS aruco objdetect xfeatures2d nonfree gpu cudafeatures2d cudaoptflow cudaimgproc)
+SET(RTABMAP_OpenCV_OPTIONAL_COMPONENTS_5 objdetect xfeatures2d nonfree gpu cudafeatures2d cudaoptflow cudaimgproc)
+SET(RTABMAP_OpenCV_OPTIONAL_COMPONENTS_4 aruco objdetect xfeatures2d nonfree gpu cudafeatures2d cudaoptflow cudaimgproc)
 
 # Probe OpenCV without a version constraint first, then request the components
 # matching the detected major version. A version-constrained find that fails to
@@ -255,9 +256,9 @@ SET(RTABMAP_OpenCV_OPTIONAL_COMPONENTS aruco objdetect xfeatures2d nonfree gpu c
 # where CMAKE_FIND_ROOT_PATH restricts the search).
 FIND_PACKAGE(OpenCV REQUIRED QUIET COMPONENTS core)
 IF(OpenCV_VERSION_MAJOR GREATER 4)
-  FIND_PACKAGE(OpenCV REQUIRED QUIET COMPONENTS ${RTABMAP_OpenCV_COMPONENTS_5} OPTIONAL_COMPONENTS ${RTABMAP_OpenCV_OPTIONAL_COMPONENTS})
+  FIND_PACKAGE(OpenCV REQUIRED QUIET COMPONENTS ${RTABMAP_OpenCV_COMPONENTS_5} OPTIONAL_COMPONENTS ${RTABMAP_OpenCV_OPTIONAL_COMPONENTS_5})
 ELSE()
-  FIND_PACKAGE(OpenCV REQUIRED QUIET COMPONENTS ${RTABMAP_OpenCV_COMPONENTS_4} OPTIONAL_COMPONENTS ${RTABMAP_OpenCV_OPTIONAL_COMPONENTS})
+  FIND_PACKAGE(OpenCV REQUIRED QUIET COMPONENTS ${RTABMAP_OpenCV_COMPONENTS_4} OPTIONAL_COMPONENTS ${RTABMAP_OpenCV_OPTIONAL_COMPONENTS_4})
 ENDIF()
 
 IF(WITH_QT)
@@ -1339,10 +1340,13 @@ install(EXPORT rtabmapTargets
 ####
 IF(OpenCV_VERSION_MAJOR GREATER 4)
   SET(CONF_OPENCV_COMPONENTS ${RTABMAP_OpenCV_COMPONENTS_5})
+  SET(CONF_OPENCV_OPTIONAL_COMPONENTS ${RTABMAP_OpenCV_OPTIONAL_COMPONENTS_5})
 ELSE()
   SET(CONF_OPENCV_COMPONENTS ${RTABMAP_OpenCV_COMPONENTS_4})
+  SET(CONF_OPENCV_OPTIONAL_COMPONENTS ${RTABMAP_OpenCV_OPTIONAL_COMPONENTS_4})
 ENDIF()
 STRING(REPLACE ";" " " CONF_OPENCV_COMPONENTS "${CONF_OPENCV_COMPONENTS}")
+STRING(REPLACE ";" " " CONF_OPENCV_OPTIONAL_COMPONENTS "${CONF_OPENCV_OPTIONAL_COMPONENTS}")
 include(CMakePackageConfigHelpers)
 write_basic_package_version_file(
   "${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}ConfigVersion.cmake"
@@ -1509,7 +1513,8 @@ ENDIF(PCL_COMPILE_OPTIONS)
 MESSAGE(STATUS "")
 MESSAGE(STATUS "Optional dependencies ('*' affects some default parameters) :")
 IF(OpenCV_FOUND)
-  IF(OPENCV_ARUCO_FOUND)
+  IF((OpenCV_VERSION_MAJOR LESS 4 AND OPENCV_ARUCO_FOUND) OR 
+     (OpenCV_VERSION_MAJOR GREATER 4 AND OPENCV_OBJDETECT_FOUND))
     set(ARUCO_STR "YES")
   ELSE()
     set(ARUCO_STR "NO")

RTABMapConfig.cmake.in

@@ -1,7 +1,7 @@
 include(CMakeFindDependencyMacro)
 
 # Mandatory dependencies
-find_dependency(OpenCV COMPONENTS @CONF_OPENCV_COMPONENTS@ OPTIONAL_COMPONENTS aruco objdetect xfeatures2d nonfree gpu cudafeatures2d)
+find_dependency(OpenCV COMPONENTS @CONF_OPENCV_COMPONENTS@ OPTIONAL_COMPONENTS @CONF_OPENCV_OPTIONAL_COMPONENTS@)
 
 if(EXISTS "${CMAKE_CURRENT_LIST_DIR}/RTABMap_guiTargets.cmake")
     find_dependency(PCL 1.7 COMPONENTS common io kdtree search surface filters registration sample_consensus segmentation visualization)

corelib/include/rtabmap/core/MarkerDetector.h

@@ -32,7 +32,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/CameraModel.h>
 #include <opencv2/opencv_modules.hpp>
 
-#ifdef HAVE_OPENCV_ARUCO
+#if (CV_MAJOR_VERSION > 4 || (CV_MAJOR_VERSION==4 && CV_MINOR_VERSION >=7)) && defined(HAVE_OPENCV_OBJDETECT)
+#include <opencv2/objdetect.hpp>
+#elif defined(HAVE_OPENCV_ARUCO)
 #include <opencv2/aruco.hpp>
 #endif
 
@@ -97,8 +99,11 @@ private:
 	float maxRange_;
 	float minRange_;
 	int dictionaryId_;
-#ifdef HAVE_OPENCV_ARUCO
-	cv::Ptr<cv::aruco::DetectorParameters> detectorParams_;
+#if ((CV_MAJOR_VERSION > 4 || (CV_MAJOR_VERSION==4 && CV_MINOR_VERSION >=7)) && defined(HAVE_OPENCV_OBJDETECT)) || defined(HAVE_OPENCV_ARUCO)
+#if CV_MAJOR_VERSION > 4 || (CV_MAJOR_VERSION==4 && CV_MINOR_VERSION >=7)
+  cv::Ptr<cv::aruco::ArucoDetector> arucoDetector_;
+#endif
+  cv::Ptr<cv::aruco::DetectorParameters> detectorParams_;
 	cv::Ptr<cv::aruco::Dictionary> dictionary_;
 #endif
   void * apriltagLibDetector_;

corelib/src/MarkerDetector.cpp

@@ -76,7 +76,7 @@ extern "C" {
 #include "apriltag/tag36h11.h"
 }
 
-#ifndef HAVE_OPENCV_ARUCO
+#if !((CV_MAJOR_VERSION > 4 || (CV_MAJOR_VERSION==4 && CV_MINOR_VERSION >=7)) && defined(HAVE_OPENCV_OBJDETECT)) && !defined(HAVE_OPENCV_ARUCO)
 // To match opencv::aruco module, add opencv::aruco dictionary enum
 namespace cv{
 namespace aruco {
@@ -204,7 +204,7 @@ MarkerDetector::MarkerDetector(const ParametersMap & parameters) :
 	apriltagLibDetector_(NULL),
 	apriltagLibFamily_(NULL)
 {
-#ifdef HAVE_OPENCV_ARUCO
+#if ((CV_MAJOR_VERSION > 4 || (CV_MAJOR_VERSION==4 && CV_MINOR_VERSION >=7)) && defined(HAVE_OPENCV_OBJDETECT)) || defined(HAVE_OPENCV_ARUCO)
 #if CV_MAJOR_VERSION > 4 || (CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION >= 7)
     detectorParams_.reset(new cv::aruco::DetectorParameters());
 #elif CV_MAJOR_VERSION > 3 || (CV_MAJOR_VERSION == 3 && CV_MINOR_VERSION >=2)
@@ -314,7 +314,7 @@ void MarkerDetector::parseParameters(const ParametersMap & parameters)
 		}
 	}
 
-#ifdef HAVE_OPENCV_ARUCO
+#if ((CV_MAJOR_VERSION > 4 || (CV_MAJOR_VERSION==4 && CV_MINOR_VERSION >=7)) && defined(HAVE_OPENCV_OBJDETECT)) || defined(HAVE_OPENCV_ARUCO)
 	detectorParams_->adaptiveThreshWinSizeMin = 3;
 	detectorParams_->adaptiveThreshWinSizeMax = 23;
 	detectorParams_->adaptiveThreshWinSizeStep = 10;
@@ -368,6 +368,7 @@ void MarkerDetector::parseParameters(const ParametersMap & parameters)
 		dictionaryId_ = Parameters::defaultMarkerDictionary();
 	}
 #endif
+
 #if CV_MAJOR_VERSION > 4 || (CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION >= 7)
     dictionary_.reset(new cv::aruco::Dictionary());
     *dictionary_ = cv::aruco::getPredefinedDictionary(cv::aruco::PredefinedDictionaryType(dictionaryId_));
@@ -377,6 +378,11 @@ void MarkerDetector::parseParameters(const ParametersMap & parameters)
 	dictionary_.reset(new cv::aruco::Dictionary());
 	*dictionary_ = cv::aruco::getPredefinedDictionary(cv::aruco::PredefinedDictionaryType(dictionaryId_));
 #endif
+
+#if CV_MAJOR_VERSION > 4 || (CV_MAJOR_VERSION==4 && CV_MINOR_VERSION >=7)
+	arucoDetector_.reset(new cv::aruco::ArucoDetector(*dictionary_, *detectorParams_));
+#endif
+
 #else
 	if(strategy_ == 0)
 	{
@@ -445,7 +451,7 @@ void MarkerDetector::parseParameters(const ParametersMap & parameters)
 #else
 	if(strategy_ == 1)
 	{
-#ifdef HAVE_OPENCV_ARUCO
+#if ((CV_MAJOR_VERSION > 4 || (CV_MAJOR_VERSION==4 && CV_MINOR_VERSION >=7)) && defined(HAVE_OPENCV_OBJDETECT)) || defined(HAVE_OPENCV_ARUCO)
 		UERROR("RTAB-Map is not built with apriltag library! Fallback to OpenCV (%s=0).", Parameters::kMarkerStrategy().c_str());
 		strategy_ = kStrategyOpencv;
 #else
@@ -475,6 +481,34 @@ std::map<int, Transform> MarkerDetector::detect(const cv::Mat & image, const Cam
     return detections;
 }
 
+#if (((CV_MAJOR_VERSION > 4 || (CV_MAJOR_VERSION==4 && CV_MINOR_VERSION >=7)) && defined(HAVE_OPENCV_OBJDETECT)) || defined(HAVE_OPENCV_ARUCO)) && (CV_MAJOR_VERSION > 4 || (CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION >= 7))
+// Drop-in replacement for cv::aruco::estimatePoseSingleMarkers(), which was deprecated
+// in OpenCV 4.7 and removed in OpenCV 5. It reproduces the legacy default behavior (marker
+// object points ordered as ARUCO_CCW_CENTER + SOLVEPNP_ITERATIVE) using cv::solvePnP() per
+// marker. For OpenCV < 4.7 the native cv::aruco::estimatePoseSingleMarkers() is still used.
+static void estimatePoseSingleMarkers(
+		const std::vector<std::vector<cv::Point2f> > & corners,
+		float markerLength,
+		const cv::Mat & cameraMatrix,
+		const cv::Mat & distCoeffs,
+		std::vector<cv::Vec3d> & rvecs,
+		std::vector<cv::Vec3d> & tvecs)
+{
+	cv::Mat objPoints(4, 1, CV_32FC3);
+	objPoints.ptr<cv::Vec3f>(0)[0] = cv::Vec3f(-markerLength/2.f,  markerLength/2.f, 0);
+	objPoints.ptr<cv::Vec3f>(0)[1] = cv::Vec3f( markerLength/2.f,  markerLength/2.f, 0);
+	objPoints.ptr<cv::Vec3f>(0)[2] = cv::Vec3f( markerLength/2.f, -markerLength/2.f, 0);
+	objPoints.ptr<cv::Vec3f>(0)[3] = cv::Vec3f(-markerLength/2.f, -markerLength/2.f, 0);
+
+	rvecs.resize(corners.size());
+	tvecs.resize(corners.size());
+	for(size_t i=0; i<corners.size(); ++i)
+	{
+		cv::solvePnP(objPoints, corners[i], cameraMatrix, distCoeffs, rvecs[i], tvecs[i]);
+	}
+}
+#endif
+
 std::map<int, MarkerInfo> MarkerDetector::detect(const cv::Mat & image,
                            const std::vector<CameraModel> & models,
                            const cv::Mat & depth,
@@ -486,20 +520,27 @@ std::map<int, MarkerInfo> MarkerDetector::detect(const cv::Mat & image,
 	UASSERT(int((depth.cols/models.size())*models.size()) == depth.cols);
 	int subRGBWidth = image.cols/models.size();
 
+	// Only a real depth map (CV_16UC1/CV_32FC1) can be used for marker length estimation.
+	// Ignore anything else (e.g. a stereo right image passed by mistake) instead of crashing
+	// later in util2d::getDepth().
+	cv::Mat depthMap = depth;
+	if(!depthMap.empty() && depthMap.type()!=CV_16UC1 && depthMap.type()!=CV_32FC1)
+	{
+		UWARN("Marker detection: ignoring depth image with unsupported type=%d (expected CV_16UC1 or CV_32FC1).", depthMap.type());
+		depthMap = cv::Mat();
+	}
+
 	float rgbToDepthFactorX = 1.0f;
 	float rgbToDepthFactorY = 1.0f;
-	if(!depth.empty())
+	if(!depthMap.empty())
 	{
-		rgbToDepthFactorX = float(depth.cols) / float(image.cols);
-		rgbToDepthFactorY = float(depth.rows) / float(image.rows);
+		rgbToDepthFactorX = float(depthMap.cols) / float(image.cols);
+		rgbToDepthFactorY = float(depthMap.rows) / float(image.rows);
 	}
 	else if(markerLength_ == 0)
 	{
-		if(depth.empty())
-		{
-			UERROR("Depth image is empty, please set %s parameter to non-null.", Parameters::kMarkerLength().c_str());
-			return std::map<int, MarkerInfo>();
-		}
+		UERROR("Depth image is empty, please set %s parameter to non-null.", Parameters::kMarkerLength().c_str());
+		return std::map<int, MarkerInfo>();
 	}
 
 	std::vector< int > ids;
@@ -636,8 +677,10 @@ std::map<int, MarkerInfo> MarkerDetector::detect(const cv::Mat & image,
 	{
 		std::vector< int > cvIds;
 		std::vector< std::vector< cv::Point2f > > cvCorners, cvRejected;
-#ifdef HAVE_OPENCV_ARUCO
-#if CV_MAJOR_VERSION > 3 || (CV_MAJOR_VERSION == 3 && CV_MINOR_VERSION >=2)
+#if ((CV_MAJOR_VERSION > 4 || (CV_MAJOR_VERSION==4 && CV_MINOR_VERSION >=7)) && defined(HAVE_OPENCV_OBJDETECT)) || defined(HAVE_OPENCV_ARUCO)
+#if CV_MAJOR_VERSION > 4 || (CV_MAJOR_VERSION==4 && CV_MINOR_VERSION >=7)
+        arucoDetector_->detectMarkers(image, cvCorners, cvIds, cvRejected);
+#elif CV_MAJOR_VERSION > 3 || (CV_MAJOR_VERSION == 3 && CV_MINOR_VERSION >=2)
 		cv::aruco::detectMarkers(image, dictionary_, cvCorners, cvIds, detectorParams_, cvRejected);
 #else
 		cv::aruco::detectMarkers(image, *dictionary_, cvCorners, cvIds, *detectorParams_, cvRejected);
@@ -690,9 +733,15 @@ std::map<int, MarkerInfo> MarkerDetector::detect(const cv::Mat & image,
 
 		for(size_t cam=0; cam < cvCornersPerCam.size(); ++cam)
 		{
-			std::vector< cv::Vec3d > rvecs, tvecs;
 			const CameraModel & model = models[cam];
+
+			std::vector< cv::Vec3d > rvecs, tvecs;
+#if CV_MAJOR_VERSION > 4 || (CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION >= 7)
+			estimatePoseSingleMarkers(cvCornersPerCam[cam], 1.0f, model.K(), model.D(), rvecs, tvecs);
+#else
 			cv::aruco::estimatePoseSingleMarkers(cvCornersPerCam[cam], 1.0f, model.K(), model.D(), rvecs, tvecs);
+#endif
+
 			float offsetX = cam*subRGBWidth;
 			for(size_t i=0; i<cvIdsPerCam[cam].size(); ++i)
 			{
@@ -729,13 +778,13 @@ std::map<int, MarkerInfo> MarkerDetector::detect(const cv::Mat & image,
 	{
 		float length = 0.0f;
 		std::map<int, float>::const_iterator findIter = extraMarkerLengths.find(ids[i]);
-		if(markerLengths_.empty() && !depth.empty() && (markerLength_ == 0 || (markerLength_<0 && findIter==extraMarkerLengths.end())))
+		if(markerLengths_.empty() && !depthMap.empty() && (markerLength_ == 0 || (markerLength_<0 && findIter==extraMarkerLengths.end())))
 		{
-			float d = util2d::getDepth(depth, (corners[i][0].x + (corners[i][2].x-corners[i][0].x)/2.0f)*rgbToDepthFactorX, (corners[i][0].y + (corners[i][2].y-corners[i][0].y)/2.0f)*rgbToDepthFactorY, true, 0.02f, true);
-			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);
+			float d = util2d::getDepth(depthMap, (corners[i][0].x + (corners[i][2].x-corners[i][0].x)/2.0f)*rgbToDepthFactorX, (corners[i][0].y + (corners[i][2].y-corners[i][0].y)/2.0f)*rgbToDepthFactorY, true, 0.02f, true);
+			float d1 = util2d::getDepth(depthMap, corners[i][0].x*rgbToDepthFactorX, corners[i][0].y*rgbToDepthFactorY, true, 0.02f, true);
+			float d2 = util2d::getDepth(depthMap, corners[i][1].x*rgbToDepthFactorX, corners[i][1].y*rgbToDepthFactorY, true, 0.02f, true);
+			float d3 = util2d::getDepth(depthMap, corners[i][2].x*rgbToDepthFactorX, corners[i][2].y*rgbToDepthFactorY, true, 0.02f, true);
+			float d4 = util2d::getDepth(depthMap, 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)
@@ -891,7 +940,7 @@ std::map<int, MarkerInfo> MarkerDetector::detect(const cv::Mat & image,
 
 		if(!ids.empty())
 		{
-#ifdef HAVE_OPENCV_ARUCO
+#if ((CV_MAJOR_VERSION > 4 || (CV_MAJOR_VERSION==4 && CV_MINOR_VERSION >=7)) && defined(HAVE_OPENCV_OBJDETECT)) || defined(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.");

guilib/src/CalibrationDialog.cpp

@@ -280,18 +280,23 @@ void CalibrationDialog::generateBoard()
 	if(ui_->comboBox_board_type->currentIndex() >= 1 )
 	{
 		try {
+			const int marginInPixels = squareSizeInPixels/4;
+			cv::Size size(
+				squareSizeInPixels*ui_->spinBox_boardWidth->value()  + 2*marginInPixels,
+				squareSizeInPixels*ui_->spinBox_boardHeight->value() + 2*marginInPixels);
+			UINFO("Creating board image of %dx%d pixels (%dx%d squares)",
+				size.width, size.height,
+				ui_->spinBox_boardWidth->value(), ui_->spinBox_boardHeight->value());
 #if CV_MAJOR_VERSION > 4 || (CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION >= 7)
 			charucoBoard_->generateImage(
-				cv::Size(squareSizeInPixels*ui_->spinBox_boardWidth->value(),
-						squareSizeInPixels*ui_->spinBox_boardHeight->value()), 
+				size, 
 				image, 
-				squareSizeInPixels/4, 1);
+				marginInPixels, 1);
 #else
 			charucoBoard_->draw(
-				cv::Size(squareSizeInPixels*ui_->spinBox_boardWidth->value(),
-						squareSizeInPixels*ui_->spinBox_boardHeight->value()), 
+				size, 
 				image, 
-				squareSizeInPixels/4, 1);
+				marginInPixels, 1);
 #endif
 
 			int arucoDict = ui_->comboBox_marker_dictionary->currentIndex();
@@ -301,7 +306,7 @@ void CalibrationDialog::generateBoard()
 		}
 		catch(const cv::Exception & e)
 		{
-			UERROR("%f", e.what());
+			UERROR("%s", e.what());
 			QMessageBox::critical(this, tr("Generating Board"),
 				tr("Cannot generate the board. Make sure the dictionary "
 				   "selected is big enough for the board size. Error:\"%1\"").arg(e.what()));
@@ -1405,26 +1410,77 @@ void CalibrationDialog::calibrate()
 
 		if(fishEye)
 		{
-			try
+			// cv::fisheye::calibrate() with CALIB_CHECK_COND throws as soon as a single
+			// view is ill-conditioned (e.g. too few / poorly spread ChArUco corners),
+			// aborting the whole calibration. Auto-prune the offending view (its index is
+			// reported in the exception message) and retry until it succeeds, keeping the
+			// CHECK_COND safety without discarding every good view.
+			const int minFisheyeViews = COUNT_MIN/2;
+			bool calibrated = false;
+			while(!calibrated)
 			{
-				rms = cv::fisheye::calibrate(
-					objectPoints_[id],
-					imagePoints_[id],
-					imageSize_[id],
-					K,
-					D,
-					rvecs,
-					tvecs,
-					cv::fisheye::CALIB_RECOMPUTE_EXTRINSIC |
-					cv::fisheye::CALIB_CHECK_COND |
-					cv::fisheye::CALIB_FIX_SKEW);
-			}
-			catch(const cv::Exception & e)
-			{
-				UERROR("Error: %s (try restarting the calibration)", e.what());
-				QMessageBox::warning(this, tr("Calibration failed!"), tr("Error: %1 (try restarting the calibration)").arg(e.what()));
-				processingData_ = false;
-				return;
+				try
+				{
+					rms = cv::fisheye::calibrate(
+						objectPoints_[id],
+						imagePoints_[id],
+						imageSize_[id],
+						K,
+						D,
+						rvecs,
+						tvecs,
+						cv::fisheye::CALIB_RECOMPUTE_EXTRINSIC |
+						cv::fisheye::CALIB_CHECK_COND |
+						cv::fisheye::CALIB_FIX_SKEW);
+					calibrated = true;
+				}
+				catch(const cv::Exception & e)
+				{
+					// Parse the ill-conditioned view index, e.g.
+					// "CALIB_CHECK_COND - Ill-conditioned matrix for input array 43"
+					int badIndex = -1;
+					const QString token = "input array ";
+					QString msg = e.what();
+					int tokenPos = msg.indexOf(token);
+					if(tokenPos >= 0)
+					{
+						bool ok = false;
+						int v = msg.mid(tokenPos + token.length()).section(' ', 0, 0).toInt(&ok);
+						if(ok)
+						{
+							badIndex = v;
+						}
+					}
+
+					if(badIndex >= 0 && badIndex < (int)objectPoints_[id].size() &&
+						(int)objectPoints_[id].size() > minFisheyeViews)
+					{
+						int removedImageId = badIndex < (int)imageIds_[id].size() ? imageIds_[id][badIndex] : -1;
+						UWARN("Fisheye calibration: view %d (image %d) is ill-conditioned, "
+							  "removing it and retrying (%d views left).",
+							  badIndex, removedImageId, (int)objectPoints_[id].size()-1);
+						logStream << "Fisheye calibration: removed ill-conditioned view " << badIndex
+								  << " (image " << removedImageId << "), "
+								  << (int)objectPoints_[id].size()-1 << " views left" << ENDL;
+
+						// Keep objectPoints_/imagePoints_/imageIds_ aligned: the per-view
+						// reprojection loop below indexes them together with rvecs/tvecs.
+						objectPoints_[id].erase(objectPoints_[id].begin()+badIndex);
+						imagePoints_[id].erase(imagePoints_[id].begin()+badIndex);
+						if(badIndex < (int)imageIds_[id].size())
+						{
+							imageIds_[id].erase(imageIds_[id].begin()+badIndex);
+						}
+						// loop and retry with the pruned set
+					}
+					else
+					{
+						UERROR("Error: %s (try restarting the calibration)", e.what());
+						QMessageBox::warning(this, tr("Calibration failed!"), tr("Error: %1 (try restarting the calibration)").arg(e.what()));
+						processingData_ = false;
+						return;
+					}
+				}
 			}
 		}
 		else
@@ -1580,9 +1636,14 @@ void CalibrationDialog::calibrate()
 			cv::Mat P = stereoModel_.right().P().clone();
 			P.at<double>(0,3) = -P.at<double>(0,0)*ui_->doubleSpinBox_stereoBaseline->value();
 			double scale = ui_->doubleSpinBox_stereoBaseline->value() / stereoModel_.baseline();
-			UWARN("Scale %f (setting square size from %f to %f)", scale, ui_->doubleSpinBox_squareSize->value(), ui_->doubleSpinBox_squareSize->value()*scale);
-			logStream << "Baseline rescaled from " << stereoModel_.baseline() << " to " << ui_->doubleSpinBox_stereoBaseline->value() << " scale=" << scale << ENDL;
-			ui_->doubleSpinBox_squareSize->setValue(ui_->doubleSpinBox_squareSize->value()*scale);
+			UWARN("Scale %f applied to stereo baseline (computed %f m -> expected %f m). "
+				  "If the mismatch is caused by the measured square size, it would be %f m instead of %f m.",
+					scale, stereoModel_.baseline(), ui_->doubleSpinBox_stereoBaseline->value(),
+					ui_->doubleSpinBox_squareSize->value()*scale, ui_->doubleSpinBox_squareSize->value());
+			logStream << "Baseline rescaled from " << stereoModel_.baseline() << " to " << ui_->doubleSpinBox_stereoBaseline->value()
+					  << " scale=" << scale << " (implied square size " << ui_->doubleSpinBox_squareSize->value()*scale
+					  << " m instead of " << ui_->doubleSpinBox_squareSize->value() << " m)" << ENDL;
+			UASSERT(!stereoModel_.T().empty());
 			stereoModel_ = StereoCameraModel(
 					stereoModel_.name(),
 					stereoModel_.left().imageSize(),stereoModel_.left().K_raw(), stereoModel_.left().D_raw(), stereoModel_.left().R(), stereoModel_.left().P(),
@@ -1712,26 +1773,58 @@ StereoCameraModel CalibrationDialog::stereoCalibration(const CameraModel & left,
 		cv::Vec4d D_right(right.D_raw().at<double>(0,0), right.D_raw().at<double>(0,1), right.D_raw().at<double>(0,4), right.D_raw().at<double>(0,5));
 
 		UASSERT(stereoImagePoints_[0].size() == stereoImagePoints_[1].size());
+		UASSERT(stereoObjectPoints_.size() == stereoImagePoints_[0].size());
+		// cv::fisheye::stereoCalibrate() reads the number of points from the first view and
+		// lays out its Jacobian assuming EVERY view has that same count (fisheye.cpp
+		// "reshape(1, n_points*2)"). ChArUco detects a variable number of corners per view,
+		// so we make the counts uniform by evenly subsampling every view down to the common
+		// minimum count (kept spatially spread, not just the first N).
+		size_t minPoints = stereoImagePoints_[0][0].size();
+		for(unsigned int i =0; i<stereoImagePoints_[0].size(); ++i)
+		{
+			minPoints = std::min(minPoints, stereoImagePoints_[0][i].size());
+		}
+		std::vector<std::vector<cv::Point3d> > objectPoints(stereoObjectPoints_.size());
 		std::vector<std::vector<cv::Point2d> > leftPoints(stereoImagePoints_[0].size());
 		std::vector<std::vector<cv::Point2d> > rightPoints(stereoImagePoints_[1].size());
+		bool subsampled = false;
 		for(unsigned int i =0; i<stereoImagePoints_[0].size(); ++i)
 		{
 			UASSERT(stereoImagePoints_[0][i].size() == stereoImagePoints_[1][i].size());
-			leftPoints[i].resize(stereoImagePoints_[0][i].size());
-			rightPoints[i].resize(stereoImagePoints_[1][i].size());
-			for(unsigned int j =0; j<stereoImagePoints_[0][i].size(); ++j)
+			UASSERT(stereoObjectPoints_[i].size() == stereoImagePoints_[0][i].size());
+			const size_t n = stereoImagePoints_[0][i].size();
+			if(n != minPoints)
 			{
-				leftPoints[i][j].x = stereoImagePoints_[0][i][j].x;
-				leftPoints[i][j].y = stereoImagePoints_[0][i][j].y;
-				rightPoints[i][j].x = stereoImagePoints_[1][i][j].x;
-				rightPoints[i][j].y = stereoImagePoints_[1][i][j].y;
+				subsampled = true;
 			}
+			objectPoints[i].resize(minPoints);
+			leftPoints[i].resize(minPoints);
+			rightPoints[i].resize(minPoints);
+			for(size_t k =0; k<minPoints; ++k)
+			{
+				// evenly spread the kept indices over [0, n-1]
+				size_t j = minPoints>1 ? (size_t)((k*(n-1))/(minPoints-1)) : 0;
+				objectPoints[i][k].x = stereoObjectPoints_[i][j].x;
+				objectPoints[i][k].y = stereoObjectPoints_[i][j].y;
+				objectPoints[i][k].z = stereoObjectPoints_[i][j].z;
+				leftPoints[i][k].x = stereoImagePoints_[0][i][j].x;
+				leftPoints[i][k].y = stereoImagePoints_[0][i][j].y;
+				rightPoints[i][k].x = stereoImagePoints_[1][i][j].x;
+				rightPoints[i][k].y = stereoImagePoints_[1][i][j].y;
+			}
+		}
+		if(subsampled)
+		{
+			UWARN("Fisheye stereo calibration requires the same number of points in every "
+				  "view; sub-sampled all %d views to the common minimum of %d points.",
+				  (int)stereoImagePoints_[0].size(), (int)minPoints);
+			if(logStream) (*logStream) << "Fisheye stereo: sub-sampled all views to " << (int)minPoints << " points" << ENDL;
 		}
 
 		try
 		{
 			rms = cv::fisheye::stereoCalibrate(
-					stereoObjectPoints_,
+					objectPoints,
 					leftPoints,
 					rightPoints,
 					left.K_raw(), D_left, right.K_raw(), D_right,
@@ -1743,12 +1836,21 @@ StereoCameraModel CalibrationDialog::stereoCalibration(const CameraModel & left,
 		catch(const cv::Exception & e)
 		{
 			UERROR("Error: %s (try restarting the calibration)", e.what());
+			QMessageBox::warning(const_cast<CalibrationDialog*>(this), tr("Calibration failed!"), tr("Error: %1 (try restarting the calibration)").arg(e.what()));
 			return output;
 		}
 
 		std::cout << "R = " << R << std::endl;
 		std::cout << "T = " << Tvec << std::endl;
 
+		// cv::fisheye::stereoCalibrate() returns the translation as a Vec3d (Tvec) and does
+		// not fill the cv::Mat T; populate it here so the returned model always carries a
+		// valid 3x1 extrinsic translation (used e.g. by the baseline rescaling below).
+		T = cv::Mat(3, 1, CV_64FC1);
+		T.at<double>(0,0) = Tvec[0];
+		T.at<double>(1,0) = Tvec[1];
+		T.at<double>(2,0) = Tvec[2];
+
 		if(imageSize_[0] == imageSize_[1] && !ignoreStereoRectification)
 		{
 			UINFO("Compute stereo rectification");
@@ -1797,11 +1899,6 @@ StereoCameraModel CalibrationDialog::stereoCalibration(const CameraModel & left,
 			std::cout << "P1n = " << P1 << std::endl;
 			std::cout << "P2n = " << P2 << std::endl;
 
-
-			cv::Mat T(3,1,CV_64FC1);
-			T.at <double>(0,0) = Tvec[0];
-			T.at <double>(1,0) = Tvec[1];
-			T.at <double>(2,0) = Tvec[2];
 			output = StereoCameraModel(
 							cameraName_.toStdString(),
 							imageSize_[0], left.K_raw(), left.D_raw(), R1, P1,

guilib/src/CameraViewer.cpp

@@ -85,7 +85,7 @@ CameraViewer::CameraViewer(QWidget * parent, const ParametersMap & parameters) :
 	showScanCheckbox_->setChecked(true);
 
 	markerCheckbox_ = new QCheckBox("Detect markers", this);
-#if defined(HAVE_OPENCV_ARUCO) || defined(RTABMAP_APRILTAG)
+#if ((CV_MAJOR_VERSION > 4 || (CV_MAJOR_VERSION==4 && CV_MINOR_VERSION >=7)) && defined(HAVE_OPENCV_OBJDETECT)) || defined(HAVE_OPENCV_ARUCO) || defined(RTABMAP_APRILTAG)
 	markerCheckbox_->setEnabled(true);
 	markerDetector_ = new MarkerDetector(parameters);
 #else
@@ -175,7 +175,8 @@ void CameraViewer::showImage(const rtabmap::SensorData & data)
 		if(!models.empty() && models[0].isValidForProjection())
 		{
 			cv::Mat imageWithDetections;
-			detections = markerDetector_->detect(left, models, depthOrRight, _landmarksSize, &imageWithDetections);
+			cv::Mat depth = (depthOrRight.type()==CV_16UC1 || depthOrRight.type()==CV_32FC1) ? depthOrRight : cv::Mat();
+			detections = markerDetector_->detect(left, models, depth, _landmarksSize, &imageWithDetections);
 			imageView_->setImage(uCvMat2QImage(imageWithDetections));
 			for(std::map<int, MarkerInfo>::iterator iter=detections.begin(); iter!=detections.end(); ++iter)
 			{

guilib/src/GuiUtil.h

@@ -0,0 +1,59 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef RTABMAP_GUILIB_SRC_GUIUTIL_H_
+#define RTABMAP_GUILIB_SRC_GUIUTIL_H_
+
+#include <QWidget>
+#include <QApplication>
+#include <QEventLoop>
+#include <QElapsedTimer>
+#include <QtGui/QWindow>
+
+namespace rtabmap {
+
+// Show a widget and block until it is actually painted on screen. show() only
+// schedules window mapping + painting
+inline void showAndWaitExposed(QWidget * widget)
+{
+	widget->show();
+	if(widget->windowHandle())
+	{
+		QElapsedTimer timer;
+		timer.start();
+		while(!widget->windowHandle()->isExposed() && timer.elapsed() < 2000)
+		{
+			QApplication::processEvents(QEventLoop::ExcludeUserInputEvents | QEventLoop::WaitForMoreEvents, 50);
+		}
+	}
+	QApplication::processEvents(QEventLoop::ExcludeUserInputEvents);
+	widget->repaint(); // synchronous, unlike update()
+}
+
+} // namespace rtabmap
+
+#endif /* RTABMAP_GUILIB_SRC_GUIUTIL_H_ */

guilib/src/MainWindow.cpp

@@ -28,6 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/gui/MainWindow.h"
 
 #include "ui_mainWindow.h"
+#include "GuiUtil.h"
 
 #include "rtabmap/core/CameraRGB.h"
 #include "rtabmap/core/CameraStereo.h"
@@ -129,10 +130,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/global_map/GridMap.h>
 #endif
 
-#ifdef HAVE_OPENCV_ARUCO
-#include <opencv2/aruco.hpp>
-#endif
-
 #define LOG_FILE_NAME "LogRtabmap.txt"
 #define SHARE_SHOW_LOG_FILE "share/rtabmap/showlogs.m"
 #define SHARE_GET_PRECISION_RECALL_FILE "share/rtabmap/getPrecisionRecall.m"
@@ -5963,9 +5960,7 @@ void MainWindow::startDetection()
 	progress.setCancelButton(0);
 	progress.setMinimumDuration(0);
 	progress.setValue(0);
-	progress.show();
-	QApplication::processEvents();
-	QApplication::processEvents(); // make sure it is drawn
+	showAndWaitExposed(&progress);
 
 	if(_preferencesDialog->getLidarSourceDriver() != PreferencesDialog::kSrcUndef)
 	{
@@ -6279,9 +6274,7 @@ void MainWindow::stopDetection()
 		progress.setCancelButton(0);
 		progress.setMinimumDuration(0);
 		progress.setValue(0);
-		progress.show();
-		QApplication::processEvents();
-		QApplication::processEvents(); // make sure it is drawn
+		showAndWaitExposed(&progress);
 	}
 
 	// kill the processes

guilib/src/PreferencesDialog.cpp

@@ -61,6 +61,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <QMainWindow>
 #include <QProgressDialog>
 #include <QApplication>
+#include <QEventLoop>
+#include <QElapsedTimer>
+#include <QtGui/QWindow>
 #include <QLabel>
 #include <functional>
 #include <QScrollBar>
@@ -70,6 +73,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <QtGui/QCloseEvent>
 
 #include "ui_preferencesDialog.h"
+#include "GuiUtil.h"
 
 #include "rtabmap/core/Version.h"
 #include "rtabmap/core/Parameters.h"
@@ -506,10 +510,10 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->checkBox_showOdomFrustums->setChecked(false);
 #endif
 
-#if !defined(HAVE_OPENCV_ARUCO) && !defined(RTABMAP_APRILTAG)
+#if !((CV_MAJOR_VERSION > 4 || (CV_MAJOR_VERSION==4 && CV_MINOR_VERSION >=7)) && defined(HAVE_OPENCV_OBJDETECT)) && !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
+#if !(((CV_MAJOR_VERSION > 4 || (CV_MAJOR_VERSION==4 && CV_MINOR_VERSION >=7)) && defined(HAVE_OPENCV_OBJDETECT)) || defined(HAVE_OPENCV_ARUCO))
 	_ui->MarkerStrategy->setItemData(0, 0, Qt::UserRole - 1);
 #endif
 #ifndef RTABMAP_APRILTAG
@@ -7846,9 +7850,7 @@ void PreferencesDialog::testOdometry()
 	progress.setCancelButton(0);
 	progress.setMinimumDuration(0);
 	progress.setValue(0);
-	progress.show();
-	QApplication::processEvents();
-	QApplication::processEvents(); // make sure it is drawn
+	showAndWaitExposed(&progress);
 
 	Camera * camera = this->createCamera();
 	progress.hide();
@@ -7989,9 +7991,7 @@ void PreferencesDialog::testOdometry()
 	// at function scope end (not in join()), so 'progress' stays visible across it. On Windows
 	// the first 2-3 RealSense closes per launch stall ~20s in the Motion Module stop().
 	progress.setLabelText(tr("Closing camera..."));
-	progress.show();
-	QApplication::processEvents();
-	QApplication::processEvents(); // make sure it is drawn
+	showAndWaitExposed(&progress);
 	cameraThread.join(true);
 	odomThread.join(true);
 
@@ -8033,9 +8033,7 @@ void PreferencesDialog::testCamera()
 	progress.setCancelButton(0);
 	progress.setMinimumDuration(0);
 	progress.setValue(0);
-	progress.show();
-	QApplication::processEvents();
-	QApplication::processEvents(); // make sure it is drawn
+	showAndWaitExposed(&progress);
 
 	// createCamera() init()s the device on the GUI thread (required by ZED) and takes a few seconds.
 	Camera * camera = this->createCamera();
@@ -8092,9 +8090,7 @@ void PreferencesDialog::testCamera()
 		// stays visible across it. On Windows the first 2-3 RealSense closes per launch stall
 		// ~20s in the Motion Module stop() (librealsense warm-up); this keeps the user informed.
 		progress.setLabelText(tr("Closing camera..."));
-		progress.show();
-		QApplication::processEvents();
-		QApplication::processEvents(); // make sure it is drawn
+		showAndWaitExposed(&progress);
 		cameraThread.join(true); // cameraThread's destructor (scope end) closes the device
 		// deleteLater() (not delete): defer destruction to the event loop so Qt finishes
 		// tearing down the OpenGL widget's context and window-proc subclass and drains
@@ -8582,9 +8578,7 @@ void PreferencesDialog::testLidar()
 	progress.setCancelButton(0);
 	progress.setMinimumDuration(0);
 	progress.setValue(0);
-	progress.show();
-	QApplication::processEvents();
-	QApplication::processEvents(); // make sure it is drawn
+	showAndWaitExposed(&progress);
 
 	Lidar * lidar = this->createLidar();
 	progress.hide();
@@ -8613,9 +8607,7 @@ void PreferencesDialog::testLidar()
 		// destructor at scope end (not in join()), so 'progress' - declared in the outer
 		// scope - stays visible across it.
 		progress.setLabelText(tr("Closing sensor..."));
-		progress.show();
-		QApplication::processEvents();
-		QApplication::processEvents(); // make sure it is drawn
+		showAndWaitExposed(&progress);
 		lidarThread.join(true); // lidarThread's destructor (scope end) closes the device
 		// deleteLater() (not delete): see testCamera() - avoids a dangling OpenGL platform
 		// window that crashes in QWindowsWindow::alertWindow when Preferences later closes.

guilib/src/ui/calibrationDialog.ui

@@ -345,10 +345,10 @@
             <item row="2" column="0">
              <widget class="QLabel" name="label_15">
               <property name="toolTip">
-               <string/>
+               <string>Number of inner squares on the board</string>
               </property>
               <property name="text">
-               <string>Square Size</string>
+               <string>Board Size</string>
               </property>
              </widget>
             </item>
@@ -385,10 +385,10 @@
             <item row="3" column="0">
              <widget class="QLabel" name="label_12">
               <property name="toolTip">
-               <string>Number of inner squares on the board</string>
+               <string/>
               </property>
               <property name="text">
-               <string>Board Size</string>
+               <string>Square Size</string>
               </property>
              </widget>
             </item>