DBReader: added ignore imu option, GUI: added tf overrides option (#1637)

* DBReader: added ignore imu option, GUI: added tf overrides option * transform offset
2026-09-02 17:40:23 +08:00 · 2026-01-17 18:21:16 -08:00
parent 7db14533f1
commit 42faef3941
16 changed files with 330 additions and 166 deletions
--- a/corelib/src/DBReader.cpp
+++ b/corelib/src/DBReader.cpp
@@ -56,6 +56,7 @@ DBReader::DBReader(const std::string & databasePath,
 				   int startMapId,
 				   int stopMapId,
 				   bool priorsIgnored,
+				   bool imuIgnored,
 				   const std::vector<Transform> & cameraLocalTransformOverrides) :
 	Camera(frameRate),
 	_paths(uSplit(databasePath, ';')),
@@ -69,6 +70,7 @@ DBReader::DBReader(const std::string & databasePath,
 	_landmarksIgnored(landmarksIgnored),
 	_featuresIgnored(featuresIgnored),
 	_priorsIgnored(priorsIgnored),
+	_imuIgnored(imuIgnored),
 	_startMapId(startMapId),
 	_stopMapId(stopMapId),
 	_cameraLocalTransformOverrides(cameraLocalTransformOverrides),
@@ -96,6 +98,7 @@ DBReader::DBReader(const std::list<std::string> & databasePaths,
 				   int startMapId,
 				   int stopMapId,
 				   bool priorsIgnored,
+				   bool imuIgnored,
 				   const std::vector<Transform> & cameraLocalTransformOverrides) :
 	Camera(frameRate),
   _paths(databasePaths),
@@ -109,6 +112,7 @@ DBReader::DBReader(const std::list<std::string> & databasePaths,
 	_landmarksIgnored(landmarksIgnored),
 	_featuresIgnored(featuresIgnored),
 	_priorsIgnored(priorsIgnored),
+	_imuIgnored(imuIgnored),
 	_startMapId(startMapId),
 	_stopMapId(stopMapId),
 	_cameraLocalTransformOverrides(cameraLocalTransformOverrides),
@@ -463,14 +467,17 @@ SensorData DBReader::getNextData(SensorCaptureInfo * info)
 			}

 			Transform gravityTransform;
-			std::multimap<int, Link> gravityLinks;
-			_dbDriver->loadLinks(*_currentId, gravityLinks, Link::kGravity);
-			if( gravityLinks.size() &&
-				!gravityLinks.begin()->second.transform().isNull() &&
-				gravityLinks.begin()->second.infMatrix().cols == 6 &&
-				gravityLinks.begin()->second.infMatrix().rows == 6)
+			if(!_imuIgnored)
 			{
-				gravityTransform = gravityLinks.begin()->second.transform();
+				std::multimap<int, Link> gravityLinks;
+				_dbDriver->loadLinks(*_currentId, gravityLinks, Link::kGravity);
+				if( gravityLinks.size() &&
+					!gravityLinks.begin()->second.transform().isNull() &&
+					gravityLinks.begin()->second.infMatrix().cols == 6 &&
+					gravityLinks.begin()->second.infMatrix().rows == 6)
+				{
+					gravityTransform = gravityLinks.begin()->second.transform();
+				}
 			}

 			Landmarks landmarks;
--- a/corelib/src/Features2d.cpp
+++ b/corelib/src/Features2d.cpp
@@ -303,7 +303,7 @@ void Feature2D::limitKeypoints(std::vector<cv::KeyPoint> & keypoints, std::vecto
 		cv::Mat descriptorsTmp;
 		if(ssc)
 		{
-			ULOGGER_DEBUG("too much words (%d), removing words with SSC", keypoints.size());
+			ULOGGER_DEBUG("too many words (%d), removing words with SSC", keypoints.size());

 			// Sorting keypoints by deacreasing order of strength
 			std::vector<float> responseVector;
@@ -419,7 +419,7 @@ void Feature2D::limitKeypoints(const std::vector<cv::KeyPoint> & keypoints, std:
 		inliers.resize(keypoints.size(), false);
 		if(ssc)
 		{
-			ULOGGER_DEBUG("too much words (%d), removing words with SSC", keypoints.size());
+			ULOGGER_DEBUG("too many words (%d), removing words with SSC", keypoints.size());

 			// Sorting keypoints by deacreasing order of strength
 			std::vector<float> responseVector;
--- a/corelib/src/Memory.cpp
+++ b/corelib/src/Memory.cpp
@@ -5836,7 +5836,6 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 		cameraModels.size() == 1 &&
 		words.size() &&
 		(words3D.size() == 0 || (words.size() == words3D.size() && words3DValid!=(int)words3D.size())) &&
-		_registrationPipeline->isImageRequired() &&
 		_signatures.size() &&
 		_signatures.rbegin()->second->mapId() == _idMapCount) // same map
 	{
@@ -5880,11 +5879,14 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor

 			// The following is used only to re-estimate the correspondences, the returned transform is ignored
 			Transform tmpt;
-			RegistrationVis reg(parameters_);
+			ParametersMap tmpParams = parameters_;
+			// Pure 2D-2D without guess would generate variance=1
+			uInsert(tmpParams, ParametersPair(Parameters::kVisEpipolarGeometryVar(), "1"));
+			RegistrationVis reg(tmpParams);
 			if(_registrationPipeline->isScanRequired())
 			{
 				// If icp is used, remove it to just do visual registration
-				RegistrationVis vis(parameters_);
+				RegistrationVis vis(tmpParams);
 				tmpt = vis.computeTransformationMod(cpCurrent, cpPrevious, cameraTransform);
 			}
 			else
@@ -5906,11 +5908,18 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 			{
 				previousWords.insert(std::make_pair(iter->first, cpPrevious.getWordsKpts()[iter->second]));
 			}
+			float reprojError = Parameters::defaultVisPnPReprojError();
+			int varianceMedianRatio = Parameters::defaultVisPnPVarianceMedianRatio();
+			Parameters::parse(parameters_, Parameters::kVisPnPReprojError(), reprojError);
+			Parameters::parse(parameters_, Parameters::kVisPnPVarianceMedianRatio(), varianceMedianRatio);
 			std::map<int, cv::Point3f> inliers = util3d::generateWords3DMono(
 					currentWords,
 					previousWords,
 					cameraModels[0],
-					cameraTransform);
+					cameraTransform,
+					reprojError,
+					0.99f,
+					varianceMedianRatio);

 			UDEBUG("inliers=%d", (int)inliers.size());

--- a/corelib/src/RegistrationIcp.cpp
+++ b/corelib/src/RegistrationIcp.cpp
@@ -529,6 +529,7 @@ Transform RegistrationIcp::computeTransformationImpl(
 				double toComplexity = util3d::computeNormalsComplexity(toScan, guess, &complexityVectorsTo, &complexityValuesTo);
 				float complexity = fromComplexity<toComplexity?fromComplexity:toComplexity;
 				info.icpStructuralComplexity = complexity;
+				UDEBUG("structural complexity: from=%f to=%f", fromComplexity, toComplexity);
 				if(complexity < _pointToPlaneMinComplexity)
 				{
 					tooLowComplexityForPlaneToPlane = true;
--- a/corelib/src/RegistrationVis.cpp
+++ b/corelib/src/RegistrationVis.cpp
@@ -321,6 +321,7 @@ Transform RegistrationVis::computeTransformationImpl(
 	UDEBUG("%s=%d", Parameters::kVisPnPFlags().c_str(), _PnPFlags);
 	UDEBUG("%s=%f", Parameters::kVisPnPMaxVariance().c_str(), _PnPMaxVar);
 	UDEBUG("%s=%f", Parameters::kVisPnPSplitLinearCovComponents().c_str(), _PnPSplitLinearCovarianceComponents);
+	UDEBUG("%s=%f", Parameters::kVisPnPVarianceMedianRatio().c_str(), _PnPVarMedianRatio);
 	UDEBUG("%s=%d", Parameters::kVisCorType().c_str(), _correspondencesApproach);
 	UDEBUG("%s=%d", Parameters::kVisCorFlowWinSize().c_str(), _flowWinSize);
 	UDEBUG("%s=%d", Parameters::kVisCorFlowIterations().c_str(), _flowIterations);
@@ -1632,6 +1633,7 @@ Transform RegistrationVis::computeTransformationImpl(
 						cameraTransform,
 						_PnPReprojError,
 						0.99f,
+						_PnPVarMedianRatio,
 						words3A, // for scale estimation
 						&variance,
 						&matchesV);
@@ -2204,6 +2206,10 @@ Transform RegistrationVis::computeTransformationImpl(
 	info.matches = matchesCount;
 	info.rejectedMsg = msg;
 	info.covariance = covariance;
+	if(!covariance.empty())
+	{
+		info.variance = covariance.at<double>(0,0);
+	}

 	UDEBUG("inliers=%d/%d", info.inliers, info.matches);
 	UDEBUG("transform=%s", transform.prettyPrint().c_str());
--- a/corelib/src/Rtabmap.cpp
+++ b/corelib/src/Rtabmap.cpp
@@ -2613,6 +2613,7 @@ bool Rtabmap::process(
 	int loopClosureVisualInliers = 0; // for statistics
 	float loopClosureVisualInliersRatio = 0.0f;
 	int loopClosureVisualMatches = 0;
+	float loopClosureVisualVariance = 0.0f;
 	float loopClosureLinearVariance = 0.0f;
 	float loopClosureAngularVariance = 0.0f;
 	float loopClosureVisualInliersMeanDist = 0;
@@ -2795,6 +2796,7 @@ bool Rtabmap::process(
 									loopClosureVisualInliers = info.inliers;
 									loopClosureVisualInliersRatio = info.inliersRatio;
 									loopClosureVisualMatches = info.matches;
+									loopClosureVisualVariance = info.variance;

 									cv::Mat information = getInformation(info.covariance);
 									loopClosureLinearVariance = 1.0/information.at<double>(0,0);
@@ -3062,6 +3064,7 @@ bool Rtabmap::process(
 				loopClosureVisualInliers = info.inliers;
 				loopClosureVisualInliersRatio = info.inliersRatio;
 				loopClosureVisualMatches = info.matches;
+				loopClosureVisualVariance = info.variance;
 				rejectedLoopClosure = transform.isNull();
 				if(rejectedLoopClosure)
 				{
@@ -4049,6 +4052,7 @@ bool Rtabmap::process(
 			statistics_.addStatistic(Statistics::kLoopVisual_inliers(), loopClosureVisualInliers);
 			statistics_.addStatistic(Statistics::kLoopVisual_inliers_ratio(), loopClosureVisualInliersRatio);
 			statistics_.addStatistic(Statistics::kLoopVisual_matches(), loopClosureVisualMatches);
+			statistics_.addStatistic(Statistics::kLoopVisual_variance(), loopClosureVisualVariance);
 			statistics_.addStatistic(Statistics::kLoopLinear_variance(), loopClosureLinearVariance);
 			statistics_.addStatistic(Statistics::kLoopAngular_variance(), loopClosureAngularVariance);
 			statistics_.addStatistic(Statistics::kLoopLast_id(), _memory->getLastGlobalLoopClosureId());
--- a/corelib/src/odometry/OdometryMono.cpp
+++ b/corelib/src/odometry/OdometryMono.cpp
@@ -775,6 +775,7 @@ Transform OdometryMono::computeTransform(SensorData & data, const Transform & gu
 					cameraTransform,
 					fundMatrixReprojError_,
 					fundMatrixConfidence_,
+					4,
 					refWords3Guess);  // for scale estimation

 			if(cameraTransform.getNorm() < minTranslation_*5)
--- a/corelib/src/util3d_features.cpp
+++ b/corelib/src/util3d_features.cpp
@@ -213,6 +213,7 @@ std::map<int, cv::Point3f> generateWords3DMono(
 		Transform & cameraTransform,
 		float ransacReprojThreshold,
 		float ransacConfidence,
+		int varianceMedianRatio,
 		const std::map<int, cv::Point3f> & refGuess3D,
 		double * varianceOut,
 		std::vector<int> * matchesOut)
@@ -345,7 +346,7 @@ std::map<int, cv::Point3f> generateWords3DMono(
 							errorSqrdDists[j] = uNormSquared(refPt.x-newPt.x, refPt.y-newPt.y, refPt.z-newPt.z);
 						}
 						std::sort(errorSqrdDists.begin(), errorSqrdDists.end());
-						double median_error_sqr = (double)errorSqrdDists[errorSqrdDists.size () >> 2];
+						double median_error_sqr = (double)errorSqrdDists[errorSqrdDists.size () >> varianceMedianRatio];
 						float var = 2.1981 * median_error_sqr;
 						//UDEBUG("scale %d = %f variance = %f", (int)i, s, variance);

@@ -369,7 +370,7 @@ std::map<int, cv::Point3f> generateWords3DMono(
 						errorSqrdDists[j] = uNormSquared(refPt.x-newPt.x, refPt.y-newPt.y, refPt.z-newPt.z);
 					}
 					std::sort(errorSqrdDists.begin(), errorSqrdDists.end());
-					double median_error_sqr = (double)errorSqrdDists[errorSqrdDists.size () >> 2];
+					double median_error_sqr = (double)errorSqrdDists[errorSqrdDists.size () >> varianceMedianRatio];
 					variance = 2.1981 * median_error_sqr;
 				}
 			}