Added smartfactor gtsam

corelib/src/optimizer/OptimizerGTSAM.cpp

@@ -48,12 +48,15 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <gtsam/slam/BetweenFactor.h>
 #include <gtsam/slam/ProjectionFactor.h>
 #include <gtsam/slam/StereoFactor.h>
+#include <gtsam/slam/SmartProjectionPoseFactor.h>
 #include <gtsam/sam/BearingFactor.h>
 #include <gtsam/sam/BearingRangeFactor.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/nonlinear/GaussNewtonOptimizer.h>
 #include <gtsam/nonlinear/DoglegOptimizer.h>
 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
+#include <gtsam/linear/PCGSolver.h>
+#include <gtsam/linear/Preconditioner.h>
 #include <gtsam/nonlinear/NonlinearOptimizer.h>
 #include <gtsam/nonlinear/Marginals.h>
 #include <gtsam/nonlinear/Values.h>
@@ -1178,7 +1181,7 @@ std::map<int, Transform> OptimizerGTSAM::optimizeBA(
 	}
 
 	gtsam::NonlinearFactorGraph graph;
-	gtsam::Values initial;
+	gtsam::Values initialEstimate;
 
 	// Cache per-frame, per-camera intrinsics. Note that GTSAM's
 	// GenericProjectionFactor/GenericStereoFactor hold a shared_ptr to the
@@ -1217,7 +1220,7 @@ std::map<int, Transform> OptimizerGTSAM::optimizeBA(
 				return optimizedPoses;
 			}
 			const gtsam::Symbol xkey('x', iter->first * GTSAM_BA_MULTICAM_OFFSET + (int)i);
-			initial.insert(xkey, gtsam::Pose3(camPose.toEigen4d()));
+			initialEstimate.insert(xkey, gtsam::Pose3(camPose.toEigen4d()));
 
 			// Intrinsics: skew=0 (no shear in any CameraModel rtabmap supports).
 			gtsam::Cal3_S2::shared_ptr K(new gtsam::Cal3_S2(m.fx(), m.fy(), 0.0, m.cx(), m.cy()));
@@ -1341,7 +1344,13 @@ std::map<int, Transform> OptimizerGTSAM::optimizeBA(
 	const double sigmaDisparity = std::sqrt(disparityVariance_);
 	gtsam::SharedNoiseModel stereoNoiseModel = gtsam::noiseModel::Diagonal::Sigmas(
 			(gtsam::Vector(3) << sigmaPixel, sigmaDisparity, sigmaPixel).finished());
-	gtsam::SharedNoiseModel monoNoiseModel = gtsam::noiseModel::Isotropic::Sigma(2, sigmaPixel);
+	// SmartProjectionFactor requires an isotropic noise model — its
+	// constructor rejects diagonal/robust wrappers. Keep the un-wrapped
+	// isotropic around for the SmartFactor path; the generic factors
+	// still get the Huber-wrapped version when robust is on.
+	gtsam::SharedNoiseModel monoIsotropicNoise =
+			gtsam::noiseModel::Isotropic::Sigma(2, sigmaPixel);
+	gtsam::SharedNoiseModel monoNoiseModel = monoIsotropicNoise;
 	if(robustKernelDelta_ > 0.0)
 	{
 		gtsam::noiseModel::mEstimator::Base::shared_ptr huber =
@@ -1349,6 +1358,18 @@ std::map<int, Transform> OptimizerGTSAM::optimizeBA(
 		stereoNoiseModel = gtsam::noiseModel::Robust::Create(huber, stereoNoiseModel);
 		monoNoiseModel   = gtsam::noiseModel::Robust::Create(huber, monoNoiseModel);
 	}
+	// Per-landmark: if EVERY observation is mono (no usable stereo
+	// depth) we fold all observations into a single
+	// SmartProjectionPoseFactor — it triangulates the 3D point
+	// internally and applies Schur complement per-factor, so the
+	// point doesn't appear as a graph variable. That's the GTSAM-
+	// native way to do BA (see the SFMExample_SmartFactorPCG demo).
+	//
+	// Stereo-bearing landmarks still go through GenericStereoFactor:
+	// the stereo smart factor lives in gtsam_unstable which we don't
+	// link.
+	using SmartMono = gtsam::SmartProjectionPoseFactor<gtsam::Cal3_S2>;
+	std::map<int, SmartMono::shared_ptr> smartByWord;
 	for(std::map<int, std::map<int, FeatureBA> >::const_iterator iter = wordReferences.begin(); iter!=wordReferences.end(); ++iter)
 	{
 		const int wordId = iter->first;
@@ -1362,9 +1383,52 @@ std::map<int, Transform> OptimizerGTSAM::optimizeBA(
 			UWARN("Ignoring 3D point %d because it has nan value(s)!", wordId);
 			continue;
 		}
+
+		// Probe whether this landmark has any usable stereo observation;
+		// that decides which factor type we use.
+		bool anyStereoForWord = false;
+		for(const auto & jkv : iter->second)
+		{
+			const std::pair<int,int> camKey(jkv.first, jkv.second.cameraIndex);
+			const double depth    = jkv.second.depth;
+			const double baseline = baselineByCam.count(camKey) ? baselineByCam.at(camKey) : 0.0;
+			if(uIsFinite(depth) && depth > 0.0 && baseline > 0.0
+					&& calStereo.count(camKey))
+			{
+				anyStereoForWord = true;
+				break;
+			}
+		}
+
 		const gtsam::Symbol pkey = point3dSymbol(wordId);
-		initial.insert(pkey, gtsam::Point3(pt3d.x, pt3d.y, pt3d.z));
+		SmartMono::shared_ptr smartFactor;
-		insertedPoints.insert(pkey);
+		if(!anyStereoForWord)
+		{
+			// Mono-only landmark → SmartProjectionPoseFactor. Use the
+			// first observation's Cal3_S2 (the smart factor needs one
+			// K shared across all observations).
+			gtsam::Cal3_S2::shared_ptr Kshared;
+			for(const auto & jkv : iter->second)
+			{
+				const std::pair<int,int> camKey(jkv.first, jkv.second.cameraIndex);
+				if(calMono.count(camKey))
+				{
+					Kshared = calMono.at(camKey);
+					break;
+				}
+			}
+			if(Kshared)
+			{
+				smartFactor = boost::make_shared<SmartMono>(monoIsotropicNoise, Kshared);
+			}
+		}
+		if(!smartFactor)
+		{
+			// Stereo path: keep per-observation factors with an
+			// explicit Point3 variable in initialEstimate.
+			initialEstimate.insert(pkey, gtsam::Point3(pt3d.x, pt3d.y, pt3d.z));
+			insertedPoints.insert(pkey);
+		}
 
 		for(std::map<int, FeatureBA>::const_iterator jter = iter->second.begin(); jter != iter->second.end(); ++jter)
 		{
@@ -1381,7 +1445,7 @@ std::map<int, Transform> OptimizerGTSAM::optimizeBA(
 				continue;
 			}
 			const gtsam::Symbol xkey('x', poseId * GTSAM_BA_MULTICAM_OFFSET + camIdx);
-			if(!initial.exists(xkey))
+			if(!initialEstimate.exists(xkey))
 			{
 				continue;
 			}
@@ -1390,14 +1454,19 @@ std::map<int, Transform> OptimizerGTSAM::optimizeBA(
 			const double baseline = baselineByCam.count(camKey) ? baselineByCam.at(camKey) : 0.0;
 			const bool isStereo   = (uIsFinite(depth) && depth > 0.0 && baseline > 0.0 && calStereo.count(camKey));
 
-			size_t factorIdx = graph.size();
+			if(smartFactor)
-			if(isStereo)
+			{
+				smartFactor->add(gtsam::Point2(f.kpt.pt.x, f.kpt.pt.y), xkey);
+			}
+			else if(isStereo)
 			{
 				const gtsam::Cal3_S2Stereo::shared_ptr & Ks = calStereo.at(camKey);
 				const double disparity = baseline * Ks->fx() / depth;
 				const gtsam::StereoPoint2 obs(f.kpt.pt.x, f.kpt.pt.x - disparity, f.kpt.pt.y);
+				size_t factorIdx = graph.size();
 				graph.add(gtsam::GenericStereoFactor<gtsam::Pose3, gtsam::Point3>(
 						obs, stereoNoiseModel, xkey, pkey, Ks));
+				obsFactors.push_back(std::make_pair(factorIdx, wordId));
 			}
 			else
 			{
@@ -1408,10 +1477,17 @@ std::map<int, Transform> OptimizerGTSAM::optimizeBA(
 				}
 				const gtsam::Cal3_S2::shared_ptr & K = calMono.at(camKey);
 				const gtsam::Point2 obs(f.kpt.pt.x, f.kpt.pt.y);
+				size_t factorIdx = graph.size();
 				graph.add(gtsam::GenericProjectionFactor<gtsam::Pose3, gtsam::Point3, gtsam::Cal3_S2>(
 						obs, monoNoiseModel, xkey, pkey, K));
+				obsFactors.push_back(std::make_pair(factorIdx, wordId));
 			}
-			obsFactors.push_back(std::make_pair(factorIdx, wordId));
+		}
+
+		if(smartFactor && smartFactor->size() >= 2)
+		{
+			graph.add(smartFactor);
+			smartByWord[wordId] = smartFactor;
 		}
 	}
 
@@ -1427,20 +1503,33 @@ std::map<int, Transform> OptimizerGTSAM::optimizeBA(
 		// Gauss-Newton's unbounded step can blow up. Dogleg works but
 		// offers no advantage over LM on BA. LM is what every major BA
 		// library (Ceres, g2o, COLMAP) defaults to.
-		// Use a tight tolerance so the optimizer runs to convergence
-		// instead of stopping early on GTSAM's default absoluteErrorTol
-		// (1e-5), which leaves the longest-range points off truth on
-		// mono BA.
-		const double tol = epsilon() > 0.0 ? epsilon() : 1e-12;
 		gtsam::LevenbergMarquardtParams params;
-		params.relativeErrorTol = tol;
+		if(epsilon() > 0.0)
-		params.absoluteErrorTol = tol;
+		{
+			params.relativeErrorTol = epsilon();
+			params.absoluteErrorTol = epsilon();
+		}
 		params.maxIterations    = iterations();
-		gtsam::NonlinearOptimizer * optimizer = new gtsam::LevenbergMarquardtOptimizer(graph, initial, params);
+		// Use PCG + Block-Jacobi instead of GTSAM's default multifrontal
+		// Cholesky. The example in the GTSAM repo (SFMExample_SmartFactorPCG)
+		// confirms the inner-solve tolerances must be tight enough that
+		// the iterative solver doesn't bottom out before LM converges —
+		// 1e-10 matches the example and keeps point accuracy within
+		// the test bounds. On our small problems this is ~3× faster
+		// than the direct Cholesky path.
+		params.linearSolverType = gtsam::NonlinearOptimizerParams::Iterative;
+		gtsam::PCGSolverParameters::shared_ptr pcg =
+				boost::make_shared<gtsam::PCGSolverParameters>();
+		pcg->setPreconditionerParams(
+				boost::make_shared<gtsam::BlockJacobiPreconditionerParameters>());
+		pcg->epsilon_abs_ = 1e-10;
+		pcg->epsilon_rel_ = 1e-10;
+		params.iterativeParams = pcg;
+		gtsam::NonlinearOptimizer * optimizer = new gtsam::LevenbergMarquardtOptimizer(graph, initialEstimate, params);
 		UDEBUG("GTSAM BA optimizing (max iterations=%d, robustKernel=%f)...", iterations(), robustKernelDelta_);
 		result = optimizer->optimize();
 		finalError = optimizer->error();
-		UDEBUG("GTSAM BA done (initialError=%f finalError=%f time=%fs)", graph.error(initial), finalError, timer.ticks());
+		UDEBUG("GTSAM BA done (initialError=%f finalError=%f time=%fs)", graph.error(initialEstimate), finalError, timer.ticks());
 		delete optimizer;
 	}
 	catch(const gtsam::IndeterminantLinearSystemException & e)
@@ -1525,6 +1614,21 @@ std::map<int, Transform> OptimizerGTSAM::optimizeBA(
 	// 8) Read back 3D points.
 	for(std::map<int, cv::Point3f>::iterator iter = points3DMap.begin(); iter != points3DMap.end(); ++iter)
 	{
+		// SmartFactor landmarks aren't graph variables — triangulate from
+		// the optimized poses instead.
+		std::map<int, SmartMono::shared_ptr>::const_iterator sit = smartByWord.find(iter->first);
+		if(sit != smartByWord.end())
+		{
+			boost::optional<gtsam::Point3> p = sit->second->point(result);
+			if(p)
+			{
+				iter->second = cv::Point3f(
+						static_cast<float>(p->x()),
+						static_cast<float>(p->y()),
+						static_cast<float>(p->z()));
+			}
+			continue;
+		}
 		const gtsam::Symbol pkey = point3dSymbol(iter->first);
 		if(insertedPoints.count(pkey) && result.exists(pkey))
 		{

corelib/test/test_optimizer.cpp

@@ -20,6 +20,7 @@
 #include <rtabmap/core/CameraModel.h>
 #include <rtabmap/core/util3d_transforms.h>
 #include <rtabmap/utilite/UConversion.h>
+#include <rtabmap/utilite/UTimer.h>
 #include <memory>
 #include <random>
 
@@ -1900,8 +1901,14 @@ TEST_P(BundleAdjustmentTest, CircleCamerasRecoverPosesAndPoints)
 	}
 
 	std::map<int, cv::Point3f> outPoints = g.initialPoints3D;
+	UTimer baTimer;
 	std::map<int, Transform> outPoses = opt->optimizeBA(
 			/*rootId=*/1, g.initialPoses, g.links, g.models, outPoints, g.wordReferences);
+	const double baSeconds = baTimer.getElapsedTime();
+	std::cerr << "[BA-timing] backend=" << optimizerTypeName(backend)
+			  << " variant=" << baVariantName(variant)
+			  << " roundPixels=" << (roundPixels?1:0)
+			  << " seconds=" << baSeconds << "\n";
 
 	ASSERT_FALSE(outPoses.empty()) << "optimizeBA returned no poses";
 

corelib/test/test_rtabmap_integration.cpp

@@ -132,7 +132,12 @@ ReplayResult replayDatabase(
 		const ParametersMap & odometryParameters,
 		bool useStoredOdomAsGuess = false,
 		bool passOdomDataToRtabmap = false,
-		const std::map<double, Transform> * goldenStampedGroundTruth = nullptr)
+		const std::map<double, Transform> * goldenStampedGroundTruth = nullptr,
+		// When >1, drop (stride-1) frames out of every `stride` reads
+		// before any odometry/rtabmap work — halves work at stride=2,
+		// thirds at stride=3, etc. The throttle inside rtabmap still
+		// applies on top.
+		int frameStride = 1)
 {
 	ReplayResult result;
 
@@ -202,9 +207,11 @@ ReplayResult replayDatabase(
 	// Some old-format DBs (Stereo20Hz, Version 0.8.0) have no stored
 	// stamps, so DBReader fills them with wall-clock at read time.
 	// That makes the Rtabmap/DetectionRate throttle depend on
-	// processing speed and skews per-optimizer comparisons. Use a
+	// processing speed and skews per-optimizer comparisons. Detect
-	// synthetic monotonic 20 Hz timeline whenever the first frame
+	// that case by checking whether the first frame's stamp is
-	// looks like a wall-clock stamp (years 2000+).
+	// suspiciously close to "right now" (within the last hour); if
+	// so, rewrite every frame's stamp to a synthetic monotonic 20 Hz
+	// timeline.
 	constexpr double kSyntheticFrameDt = 1.0 / 20.0;  // 20 Hz
 	int syntheticFrameIdx = 0;
 	bool overrideStamps = false;
@@ -212,7 +219,7 @@ ReplayResult replayDatabase(
 	// Prime the loop with the first sample.
 	SensorCaptureInfo info;
 	SensorData data = dbReader.takeData(&info);
-	overrideStamps = data.stamp() > 1.0e9;  // > year 2001 in unix-time
+	overrideStamps = data.stamp() > UTimer::now() - 3600.0;
 	if(overrideStamps)
 	{
 		data.setStamp(syntheticFrameIdx++ * kSyntheticFrameDt);
@@ -224,6 +231,22 @@ ReplayResult replayDatabase(
 	{
 		++result.framesRead;
 
+		// Drop (stride-1) of every `stride` frames before any
+		// odom/rtabmap processing. Note: lastUpdateStamp /
+		// previousStoredOdomPose advance only on processed frames,
+		// so the throttle window still measures against the last
+		// frame we actually fed in.
+		if(frameStride > 1 && (result.framesRead - 1) % frameStride != 0)
+		{
+			data = dbReader.takeData(&info);
+			if(overrideStamps && data.isValid())
+			{
+				data.setStamp(syntheticFrameIdx++ * kSyntheticFrameDt);
+			}
+			applyGoldenGroundTruth(data);
+			continue;
+		}
+
 		// Build the motion guess from the stored odom delta if requested.
 		// Null Transform = no guess.
 		Transform guess;