Added GTSAM BA, updated Ceres to use g2o ba parameters. Renamed g2o's ba related parameters to Optimizer group and used by both gtsam and ceres.

corelib/include/rtabmap/core/Parameters.h

@@ -472,10 +472,11 @@ class RTABMAP_CORE_EXPORT Parameters
     RTABMAP_PARAM(g2o, Solver,            int, 0,          "0=csparse 1=pcg 2=cholmod 3=Eigen");
 #endif
     RTABMAP_PARAM(g2o, Optimizer,         int, 0,          "0=Levenberg 1=GaussNewton");
-    RTABMAP_PARAM(g2o, PixelVariance,     double, 1.0,     "Pixel variance used on the u/v axes of every bundle adjustment reprojection edge. Should approximate the squared 1-sigma keypoint localization error in pixels. Set higher (e.g. 4-9) if features are noisy (low texture, motion blur, low light, or large detector scale). Set lower (e.g. 0.01-0.1) if features are sub-pixel refined (Lucas-Kanade tracking, parabolic peak interpolation). Intuition: the lower the pixel variance, the more the optimizer trusts the keypoint positions.");
-    RTABMAP_PARAM(g2o, DisparityVariance, double, 1.0,     "Disparity variance used on the disparity axis (u - u_right) of stereo / RGB-D bundle adjustment edges. Defaults to the same value as PixelVariance for backward compatibility. Set higher (e.g. 2-4) if your depth source is noisier than your feature detector's u/v precision (typical for stereo block matchers / SGM at long range). Set lower (e.g. 0.01-0.1) if your depth source is more accurate than the u/v detector (typical for ToF / LiDAR-fused depth where range is measured directly rather than triangulated). Intuition: the lower the disparity variance, the more the optimizer trusts the depth measurements. Geometric note: wider baseline and/or higher image resolution improve a block matcher's effective disparity precision (larger disparity magnitudes and finer sub-pixel refinement), so wide-baseline high-resolution stereo pairs can usually afford a lower disparity variance (e.g. 0.1-0.5); narrow-baseline low-resolution pairs should keep it higher (e.g. 1-4).");
-    RTABMAP_PARAM(g2o, RobustKernelDelta, double, 8,       "Robust kernel delta used for bundle adjustment (0 means don't use robust kernel). Observations with chi2 over this threshold will be ignored in the second optimization pass.");
-    RTABMAP_PARAM(g2o, Baseline,          double, 0.075,   "When doing bundle adjustment with RGB-D data, we can set a fake baseline (m) to do stereo bundle adjustment (if 0, mono bundle adjustment is done). For stereo data, the baseline in the calibration is used directly.");
+
+    RTABMAP_PARAM(Optimizer, Baseline,           double, 0.075,   "When doing bundle adjustment with RGB-D data (mono camera + depth), set a fake baseline (m) so the BA backend treats depth as stereo disparity. Applies to all BA-capable backends (g2o, GTSAM, Ceres). Set to 0 to keep the problem mono (depth observations are ignored). For real stereo data the baseline in the calibration (Tx) is used directly.");
+    RTABMAP_PARAM(Optimizer, PixelVariance,     double, 1.0,     "Pixel variance used on the u/v axes of every bundle adjustment reprojection edge. Applies to all BA-capable backends (g2o, GTSAM, Ceres). Should approximate the squared 1-sigma keypoint localization error in pixels. Set higher (e.g. 4-9) if features are noisy (low texture, motion blur, low light, or large detector scale). Set lower (e.g. 0.01-0.1) if features are sub-pixel refined (Lucas-Kanade tracking, parabolic peak interpolation). Intuition: the lower the pixel variance, the more the optimizer trusts the keypoint positions.");
+    RTABMAP_PARAM(Optimizer, DisparityVariance, double, 1.0,     "Disparity variance used on the disparity axis (u - u_right) of stereo / RGB-D bundle adjustment edges. Applies to all BA-capable backends (g2o, GTSAM, Ceres). Defaults to the same value as PixelVariance for backward compatibility. Set higher (e.g. 2-4) if your depth source is noisier than your feature detector's u/v precision (typical for stereo block matchers / SGM at long range). Set lower (e.g. 0.01-0.1) if your depth source is more accurate than the u/v detector (typical for ToF / LiDAR-fused depth where range is measured directly rather than triangulated). Intuition: the lower the disparity variance, the more the optimizer trusts the depth measurements. Geometric note: wider baseline and/or higher image resolution improve a block matcher's effective disparity precision (larger disparity magnitudes and finer sub-pixel refinement), so wide-baseline high-resolution stereo pairs can usually afford a lower disparity variance (e.g. 0.1-0.5); narrow-baseline low-resolution pairs should keep it higher (e.g. 1-4).");
+    RTABMAP_PARAM(Optimizer, RobustKernelDelta, double, 8,       "Robust kernel delta used for bundle adjustment (0 means don't use robust kernel). Applies to all BA-capable backends (g2o, GTSAM, Ceres). Observations with chi2 over this threshold will be ignored in the second optimization pass.");
 
     RTABMAP_PARAM(GTSAM, Optimizer,       int, 1,          "0=Levenberg 1=GaussNewton 2=Dogleg");
     RTABMAP_PARAM(GTSAM, Incremental,     bool, false,     uFormat("Do graph optimization incrementally (iSAM2) to increase optimization speed on loop closures. Note that only GaussNewton and Dogleg optimization algorithms are supported (%s) in this mode.", kGTSAMOptimizer().c_str()));

corelib/include/rtabmap/core/Rtabmap.h

@@ -678,7 +678,7 @@ public:
 	 *        before BA (otherwise reuse existing word-id correspondences).
 	 * @param iterations Solver iterations (0 falls back to @ref Parameters::kOptimizerIterations()).
 	 * @param pixelVariance Pixel reprojection variance used by the cost (0 falls back
-	 *        to @ref Parameters::kg2oPixelVariance()).
+	 *        to @ref Parameters::kOptimizerPixelVariance()).
 	 * @return True if BA was run and improved poses were stored.
 	 */
 	bool globalBundleAdjustment(

corelib/include/rtabmap/core/optimizer/OptimizerCeres.h

@@ -43,13 +43,18 @@ public:
 			bool slam2d            = Parameters::defaultRegForce3DoF(),
 			bool covarianceIgnored = Parameters::defaultOptimizerVarianceIgnored(),
 			double epsilon         = Parameters::defaultOptimizerEpsilon()) :
-		Optimizer(iterations, slam2d, covarianceIgnored, epsilon) {}
-	OptimizerCeres(const ParametersMap & parameters) :
-		Optimizer(parameters) {}
+		Optimizer(iterations, slam2d, covarianceIgnored, epsilon),
+		pixelVariance_(Parameters::defaultOptimizerPixelVariance()),
+		disparityVariance_(Parameters::defaultOptimizerDisparityVariance()),
+		robustKernelDelta_(Parameters::defaultOptimizerRobustKernelDelta()),
+		baseline_(Parameters::defaultOptimizerBaseline()) {}
+	OptimizerCeres(const ParametersMap & parameters);
 	virtual ~OptimizerCeres() {}
 
 	virtual Type type() const {return kTypeCeres;}
 
+	virtual void parseParameters(const ParametersMap & parameters);
+
 	virtual std::map<int, Transform> optimize(
 			int rootId,
 			const std::map<int, Transform> & poses,
@@ -67,6 +72,12 @@ public:
 			std::map<int, cv::Point3f> & points3DMap,
 			const std::map<int, std::map<int, FeatureBA> > & wordReferences, // <ID words, IDs frames + keypoint(x,y,depth)>
 			std::set<int> * outliers = 0);
+
+private:
+	double pixelVariance_;
+	double disparityVariance_;
+	double robustKernelDelta_;
+	double baseline_;
 };
 
 } /* namespace rtabmap */

corelib/include/rtabmap/core/optimizer/OptimizerGTSAM.h

@@ -58,8 +58,21 @@ public:
 			double * finalError = 0,
 			int * iterationsDone = 0);
 
+	virtual std::map<int, Transform> optimizeBA(
+			int rootId,
+			const std::map<int, Transform> & poses,
+			const std::multimap<int, Link> & links,
+			const std::map<int, std::vector<CameraModel> > & models,
+			std::map<int, cv::Point3f> & points3DMap,
+			const std::map<int, std::map<int, FeatureBA> > & wordReferences,
+			std::set<int> * outliers = 0);
+
 private:
 	int internalOptimizerType_;
+	double pixelVariance_;
+	double disparityVariance_;
+	double robustKernelDelta_;
+	double baseline_;
 
 	gtsam::ISAM2 * isam2_;
 	struct ConstraintToFactor {

corelib/src/Parameters.cpp

@@ -241,6 +241,13 @@ const std::map<std::string, std::pair<bool, std::string> > & Parameters::getRemo
 		// 0.23.7
 		removedParameters_.insert(std::make_pair("Marker/CornerRefinementMethod", std::make_pair(true, Parameters::kMarkerOpenCVCornerRefinementMethod())));
 
+		// BA tunables moved from g2o/ namespace to Optimizer/ since they
+		// now apply to g2o, GTSAM, and Ceres backends.
+		removedParameters_.insert(std::make_pair("g2o/PixelVariance",     std::make_pair(true, Parameters::kOptimizerPixelVariance())));
+		removedParameters_.insert(std::make_pair("g2o/DisparityVariance", std::make_pair(true, Parameters::kOptimizerDisparityVariance())));
+		removedParameters_.insert(std::make_pair("g2o/RobustKernelDelta", std::make_pair(true, Parameters::kOptimizerRobustKernelDelta())));
+		removedParameters_.insert(std::make_pair("g2o/Baseline",          std::make_pair(true, Parameters::kOptimizerBaseline())));
+
 		// 0.23.1
 		removedParameters_.insert(std::make_pair("OdomVINS/ConfigPath",    std::make_pair(true, Parameters::kOdomVINSFusionConfigPath())));
 

corelib/src/Rtabmap.cpp

@@ -6401,17 +6401,17 @@ bool Rtabmap::globalBundleAdjustment(
 	if(!_optimizedPoses.empty() && !_constraints.empty())
 	{
 		int iterations = Parameters::defaultOptimizerIterations();
-		float pixelVariance = Parameters::defaultg2oPixelVariance();
+		float pixelVariance = Parameters::defaultOptimizerPixelVariance();
 		ParametersMap params = _parameters;
 		Parameters::parse(params, Parameters::kOptimizerIterations(), iterations);
-		Parameters::parse(params, Parameters::kg2oPixelVariance(), pixelVariance);
+		Parameters::parse(params, Parameters::kOptimizerPixelVariance(), pixelVariance);
 		if(iterations > 0)
 		{
 			uInsert(params, ParametersPair(Parameters::kOptimizerIterations(), uNumber2Str(iterations)));
 		}
 		if(pixelVariance > 0.0f)
 		{
-			uInsert(params, ParametersPair(Parameters::kg2oPixelVariance(), uNumber2Str(pixelVariance)));
+			uInsert(params, ParametersPair(Parameters::kOptimizerPixelVariance(), uNumber2Str(pixelVariance)));
 		}
 
 		std::map<int, Signature> signatures;

corelib/src/optimizer/OptimizerCeres.cpp

@@ -52,6 +52,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "ceres/pose_graph_3d/pose_graph_3d_error_term.h"
 #include "ceres/bundle/BAProblem.h"
 #include "ceres/bundle/snavely_reprojection_error.h"
+#include "ceres/bundle/snavely_stereo_reprojection_error.h"
+#include "ceres/bundle/between_cameras_error.h"
+#include "ceres/bundle/planar_constraint_error.h"
 
 #if not(CERES_VERSION_MAJOR > 1 || (CERES_VERSION_MAJOR == 1 && CERES_VERSION_MINOR >= 12))
 #include "ceres/pose_graph_3d/eigen_quaternion_manifold.h"
@@ -88,6 +91,28 @@ bool OptimizerCeres::available()
 #endif
 }
 
+OptimizerCeres::OptimizerCeres(const ParametersMap & parameters) :
+		Optimizer(parameters),
+		pixelVariance_(Parameters::defaultOptimizerPixelVariance()),
+		disparityVariance_(Parameters::defaultOptimizerDisparityVariance()),
+		robustKernelDelta_(Parameters::defaultOptimizerRobustKernelDelta()),
+		baseline_(Parameters::defaultOptimizerBaseline())
+{
+	parseParameters(parameters);
+}
+
+void OptimizerCeres::parseParameters(const ParametersMap & parameters)
+{
+	Optimizer::parseParameters(parameters);
+	Parameters::parse(parameters, Parameters::kOptimizerPixelVariance(),     pixelVariance_);
+	Parameters::parse(parameters, Parameters::kOptimizerDisparityVariance(), disparityVariance_);
+	Parameters::parse(parameters, Parameters::kOptimizerRobustKernelDelta(), robustKernelDelta_);
+	Parameters::parse(parameters, Parameters::kOptimizerBaseline(),          baseline_);
+	UASSERT(pixelVariance_ > 0.0);
+	UASSERT(disparityVariance_ > 0.0);
+	UASSERT(baseline_ >= 0.0);
+}
+
 std::map<int, Transform> OptimizerCeres::optimize(
 		int rootId,
 		const std::map<int, Transform> & poses,
@@ -178,7 +203,7 @@ std::map<int, Transform> OptimizerCeres::optimize(
 					}
 
 					float yaw_radians = ceres::examples::NormalizeAngle(iter->second.transform().theta());
-					const Eigen::Matrix3d sqrt_information = information.llt().matrixL();
+					const Eigen::Matrix3d sqrt_information = information.llt().matrixU();
 
 					// Ceres will take ownership of the pointer.
 					ceres::CostFunction* cost_function = ceres::examples::PoseGraph2dErrorTerm::Create(
@@ -219,7 +244,7 @@ std::map<int, Transform> OptimizerCeres::optimize(
 					t.p.z() = iter->second.transform().z();
 					t.q = iter->second.transform().getQuaterniond();
 
-					const Eigen::Matrix<double, 6, 6> sqrt_information = information.llt().matrixL();
+					const Eigen::Matrix<double, 6, 6> sqrt_information = information.llt().matrixU();
 					// Ceres will take ownership of the pointer.
 					ceres::CostFunction* cost_function = ceres::examples::PoseGraph3dErrorTerm::Create(t, sqrt_information);
 					problem.AddResidualBlock(cost_function, loss_function,
@@ -441,6 +466,12 @@ std::map<int, Transform> OptimizerCeres::optimizeBA(
 	}
 	UASSERT(oi == baProblem.num_points_*3);
 
+	// Per-observation stereo metadata. For mono observations
+	// observed_disparity[i] = 0 and baseline_fx[i] = 0 -- the second-loop
+	// branch picks the mono cost function in that case.
+	std::vector<double> observed_disparity(baProblem.num_observations_, 0.0);
+	std::vector<double> baseline_fx(baProblem.num_observations_, 0.0);
+
 	oi = 0;
 	for(std::map<int, std::map<int, FeatureBA> >::const_iterator iter=wordReferences.begin();
 		iter!=wordReferences.end();
@@ -465,6 +496,25 @@ std::map<int, Transform> OptimizerCeres::optimizeBA(
 			baProblem.observations_[4*oi+1] = jter->second.kpt.pt.y - iterModel->second[0].cy();
 			baProblem.observations_[4*oi+2] = iterModel->second[0].fx();
 			baProblem.observations_[4*oi+3] = iterModel->second[0].fy();
+
+			// Stereo path: if a baseline is encoded in the camera model
+			// (Tx<0, the rtabmap convention) AND we have a finite positive
+			// depth from the observation, derive the observed disparity
+			// and cache baseline*fx for the cost function. For RGB-D /
+			// mono-with-depth (Tx==0) we fall back on the configurable
+			// Optimizer/Baseline -- a "fake baseline" that lets BA treat
+			// depth observations as stereo disparity. depth==0 or
+			// effective baseline==0 -> mono observation; the second loop
+			// will pick SnavelyReprojectionError instead.
+			const double Tx    = iterModel->second[0].Tx();
+			const double fx    = iterModel->second[0].fx();
+			const double depth = jter->second.depth;
+			const double baseline = Tx < 0.0 ? (-Tx / fx) : baseline_;
+			if(baseline > 0.0 && uIsFinite(depth) && depth > 0.0)
+			{
+				baseline_fx[oi]        = baseline * fx;
+				observed_disparity[oi] = baseline_fx[oi] / depth;
+			}
 			++oi;
 		}
 	}
@@ -476,22 +526,161 @@ std::map<int, Transform> OptimizerCeres::optimizeBA(
 	// parameters for cameras and points are added automatically.
 	ceres::Problem problem;
 
+	// Per-axis weighting mirrors g2o's stereo information matrix:
+	// 1/pixelVariance on u/v, 1/disparityVariance on disparity. Pure
+	// loop-invariant, hoisted out.
+	const double inv_sigma_uv = 1.0 / std::sqrt(pixelVariance_);
+	const double inv_sigma_d  = 1.0 / std::sqrt(disparityVariance_);
+	int monoObsCount   = 0;
+	int stereoObsCount = 0;
 	for (int i = 0; i < baProblem.num_observations(); ++i) {
-		// Each Residual block takes a point and a camera as input and outputs a 2
-		// dimensional residual. Internally, the cost function stores the observed
-		// image location and compares the reprojection against the observation.
-		ceres::CostFunction* cost_function =
-			ceres::SnavelyReprojectionError::Create(
-					observations[4 * i],      //u
-					observations[4 * i + 1],  //v
-					observations[4 * i + 2],  //fx
-					observations[4 * i + 3]); //fy
-		ceres::LossFunction* loss_function = new ceres::HuberLoss(8.0);
+		const double u  = observations[4 * i];
+		const double v  = observations[4 * i + 1];
+		const double fx = observations[4 * i + 2];
+		const double fy = observations[4 * i + 3];
+
+		ceres::CostFunction* cost_function = 0;
+		if(baseline_fx[i] > 0.0 && observed_disparity[i] > 0.0)
+		{
+			// Stereo (3 residuals: u, v, disparity). The disparity channel
+			// pins z relative to the observing camera, which collapses the
+			// mono BA gauge from 7 DOF to 6 (scale becomes observable).
+			cost_function = ceres::SnavelyStereoReprojectionError::Create(
+					u, v, observed_disparity[i], fx, fy, baseline_fx[i],
+					inv_sigma_uv, inv_sigma_d);
+			++stereoObsCount;
+		}
+		else
+		{
+			// Mono (2 residuals: u, v).
+			cost_function = ceres::SnavelyReprojectionError::Create(u, v, fx, fy);
+			++monoObsCount;
+		}
+		// Pass nullptr when robustKernelDelta_ <= 0 -- Ceres treats that as
+		// identity (no kernel). A new loss instance per block is required:
+		// Ceres takes ownership and deletes each.
+		ceres::LossFunction* loss_function =
+				robustKernelDelta_ > 0.0 ? new ceres::HuberLoss(robustKernelDelta_) : nullptr;
 		problem.AddResidualBlock(cost_function,
 								 loss_function,
 								 baProblem.mutable_camera_for_observation(i),
 								 baProblem.mutable_point_for_observation(i));
 	}
+	UDEBUG("Ceres BA: %d mono + %d stereo observations", monoObsCount, stereoObsCount);
+
+	// Pose-graph constraints (kNeighbor / etc.) between cameras. Same role
+	// as the EdgeSBACam edges in OptimizerG2O and the BetweenFactor<Pose3>
+	// factors in OptimizerGTSAM -- folds the relative-pose chain into the
+	// BA cost so chains pulled by odometry don't drift freely.
+	int linkObsCount = 0;
+	for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
+	{
+		const Link & link = iter->second;
+		if(link.from() <= 0 || link.to() <= 0 || link.from() == link.to())
+		{
+			continue;
+		}
+		std::map<int, int>::const_iterator itA = camIdToIndex.find(link.from());
+		std::map<int, int>::const_iterator itB = camIdToIndex.find(link.to());
+		if(itA == camIdToIndex.end() || itB == camIdToIndex.end())
+		{
+			continue;
+		}
+		// Convert link from body-to-body into camera-to-camera using the
+		// localTransforms of both endpoints (same idiom as g2o / GTSAM).
+		const Transform camLink = models.at(link.from())[0].localTransform().inverse() *
+		                          link.transform() *
+		                          models.at(link.to())[0].localTransform();
+
+		// Decompose camLink (cam_b in cam_a's frame) into (angle-axis, translation).
+		const cv::Mat R = (cv::Mat_<double>(3, 3) <<
+				(double)camLink.r11(), (double)camLink.r12(), (double)camLink.r13(),
+				(double)camLink.r21(), (double)camLink.r22(), (double)camLink.r23(),
+				(double)camLink.r31(), (double)camLink.r32(), (double)camLink.r33());
+		cv::Mat rvec(1, 3, CV_64FC1);
+		cv::Rodrigues(R, rvec);
+		const Eigen::Vector3d aa_meas(rvec.at<double>(0,0), rvec.at<double>(0,1), rvec.at<double>(0,2));
+		const Eigen::Vector3d t_meas(camLink.x(), camLink.y(), camLink.z());
+
+		// Information matrix from the link covariance. rtabmap stores it as
+		// [linear|angular] -- same axis order as our residual [t; rot], so
+		// no block swap is needed (unlike the GTSAM path which expects
+		// [angular|linear]). cv::Mat is row-major; copying into Eigen
+		// (column-major) effectively transposes, which is a no-op for the
+		// symmetric info matrix.
+		Eigen::Matrix<double, 6, 6> information = Eigen::Matrix<double, 6, 6>::Identity();
+		if(!isCovarianceIgnored())
+		{
+			memcpy(information.data(), link.infMatrix().data, link.infMatrix().total()*sizeof(double));
+		}
+		// Whitening matrix: we want sqrt_info such that
+		// sqrt_info^T * sqrt_info = info. LLT gives info = L*L^T with L lower
+		// triangular, so the upper-triangular U = L^T (matrixU()) satisfies
+		// U^T*U = info. Then ||U*r||² = r^T*info*r as desired.
+		const Eigen::Matrix<double, 6, 6> sqrt_info =
+				information.llt().matrixU();
+
+		ceres::CostFunction * cost = ceres::BetweenCamerasError::Create(t_meas, aa_meas, sqrt_info);
+		// No robust kernel on between-camera constraints (matches g2o/GTSAM:
+		// only projection edges carry the Huber kernel in BA).
+		problem.AddResidualBlock(cost,
+				nullptr,
+				baProblem.cameras_ + itA->second * 6,
+				baProblem.cameras_ + itB->second * 6);
+		++linkObsCount;
+	}
+	if(linkObsCount > 0)
+	{
+		UDEBUG("Ceres BA: %d pose-graph links", linkObsCount);
+	}
+
+	// 2D / planar BA mode: lock each non-root camera to its initial body-z
+	// (lateral motion + yaw stay free). Root pose is fixed entirely so the
+	// gauge has no remaining z-DOF. Mirrors the g2o EdgeSBACamPrior path.
+	if(isSlam2d())
+	{
+		const double sqrtInfo = std::sqrt(1e9);  // matches g2o pinfo(2,2) = 1e9
+		int planarObsCount = 0;
+		for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
+		{
+			std::map<int, int>::const_iterator camIt = camIdToIndex.find(iter->first);
+			if(camIt == camIdToIndex.end())
+			{
+				continue;
+			}
+			double * cam_block = baProblem.cameras_ + camIt->second * 6;
+			const bool fixNode = (rootId >= 0 && iter->first == rootId) ||
+			                     (rootId <  0 && iter->first != -rootId);
+			if(fixNode)
+			{
+				problem.SetParameterBlockConstant(cam_block);
+				continue;
+			}
+			// Unary planar constraint on the BODY z (the camera vertex is in
+			// world-to-camera; we extract body z by composing with the
+			// inverse localTransform inside the cost function).
+			std::map<int, std::vector<CameraModel> >::const_iterator iterModel = models.find(iter->first);
+			if(iterModel == models.end() || iterModel->second.empty())
+			{
+				continue;
+			}
+			const Transform & localTransform = iterModel->second[0].localTransform();
+			const Eigen::Matrix3d R_bc = (Eigen::Matrix3d() <<
+					(double)localTransform.r11(), (double)localTransform.r12(), (double)localTransform.r13(),
+					(double)localTransform.r21(), (double)localTransform.r22(), (double)localTransform.r23(),
+					(double)localTransform.r31(), (double)localTransform.r32(), (double)localTransform.r33()).finished();
+			const Eigen::Vector3d t_bc(localTransform.x(), localTransform.y(), localTransform.z());
+
+			ceres::CostFunction * planar = ceres::PlanarConstraintError::Create(
+					R_bc, t_bc, iter->second.z(), sqrtInfo);
+			problem.AddResidualBlock(planar, nullptr, cam_block);
+			++planarObsCount;
+		}
+		if(planarObsCount > 0)
+		{
+			UDEBUG("Ceres BA: %d planar-constraint blocks (2D mode)", planarObsCount);
+		}
+	}
 
 	// SBA
 	// Make Ceres automatically detect the bundle structure. Note that the
@@ -502,7 +691,14 @@ std::map<int, Transform> OptimizerCeres::optimizeBA(
 	options.sparse_linear_algebra_library_type = ceres::SUITE_SPARSE;
 	options.max_num_iterations = iterations();
 	//options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;
-	options.function_tolerance = this->epsilon();
+	options.function_tolerance  = this->epsilon();
+	// Force the LM to run to max_num_iterations rather than stopping early
+	// on Ceres' default parameter/gradient tolerances. Matters in particular
+	// for high-weight unary constraints (e.g. the 2D planar lock at sqrt(1e9))
+	// whose residual can stall the parameter step below the default 1e-8
+	// before the constraint is fully satisfied.
+	options.parameter_tolerance = 0.0;
+	options.gradient_tolerance  = 0.0;
 	ceres::Solver::Summary summary;
 	ceres::Solve(options, &problem, &summary);
 	if(ULogger::level() == ULogger::kDebug)
@@ -534,9 +730,26 @@ std::map<int, Transform> OptimizerCeres::optimizeBA(
 
 		if(this->isSlam2d())
 		{
-			t = (models.at(iter->first)[0].localTransform() * t).inverse();
-			t = iter->second.inverse() * t;
-			iter->second *= t.to3DoF();
+			// Body pose recovered by BA (with the planar constraint that locks
+			// each non-root body z to its initial value). If the constraint held,
+			// z is already within ~mm of the initial; snap it back exactly.
+			// Otherwise the optimizer's planar lock didn't bite -- fall back to
+			// projecting the BA delta onto SE(2) and applying it to the initial.
+			Transform body = (models.at(iter->first)[0].localTransform() * t).inverse();
+			if(std::fabs(body.z() - iter->second.z()) < 0.001f)
+			{
+				body.z() = iter->second.z();
+				iter->second = body;
+			}
+			else
+			{
+				UWARN("Planar constraints didn't work!? original pose (%d), pose %s -> %s. Falling back to old approach.",
+						iter->first,
+						iter->second.prettyPrint().c_str(),
+						body.prettyPrint().c_str());
+				const Transform delta = iter->second.inverse() * body;
+				iter->second = iter->second * delta.to3DoF();
+			}
 		}
 		else
 		{

corelib/src/optimizer/OptimizerG2O.cpp

@@ -160,10 +160,10 @@ OptimizerG2O::OptimizerG2O(const ParametersMap & parameters) :
 		Optimizer(parameters),
 		solver_(Parameters::defaultg2oSolver()),
 		optimizer_(Parameters::defaultg2oOptimizer()),
-		pixelVariance_(Parameters::defaultg2oPixelVariance()),
-		disparityVariance_(Parameters::defaultg2oDisparityVariance()),
-		robustKernelDelta_(Parameters::defaultg2oRobustKernelDelta()),
-		baseline_(Parameters::defaultg2oBaseline())
+		pixelVariance_(Parameters::defaultOptimizerPixelVariance()),
+		disparityVariance_(Parameters::defaultOptimizerDisparityVariance()),
+		robustKernelDelta_(Parameters::defaultOptimizerRobustKernelDelta()),
+		baseline_(Parameters::defaultOptimizerBaseline())
 {
 #ifdef RTABMAP_G2O
 	// Issue on android, have to explicitly register this type when using fixed root prior below
@@ -185,10 +185,10 @@ void OptimizerG2O::parseParameters(const ParametersMap & parameters)
 
 	Parameters::parse(parameters, Parameters::kg2oSolver(), solver_);
 	Parameters::parse(parameters, Parameters::kg2oOptimizer(), optimizer_);
-	Parameters::parse(parameters, Parameters::kg2oPixelVariance(), pixelVariance_);
-	Parameters::parse(parameters, Parameters::kg2oDisparityVariance(), disparityVariance_);
-	Parameters::parse(parameters, Parameters::kg2oRobustKernelDelta(), robustKernelDelta_);
-	Parameters::parse(parameters, Parameters::kg2oBaseline(), baseline_);
+	Parameters::parse(parameters, Parameters::kOptimizerPixelVariance(), pixelVariance_);
+	Parameters::parse(parameters, Parameters::kOptimizerDisparityVariance(), disparityVariance_);
+	Parameters::parse(parameters, Parameters::kOptimizerRobustKernelDelta(), robustKernelDelta_);
+	Parameters::parse(parameters, Parameters::kOptimizerBaseline(), baseline_);
 	UASSERT(pixelVariance_ > 0.0);
 	UASSERT(disparityVariance_ > 0.0);
 	UASSERT(baseline_ >= 0.0);

corelib/src/optimizer/OptimizerGTSAM.cpp

@@ -31,6 +31,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/utilite/UMath.h>
 #include <rtabmap/utilite/UConversion.h>
 #include <rtabmap/utilite/UTimer.h>
+#include <rtabmap/core/util3d.h>
 #include <set>
 
 #include <rtabmap/core/optimizer/OptimizerGTSAM.h>
@@ -38,10 +39,15 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #ifdef RTABMAP_GTSAM
 #include <gtsam/geometry/Pose2.h>
 #include <gtsam/geometry/Pose3.h>
+#include <gtsam/geometry/Cal3_S2.h>
+#include <gtsam/geometry/Cal3_S2Stereo.h>
+#include <gtsam/geometry/StereoPoint2.h>
 #include <gtsam/inference/Key.h>
 #include <gtsam/inference/Symbol.h>
 #include <gtsam/slam/PriorFactor.h>
 #include <gtsam/slam/BetweenFactor.h>
+#include <gtsam/slam/ProjectionFactor.h>
+#include <gtsam/slam/StereoFactor.h>
 #include <gtsam/sam/BearingFactor.h>
 #include <gtsam/sam/BearingRangeFactor.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
@@ -51,6 +57,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <gtsam/nonlinear/NonlinearOptimizer.h>
 #include <gtsam/nonlinear/Marginals.h>
 #include <gtsam/nonlinear/Values.h>
+#include <gtsam/base/numericalDerivative.h>
 #include <gtsam/navigation/AttitudeFactor.h>
 #include <optimizer/gtsam/XYFactor.h>
 #include <optimizer/gtsam/XYZFactor.h>
@@ -67,6 +74,10 @@ namespace rtabmap {
 OptimizerGTSAM::OptimizerGTSAM(const ParametersMap & parameters) :
 	Optimizer(parameters),
 	internalOptimizerType_(Parameters::defaultGTSAMOptimizer()),
+	pixelVariance_(Parameters::defaultOptimizerPixelVariance()),
+	disparityVariance_(Parameters::defaultOptimizerDisparityVariance()),
+	robustKernelDelta_(Parameters::defaultOptimizerRobustKernelDelta()),
+	baseline_(Parameters::defaultOptimizerBaseline()),
 	isam2_(0),
 	lastSwitchId_(1000000000)
 {
@@ -95,6 +106,13 @@ void OptimizerGTSAM::parseParameters(const ParametersMap & parameters)
 	Optimizer::parseParameters(parameters);
 #ifdef RTABMAP_GTSAM
 	Parameters::parse(parameters, Parameters::kGTSAMOptimizer(), internalOptimizerType_);
+	Parameters::parse(parameters, Parameters::kOptimizerPixelVariance(), pixelVariance_);
+	Parameters::parse(parameters, Parameters::kOptimizerDisparityVariance(), disparityVariance_);
+	Parameters::parse(parameters, Parameters::kOptimizerRobustKernelDelta(), robustKernelDelta_);
+	Parameters::parse(parameters, Parameters::kOptimizerBaseline(), baseline_);
+	UASSERT(pixelVariance_ > 0.0);
+	UASSERT(disparityVariance_ > 0.0);
+	UASSERT(baseline_ >= 0.0);
 
 	bool incremental = isam2_;
 	double threshold = Parameters::defaultGTSAMIncRelinearizeThreshold();
@@ -1118,4 +1136,439 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 	return optimizedPoses;
 }
 
+// Multi-camera offset: same convention as OptimizerG2O.cpp so per-rig camera
+// vertex keys stay disjoint from pose keys (max 10 cameras per pose).
+#define GTSAM_BA_MULTICAM_OFFSET 10
+
+namespace {
+
+// Unary planar constraint mirroring g2o's EdgeSBACamPrior (pinfo(2,2) = 1e9):
+// locks the BODY-frame z of the camera vertex to its initial value, leaving
+// lateral motion + yaw free. Used when isSlam2d() is true in BA.
+class PlanarBodyZFactor : public gtsam::NoiseModelFactor1<gtsam::Pose3>
+{
+public:
+	PlanarBodyZFactor(gtsam::Key key,
+	                  const gtsam::Pose3 & camera_to_body,
+	                  double initial_body_z,
+	                  const gtsam::SharedNoiseModel & model)
+		: gtsam::NoiseModelFactor1<gtsam::Pose3>(model, key),
+		  camera_to_body_(camera_to_body),
+		  initial_body_z_(initial_body_z) {}
+
+	gtsam::Vector evaluateError(const gtsam::Pose3 & camPose,
+#if GTSAM_VERSION_NUMERIC >= 40300
+	                            gtsam::OptionalMatrixType H = OptionalNone) const override
+#else
+	                            boost::optional<gtsam::Matrix &> H = boost::none) const override
+#endif
+	{
+		const auto error_fn = [this](const gtsam::Pose3 & p) {
+			gtsam::Vector1 e;
+			e(0) = p.compose(camera_to_body_).translation().z() - initial_body_z_;
+			return e;
+		};
+		if(H)
+		{
+			*H = gtsam::numericalDerivative11<gtsam::Vector1, gtsam::Pose3>(error_fn, camPose);
+		}
+		return error_fn(camPose);
+	}
+
+private:
+	gtsam::Pose3 camera_to_body_;
+	double initial_body_z_;
+};
+
+}  // namespace
+
+std::map<int, Transform> OptimizerGTSAM::optimizeBA(
+		int rootId,
+		const std::map<int, Transform> & poses,
+		const std::multimap<int, Link> & links,
+		const std::map<int, std::vector<CameraModel> > & models,
+		std::map<int, cv::Point3f> & points3DMap,
+		const std::map<int, std::map<int, FeatureBA> > & wordReferences,
+		std::set<int> * outliers)
+{
+	std::map<int, Transform> optimizedPoses;
+#ifdef RTABMAP_GTSAM
+	UDEBUG("Optimizing BA graph...");
+
+	if(!(poses.size() >= 2 && iterations() > 0 && (models.size() == poses.size() || poses.begin()->first < 0)))
+	{
+		UWARN("GTSAM BA: nothing to optimize (poses=%d models=%d iterations=%d)",
+				(int)poses.size(), (int)models.size(), iterations());
+		return optimizedPoses;
+	}
+
+	gtsam::NonlinearFactorGraph graph;
+	gtsam::Values initial;
+
+	// Cache per-frame, per-camera intrinsics. Note that GTSAM's
+	// GenericProjectionFactor/GenericStereoFactor hold a shared_ptr to the
+	// calibration -- we have to keep these alive for the lifetime of the
+	// graph, hence storing them by map.
+	std::map<std::pair<int,int>, gtsam::Cal3_S2::shared_ptr>       calMono;
+	std::map<std::pair<int,int>, gtsam::Cal3_S2Stereo::shared_ptr> calStereo;
+	std::map<std::pair<int,int>, double>                           baselineByCam;
+
+	// 1) Add pose variables (in CAMERA frame: pose * localTransform).
+	UDEBUG("GTSAM BA: adding %d poses... (rootId=%d)", (int)poses.size(), rootId);
+	for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
+	{
+		if(iter->first <= 0)
+		{
+			continue;
+		}
+		std::map<int, std::vector<CameraModel> >::const_iterator iterModel = models.find(iter->first);
+		if(iterModel == models.end() || iterModel->second.empty())
+		{
+			UERROR("GTSAM BA: missing camera model for pose %d", iter->first);
+			return optimizedPoses;
+		}
+		for(size_t i=0; i<iterModel->second.size(); ++i)
+		{
+			const CameraModel & m = iterModel->second[i];
+			if(!m.isValidForProjection())
+			{
+				UERROR("GTSAM BA: model %d.%d is invalid for projection", iter->first, (int)i);
+				return optimizedPoses;
+			}
+			const Transform camPose = iter->second * m.localTransform();
+			if(camPose.isNull())
+			{
+				UERROR("GTSAM BA: null camera pose for %d.%d", iter->first, (int)i);
+				return optimizedPoses;
+			}
+			const gtsam::Symbol xkey('x', iter->first * GTSAM_BA_MULTICAM_OFFSET + (int)i);
+			initial.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()));
+			calMono[std::make_pair(iter->first, (int)i)] = K;
+			const double baseline = m.Tx() < 0.0 ? (-m.Tx() / m.fx()) : baseline_;
+			if(baseline > 0.0)
+			{
+				gtsam::Cal3_S2Stereo::shared_ptr Ks(new gtsam::Cal3_S2Stereo(m.fx(), m.fy(), 0.0, m.cx(), m.cy(), baseline));
+				calStereo[std::make_pair(iter->first, (int)i)]     = Ks;
+				baselineByCam[std::make_pair(iter->first, (int)i)] = baseline;
+			}
+
+			// Fix the root pose (or fix everyone else if rootId<0). GTSAM has
+			// no equivalent of g2o's setFixed(); the standard idiom is a
+			// near-zero-sigma prior on each axis. We add this only to the
+			// primary camera (i==0) of a multi-cam rig -- the others are
+			// rigidly linked via the multi-cam BetweenFactors below.
+			const bool fixNode = (rootId >= 0 && iter->first == rootId) ||
+			                     (rootId <  0 && iter->first != -rootId);
+			if(fixNode && i == 0)
+			{
+				gtsam::noiseModel::Diagonal::shared_ptr priorNoise =
+						gtsam::noiseModel::Diagonal::Sigmas(
+								(gtsam::Vector(6) << 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9).finished());
+				graph.add(gtsam::PriorFactor<gtsam::Pose3>(xkey, gtsam::Pose3(camPose.toEigen4d()), priorNoise));
+			}
+			else if(isSlam2d() && i == 0)
+			{
+				// 2D / planar BA: lock the body-frame z of each non-root
+				// camera to its initial value (mirrors g2o's EdgeSBACamPrior
+				// with pinfo(2,2) = 1e9). Lateral motion and yaw stay free.
+				const gtsam::Pose3 cam_to_body(m.localTransform().inverse().toEigen4d());
+				gtsam::SharedNoiseModel planarNoise =
+						gtsam::noiseModel::Isotropic::Sigma(1, std::sqrt(1.0 / 1e9));
+				graph.add(PlanarBodyZFactor(xkey, cam_to_body, iter->second.z(), planarNoise));
+			}
+		}
+	}
+
+	// 2) Pose-graph BetweenFactors (same role as the g2o EdgeSBACam edges).
+	//    Expressed in camera frame: cam_from^{-1} * world * cam_to where
+	//    cam = body * localTransform.
+	UDEBUG("GTSAM BA: adding %d links...", (int)links.size());
+	for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
+	{
+		const Link & link = iter->second;
+		if(link.from() <= 0 || link.to() <= 0)
+		{
+			continue;
+		}
+		if(link.from() == link.to())
+		{
+			continue;
+		}
+		if(!uContains(poses, link.from()) || !uContains(poses, link.to()))
+		{
+			continue;
+		}
+		UASSERT(!link.transform().isNull());
+
+		const Transform camLink = models.at(link.from())[0].localTransform().inverse() *
+		                          link.transform() *
+		                          models.at(link.to())[0].localTransform();
+
+		Eigen::Matrix<double, 6, 6> information = Eigen::Matrix<double, 6, 6>::Identity();
+		if(!isCovarianceIgnored())
+		{
+			memcpy(information.data(), link.infMatrix().data, link.infMatrix().total()*sizeof(double));
+		}
+		// rtabmap's covariance/information convention is [linear|angular];
+		// GTSAM expects [angular|linear]. Swap the blocks.
+		Eigen::Matrix<double, 6, 6> mgtsam;
+		mgtsam.block<3,3>(0,0) = information.block<3,3>(3,3); // rotation
+		mgtsam.block<3,3>(3,3) = information.block<3,3>(0,0); // translation
+		mgtsam.block<3,3>(0,3) = information.block<3,3>(3,0);
+		mgtsam.block<3,3>(3,0) = information.block<3,3>(0,3);
+		gtsam::SharedNoiseModel noise = gtsam::noiseModel::Gaussian::Information(mgtsam);
+
+		graph.add(gtsam::BetweenFactor<gtsam::Pose3>(
+				gtsam::Symbol('x', link.from() * GTSAM_BA_MULTICAM_OFFSET),
+				gtsam::Symbol('x', link.to()   * GTSAM_BA_MULTICAM_OFFSET),
+				gtsam::Pose3(camLink.toEigen4d()),
+				noise));
+	}
+
+	// 3) Hard rigid edges between camera 0 and the other cameras of a
+	//    multi-cam rig (g2o uses Identity*1e7; we mirror that here).
+	for(std::map<int, std::vector<CameraModel> >::const_iterator iter=models.begin(); iter!=models.end(); ++iter)
+	{
+		if(!uContains(poses, iter->first))
+		{
+			continue;
+		}
+		for(size_t i=1; i<iter->second.size(); ++i)
+		{
+			const Transform camLink = iter->second[0].localTransform().inverse() * iter->second[i].localTransform();
+			Eigen::Matrix<double, 6, 6> information = Eigen::Matrix<double, 6, 6>::Identity() * 9999999.0;
+			gtsam::SharedNoiseModel noise = gtsam::noiseModel::Gaussian::Information(information);
+			graph.add(gtsam::BetweenFactor<gtsam::Pose3>(
+					gtsam::Symbol('x',  iter->first * GTSAM_BA_MULTICAM_OFFSET),
+					gtsam::Symbol('x',  iter->first * GTSAM_BA_MULTICAM_OFFSET + (int)i),
+					gtsam::Pose3(camLink.toEigen4d()),
+					noise));
+		}
+	}
+
+	// 4) 3D points + reprojection observations.
+	UDEBUG("GTSAM BA: adding %d 3D points and observations...", (int)points3DMap.size());
+	std::set<gtsam::Key> insertedPoints;
+	// Track factor->word mapping so the post-optimization residual sweep can
+	// report which observations went over the robust-kernel threshold.
+	std::vector<std::pair<size_t /*factorIndex*/, int /*wordId*/> > obsFactors;
+
+	// Build the per-axis noise models once (loop-invariant). Stereo: per-axis
+	// sigmas matching the g2o stereo path. StereoPoint2 is (uL, uR, v); uR =
+	// uL - disparity. uL and v carry pixel-detector noise, uR carries
+	// disparity-channel noise (matches the g2o stereo edge's (u, v, u-disp)
+	// interpretation up to a covariance rotation that is fine for typical
+	// small sigmas). The robust-Huber wrapping is also invariant.
+	const double sigmaPixel     = std::sqrt(pixelVariance_);
+	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);
+	if(robustKernelDelta_ > 0.0)
+	{
+		gtsam::noiseModel::mEstimator::Base::shared_ptr huber =
+				gtsam::noiseModel::mEstimator::Huber::Create(robustKernelDelta_);
+		stereoNoiseModel = gtsam::noiseModel::Robust::Create(huber, stereoNoiseModel);
+		monoNoiseModel   = gtsam::noiseModel::Robust::Create(huber, monoNoiseModel);
+	}
+	for(std::map<int, std::map<int, FeatureBA> >::const_iterator iter = wordReferences.begin(); iter!=wordReferences.end(); ++iter)
+	{
+		const int wordId = iter->first;
+		if(points3DMap.find(wordId) == points3DMap.end())
+		{
+			continue;
+		}
+		const cv::Point3f pt3d = points3DMap.at(wordId);
+		if(!util3d::isFinite(pt3d))
+		{
+			UWARN("Ignoring 3D point %d because it has nan value(s)!", wordId);
+			continue;
+		}
+		const gtsam::Symbol pkey('l', wordId);
+		initial.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)
+		{
+			const int poseId  = jter->first;
+			const int camIdx  = jter->second.cameraIndex;
+			const FeatureBA & f = jter->second;
+			if(poses.find(poseId) == poses.end())
+			{
+				continue;
+			}
+			const std::pair<int,int> camKey(poseId, camIdx);
+			if(calMono.find(camKey) == calMono.end())
+			{
+				continue;
+			}
+			const gtsam::Symbol xkey('x', poseId * GTSAM_BA_MULTICAM_OFFSET + camIdx);
+			if(!initial.exists(xkey))
+			{
+				continue;
+			}
+
+			const double depth    = f.depth;
+			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(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);
+				graph.add(gtsam::GenericStereoFactor<gtsam::Pose3, gtsam::Point3>(
+						obs, stereoNoiseModel, xkey, pkey, Ks));
+			}
+			else
+			{
+				if(baseline > 0.0)
+				{
+					UDEBUG("Stereo cam detected but observation (word=%d cam=%d.%d) has null depth (%f m), adding mono observation instead.",
+							wordId, poseId, camIdx, depth);
+				}
+				const gtsam::Cal3_S2::shared_ptr & K = calMono.at(camKey);
+				const gtsam::Point2 obs(f.kpt.pt.x, f.kpt.pt.y);
+				graph.add(gtsam::GenericProjectionFactor<gtsam::Pose3, gtsam::Point3, gtsam::Cal3_S2>(
+						obs, monoNoiseModel, xkey, pkey, K));
+			}
+			obsFactors.push_back(std::make_pair(factorIdx, wordId));
+		}
+	}
+
+	// 5) Optimize.
+	UTimer timer;
+	gtsam::Values result;
+	double finalError = std::numeric_limits<double>::quiet_NaN();
+	try
+	{
+		// Always use Levenberg-Marquardt for BA, ignoring GTSAM/Optimizer.
+		// Same rationale as the g2o BA path: BA's Hessian is often
+		// near-singular (points near infinity, near-parallel rays), so
+		// 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;
+		params.absoluteErrorTol = tol;
+		params.maxIterations    = iterations();
+		gtsam::NonlinearOptimizer * optimizer = new gtsam::LevenbergMarquardtOptimizer(graph, initial, 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());
+		delete optimizer;
+	}
+	catch(const gtsam::IndeterminantLinearSystemException & e)
+	{
+		UERROR("GTSAM BA: indeterminant linear system: %s", e.what());
+		return optimizedPoses;
+	}
+	catch(const std::exception & e)
+	{
+		UERROR("GTSAM BA failed: %s", e.what());
+		return optimizedPoses;
+	}
+
+	if(uIsNan(finalError))
+	{
+		UERROR("GTSAM BA produced a NaN error.");
+		return optimizedPoses;
+	}
+
+	// 6) Report observations whose per-factor residual exceeded the robust
+	//    kernel delta. Unlike g2o we don't re-optimize without them -- the
+	//    Huber kernel has already down-weighted them in the solve.
+	if(outliers && robustKernelDelta_ > 0.0)
+	{
+		const double thresholdSq = robustKernelDelta_ * robustKernelDelta_;
+		for(std::vector<std::pair<size_t, int> >::const_iterator iter = obsFactors.begin(); iter != obsFactors.end(); ++iter)
+		{
+			if(iter->first >= graph.size()) continue;
+			const double e = graph.at(iter->first)->error(result);
+			// GTSAM returns 0.5 * r^T * Σ^{-1} * r; multiply by 2 to get chi^2.
+			if(2.0 * e > thresholdSq)
+			{
+				outliers->insert(iter->second);
+			}
+		}
+		UDEBUG("GTSAM BA: %d outlier observations flagged.", (int)outliers->size());
+	}
+
+	// 7) Read back poses (camera frame -> body frame via localTransform^-1).
+	for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
+	{
+		if(iter->first <= 0)
+		{
+			continue;
+		}
+		const gtsam::Symbol xkey('x', iter->first * GTSAM_BA_MULTICAM_OFFSET);
+		if(!result.exists(xkey))
+		{
+			continue;
+		}
+		Transform t = Transform::fromEigen4d(result.at<gtsam::Pose3>(xkey).matrix());
+		t *= models.at(iter->first)[0].localTransform().inverse();
+		if(t.isNull())
+		{
+			UERROR("GTSAM BA: optimized pose %d is null", iter->first);
+			optimizedPoses.clear();
+			return optimizedPoses;
+		}
+		if(isSlam2d())
+		{
+			// Same snap-back idiom as g2o / Ceres: PlanarBodyZFactor locks
+			// each non-root body z to its initial value, but tiny LM-residual
+			// slack can still leave a sub-mm drift. Snap z back exactly when
+			// within tolerance; fall back to a 2D-projected delta otherwise.
+			if(std::fabs(t.z() - iter->second.z()) < 0.001f)
+			{
+				t.z() = iter->second.z();
+			}
+			else
+			{
+				UWARN("Planar constraints didn't work!? original pose (%d), pose %s -> %s. Falling back to old approach.",
+						iter->first,
+						iter->second.prettyPrint().c_str(),
+						t.prettyPrint().c_str());
+				const Transform delta = iter->second.inverse() * t;
+				t = iter->second * delta.to3DoF();
+			}
+		}
+		optimizedPoses.insert(std::make_pair(iter->first, t));
+	}
+
+	// 8) Read back 3D points.
+	for(std::map<int, cv::Point3f>::iterator iter = points3DMap.begin(); iter != points3DMap.end(); ++iter)
+	{
+		const gtsam::Symbol pkey('l', iter->first);
+		if(insertedPoints.count(pkey) && result.exists(pkey))
+		{
+			const gtsam::Point3 p = result.at<gtsam::Point3>(pkey);
+			iter->second = cv::Point3f(static_cast<float>(p.x()), static_cast<float>(p.y()), static_cast<float>(p.z()));
+		}
+	}
+
+#else
+	UERROR("Not built with GTSAM support!");
+	(void)rootId;
+	(void)poses;
+	(void)links;
+	(void)models;
+	(void)points3DMap;
+	(void)wordReferences;
+	(void)outliers;
+#endif
+	return optimizedPoses;
+}
+
 } /* namespace rtabmap */

corelib/src/optimizer/ceres/bundle/between_cameras_error.h

@@ -0,0 +1,114 @@
+/*
+ * between_cameras_error.h
+ *
+ * Pose-graph constraint between two BA camera variables. Used to fold
+ * relative-pose link measurements (kNeighbor edges) into the BA cost so
+ * Ceres BA can use the same pose-graph chain that g2o (EdgeSBACam) and
+ * GTSAM (BetweenFactor<Pose3>) include in their BA paths.
+ *
+ * Camera parameterization matches SnavelyReprojectionError: 6 doubles
+ * per camera [angle_axis (3), translation (3)] in world-to-camera
+ * convention (a point X_world is mapped to X_cam = R*X_world + t).
+ */
+
+#ifndef CORELIB_SRC_OPTIMIZER_CERES_BUNDLE_BETWEEN_CAMERAS_ERROR_H_
+#define CORELIB_SRC_OPTIMIZER_CERES_BUNDLE_BETWEEN_CAMERAS_ERROR_H_
+
+#include "Eigen/Core"
+#include "ceres/autodiff_cost_function.h"
+#include "ceres/rotation.h"
+
+namespace ceres {
+
+// Relative-pose constraint between two cameras, expressed in the SAME frame
+// convention as g2o's EdgeSBACam: the measurement is the pose of camera B as
+// seen from camera A's frame (i.e., the relative transform from cam_a to
+// cam_b). Residual is 6-D [Δt; Δrot] in se(3) — Δt = predicted_t - measured_t
+// and Δrot = log(R_meas^T * R_predicted) — then whitened by the square root
+// of the information matrix.
+struct BetweenCamerasError {
+  BetweenCamerasError(const Eigen::Vector3d & translation_measured,
+                      const Eigen::Vector3d & angle_axis_measured,
+                      const Eigen::Matrix<double, 6, 6> & sqrt_information)
+      : t_meas_(translation_measured),
+        aa_meas_(angle_axis_measured),
+        sqrt_info_(sqrt_information) {}
+
+  template <typename T>
+  bool operator()(const T * const cam_a,
+                  const T * const cam_b,
+                  T * residuals_ptr) const {
+    // Camera layout: [aa(3), t(3)] -- world-to-camera.
+    const T * aa_a = cam_a;
+    const T * t_a  = cam_a + 3;
+    const T * aa_b = cam_b;
+    const T * t_b  = cam_b + 3;
+
+    // Predicted relative transform "cam_b in cam_a's frame":
+    //   R_ab = R_a * R_b^T
+    //   t_ab = t_a - R_ab * t_b
+    T q_a[4];
+    T q_b[4];
+    ceres::AngleAxisToQuaternion(aa_a, q_a);
+    ceres::AngleAxisToQuaternion(aa_b, q_b);
+
+    // q_b_inv = conjugate (unit quat assumed).
+    T q_b_inv[4] = { q_b[0], -q_b[1], -q_b[2], -q_b[3] };
+    T q_ab[4];
+    ceres::QuaternionProduct(q_a, q_b_inv, q_ab);
+
+    T aa_ab[3];
+    ceres::QuaternionToAngleAxis(q_ab, aa_ab);
+
+    T R_ab_tb[3];
+    ceres::AngleAxisRotatePoint(aa_ab, t_b, R_ab_tb);
+
+    T t_ab[3] = { t_a[0] - R_ab_tb[0],
+                  t_a[1] - R_ab_tb[1],
+                  t_a[2] - R_ab_tb[2] };
+
+    // Translation residual: predicted - measured.
+    T t_residual[3] = { t_ab[0] - T(t_meas_(0)),
+                        t_ab[1] - T(t_meas_(1)),
+                        t_ab[2] - T(t_meas_(2)) };
+
+    // Rotation residual: angle-axis of (R_meas^T * R_predicted).
+    T q_meas[4];
+    const T aa_meas_T[3] = { T(aa_meas_(0)), T(aa_meas_(1)), T(aa_meas_(2)) };
+    ceres::AngleAxisToQuaternion(aa_meas_T, q_meas);
+    T q_meas_inv[4] = { q_meas[0], -q_meas[1], -q_meas[2], -q_meas[3] };
+    T q_err[4];
+    ceres::QuaternionProduct(q_meas_inv, q_ab, q_err);
+    T aa_err[3];
+    ceres::QuaternionToAngleAxis(q_err, aa_err);
+
+    Eigen::Map<Eigen::Matrix<T, 6, 1> > residuals(residuals_ptr);
+    residuals(0) = t_residual[0];
+    residuals(1) = t_residual[1];
+    residuals(2) = t_residual[2];
+    residuals(3) = aa_err[0];
+    residuals(4) = aa_err[1];
+    residuals(5) = aa_err[2];
+    residuals.applyOnTheLeft(sqrt_info_.template cast<T>());
+
+    return true;
+  }
+
+  static ceres::CostFunction * Create(
+      const Eigen::Vector3d & translation_measured,
+      const Eigen::Vector3d & angle_axis_measured,
+      const Eigen::Matrix<double, 6, 6> & sqrt_information) {
+    return new ceres::AutoDiffCostFunction<BetweenCamerasError, 6, 6, 6>(
+        new BetweenCamerasError(translation_measured, angle_axis_measured, sqrt_information));
+  }
+
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  const Eigen::Vector3d t_meas_;
+  const Eigen::Vector3d aa_meas_;
+  const Eigen::Matrix<double, 6, 6> sqrt_info_;
+};
+
+}  // namespace ceres
+
+#endif  // CORELIB_SRC_OPTIMIZER_CERES_BUNDLE_BETWEEN_CAMERAS_ERROR_H_

corelib/src/optimizer/ceres/bundle/planar_constraint_error.h

@@ -0,0 +1,68 @@
+/*
+ * planar_constraint_error.h
+ *
+ * Unary "lock the robot to a horizontal plane" constraint for 2D BA.
+ * Mirrors g2o's EdgeSBACamPrior with pinfo(2,2) = 1e9: constrains the
+ * BODY-frame z coordinate of the camera vertex to its initial value, so
+ * the recovered trajectory stays on the floor. Only the z translation
+ * axis carries weight; everything else is free.
+ *
+ * Snavely camera parameterization is [aa_cw (3), t_cw (3)] (world-to-
+ * camera). Given the body-to-camera (= localTransform) rotation R_bc and
+ * translation t_bc, the body position in world coordinates is:
+ *
+ *   body_world = R_cw^T * (-R_bc^T * t_bc - t_cw)
+ *
+ * Only z is extracted and compared to the initial body z.
+ */
+
+#ifndef CORELIB_SRC_OPTIMIZER_CERES_BUNDLE_PLANAR_CONSTRAINT_ERROR_H_
+#define CORELIB_SRC_OPTIMIZER_CERES_BUNDLE_PLANAR_CONSTRAINT_ERROR_H_
+
+#include "Eigen/Core"
+#include "ceres/autodiff_cost_function.h"
+#include "ceres/rotation.h"
+
+namespace ceres {
+
+struct PlanarConstraintError {
+  PlanarConstraintError(const Eigen::Matrix3d & R_bc,
+                        const Eigen::Vector3d & t_bc,
+                        double initial_body_z,
+                        double sqrt_information)
+      : neg_Rbc_T_tbc_(-R_bc.transpose() * t_bc),
+        initial_body_z_(initial_body_z),
+        sqrt_information_(sqrt_information) {}
+
+  template <typename T>
+  bool operator()(const T * const camera, T * residuals) const {
+    // camera = [aa_cw (3), t_cw (3)] (Snavely / world-to-camera).
+    // body_world = R_cw^T * (-R_bc^T * t_bc - t_cw)
+    const T diff[3] = { T(neg_Rbc_T_tbc_(0)) - camera[3],
+                        T(neg_Rbc_T_tbc_(1)) - camera[4],
+                        T(neg_Rbc_T_tbc_(2)) - camera[5] };
+    const T inv_aa[3] = { -camera[0], -camera[1], -camera[2] };
+    T body_world[3];
+    ceres::AngleAxisRotatePoint(inv_aa, diff, body_world);
+    residuals[0] = (body_world[2] - T(initial_body_z_)) * T(sqrt_information_);
+    return true;
+  }
+
+  static ceres::CostFunction * Create(const Eigen::Matrix3d & R_bc,
+                                      const Eigen::Vector3d & t_bc,
+                                      double initial_body_z,
+                                      double sqrt_information) {
+    return new ceres::AutoDiffCostFunction<PlanarConstraintError, 1, 6>(
+        new PlanarConstraintError(R_bc, t_bc, initial_body_z, sqrt_information));
+  }
+
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  const Eigen::Vector3d neg_Rbc_T_tbc_;  // = -R_bc^T * t_bc (constant)
+  const double initial_body_z_;
+  const double sqrt_information_;
+};
+
+}  // namespace ceres
+
+#endif  // CORELIB_SRC_OPTIMIZER_CERES_BUNDLE_PLANAR_CONSTRAINT_ERROR_H_

corelib/src/optimizer/ceres/bundle/snavely_stereo_reprojection_error.h

@@ -0,0 +1,104 @@
+/*
+ * snavely_stereo_reprojection_error.h
+ *
+ * Stereo analogue of SnavelyReprojectionError: outputs 3 residuals
+ * (u, v, u-disparity) so the disparity (depth) measurement pins the
+ * z component of each landmark relative to its observing camera. With
+ * at least one stereo observation per point the 7-DOF mono BA gauge
+ * collapses to 6 -- absolute scale becomes observable.
+ */
+
+#ifndef CORELIB_SRC_OPTIMIZER_CERES_BUNDLE_SNAVELY_STEREO_REPROJECTION_ERROR_H_
+#define CORELIB_SRC_OPTIMIZER_CERES_BUNDLE_SNAVELY_STEREO_REPROJECTION_ERROR_H_
+
+#include "ceres/ceres.h"
+#include "ceres/rotation.h"
+
+namespace ceres {
+
+// Templated pinhole stereo camera model. Camera parameterization is
+// the same as SnavelyReprojectionError (3 angle-axis + 3 translation).
+// The stereo channel is encoded via the right-image x coordinate, i.e.
+// u_right = u_left - disparity, where disparity = baseline * fx / Z.
+// observed_disparity is the measured disparity (precomputed from the
+// observation's depth: disparity = baseline * fx / depth).
+//
+// inv_sigma_uv and inv_sigma_d are 1/sigma_pixel and 1/sigma_disparity --
+// they let the cost function reproduce g2o's per-axis information matrix
+// (1/pixelVariance on u/v, 1/disparityVariance on disparity). Pass
+// inv_sigma_uv = inv_sigma_d = 1 for isotropic behavior.
+struct SnavelyStereoReprojectionError {
+  SnavelyStereoReprojectionError(
+      double observed_x,
+      double observed_y,
+      double observed_disparity,
+      double fx,
+      double fy,
+      double baseline_fx,
+      double inv_sigma_uv,
+      double inv_sigma_d)
+      : observed_x(observed_x),
+        observed_y(observed_y),
+        observed_disparity(observed_disparity),
+        fx(fx),
+        fy(fy),
+        baseline_fx(baseline_fx),
+        inv_sigma_uv(inv_sigma_uv),
+        inv_sigma_d(inv_sigma_d) {}
+
+  template <typename T>
+  bool operator()(const T* const camera,
+                  const T* const point,
+                  T* residuals) const {
+
+    // camera[0,1,2] are the angle-axis rotation.
+    T p[3];
+    ceres::AngleAxisRotatePoint(camera, point, p);
+    // camera[3,4,5] are the translation.
+    p[0] += camera[3];
+    p[1] += camera[4];
+    p[2] += camera[5];
+
+    // Pinhole projection.
+    T xp = p[0] / p[2];
+    T yp = p[1] / p[2];
+
+    T predicted_x = fx * xp;
+    T predicted_y = fy * yp;
+    // disparity = baseline * fx / Z
+    T predicted_disparity = T(baseline_fx) / p[2];
+
+    residuals[0] = (predicted_x - observed_x) * T(inv_sigma_uv);
+    residuals[1] = (predicted_y - observed_y) * T(inv_sigma_uv);
+    residuals[2] = (predicted_disparity - observed_disparity) * T(inv_sigma_d);
+
+    return true;
+  }
+
+  static ceres::CostFunction* Create(double observed_x,
+                                     double observed_y,
+                                     double observed_disparity,
+                                     double fx,
+                                     double fy,
+                                     double baseline_fx,
+                                     double inv_sigma_uv,
+                                     double inv_sigma_d) {
+    return (new ceres::AutoDiffCostFunction<SnavelyStereoReprojectionError, 3, 6, 3>(
+        new SnavelyStereoReprojectionError(
+            observed_x, observed_y, observed_disparity, fx, fy, baseline_fx,
+            inv_sigma_uv, inv_sigma_d)));
+  }
+
+  double observed_x;
+  double observed_y;
+  double observed_disparity;
+  double fx;
+  double fy;
+  double baseline_fx;   // baseline * fx (cached)
+  double inv_sigma_uv;  // 1 / sqrt(pixelVariance)
+  double inv_sigma_d;   // 1 / sqrt(disparityVariance)
+};
+
+}  // namespace ceres
+
+#endif  // CORELIB_SRC_OPTIMIZER_CERES_BUNDLE_SNAVELY_STEREO_REPROJECTION_ERROR_H_

corelib/test/test_optimizer.cpp

@@ -1727,11 +1727,11 @@ BundleGraph buildBundleGraph(bool noisy = false, bool roundPixels = false, int n
 //     stereo BA path.
 enum class BaVariant {
 	kDefault,
-	kG2ONoLinks,
-	kG2OWithDepth,
-	kG2OWithDepthNoLinks,
-	kG2OWithDepthNoLinksTuned,         // WithDepth + per-axis info calibrated to actual noise
-	kG2OWithLidarDepthNoLinksTuned,    // Accurate depth source (LiDAR-fused, 1 cm sigma) + tight DisparityVariance
+	kNoLinks,
+	kWithDepth,
+	kWithDepthNoLinks,
+	kWithDepthNoLinksTuned,         // WithDepth + per-axis info calibrated to actual noise
+	kWithLidarDepthNoLinksTuned,    // Accurate depth source (LiDAR-fused, 1 cm sigma) + tight DisparityVariance
 };
 
 // (backend, variant, roundPixels). roundPixels=true simulates discrete
@@ -1745,11 +1745,11 @@ const char * baVariantName(BaVariant v)
 	switch(v)
 	{
 		case BaVariant::kDefault:             return "Default";
-		case BaVariant::kG2ONoLinks:          return "NoLinks";
-		case BaVariant::kG2OWithDepth:        return "WithDepth";
-		case BaVariant::kG2OWithDepthNoLinks: return "WithDepthNoLinks";
-		case BaVariant::kG2OWithDepthNoLinksTuned:      return "WithDepthNoLinksTuned";
-		case BaVariant::kG2OWithLidarDepthNoLinksTuned: return "WithLidarDepthNoLinksTuned";
+		case BaVariant::kNoLinks:          return "NoLinks";
+		case BaVariant::kWithDepth:        return "WithDepth";
+		case BaVariant::kWithDepthNoLinks: return "WithDepthNoLinks";
+		case BaVariant::kWithDepthNoLinksTuned:      return "WithDepthNoLinksTuned";
+		case BaVariant::kWithLidarDepthNoLinksTuned: return "WithLidarDepthNoLinksTuned";
 	}
 	return "?";
 }
@@ -1781,8 +1781,8 @@ TEST_P(BundleAdjustmentTest, CircleCamerasRecoverPosesAndPoints)
 	// For the stereo BA variants we set Tx on the CameraModel below, which
 	// g2o picks up directly. Set g2o/Baseline to the same 0.15 m for
 	// consistency (used as a fallback when Tx isn't set on the model).
-	params[Parameters::kg2oBaseline()]         = "0.15";
-	if(variant == BaVariant::kG2OWithLidarDepthNoLinksTuned)
+	params[Parameters::kOptimizerBaseline()]         = "0.15";
+	if(variant == BaVariant::kWithLidarDepthNoLinksTuned)
 	{
 		// Accurate-depth scenario (LiDAR-fused / structured-light): the
 		// depth measurement is *more* precise than a typical feature
@@ -1794,10 +1794,10 @@ TEST_P(BundleAdjustmentTest, CircleCamerasRecoverPosesAndPoints)
 		// (e.g. 0.01 gives same residuals) but g2o's Hessian becomes
 		// ill-conditioned around info=10000, so we stay comfortably
 		// within the stable range.
-		params[Parameters::kg2oPixelVariance()]     = "1.0";
-		params[Parameters::kg2oDisparityVariance()] = "0.1";
+		params[Parameters::kOptimizerPixelVariance()]     = "1.0";
+		params[Parameters::kOptimizerDisparityVariance()] = "0.1";
 	}
-	else if(variant == BaVariant::kG2OWithDepthNoLinksTuned)
+	else if(variant == BaVariant::kWithDepthNoLinksTuned)
 	{
 		// Sub-pixel feature detector + standard stereo block matcher tuning:
 		// PixelVariance=0.1 (sigma_uv ~ 0.3 px), DisparityVariance=1
@@ -1807,8 +1807,8 @@ TEST_P(BundleAdjustmentTest, CircleCamerasRecoverPosesAndPoints)
 		// because the optimizer over-trusts noisy disparity and pushes points
 		// along the depth axis to fit it). With this tuning the points
 		// converge to ~4 mm.
-		params[Parameters::kg2oPixelVariance()]     = "0.1";
-		params[Parameters::kg2oDisparityVariance()] = "1.0";
+		params[Parameters::kOptimizerPixelVariance()]     = "0.1";
+		params[Parameters::kOptimizerDisparityVariance()] = "1.0";
 	}
 	std::unique_ptr<Optimizer> opt(Optimizer::create(params));
 	ASSERT_NE(opt.get(), nullptr);
@@ -1816,19 +1816,19 @@ TEST_P(BundleAdjustmentTest, CircleCamerasRecoverPosesAndPoints)
 
 	BundleGraph g = buildBundleGraph(/*noisy=*/true, /*roundPixels=*/roundPixels);
 
-	if(variant == BaVariant::kG2ONoLinks
-			|| variant == BaVariant::kG2OWithDepthNoLinks
-			|| variant == BaVariant::kG2OWithDepthNoLinksTuned
-			|| variant == BaVariant::kG2OWithLidarDepthNoLinksTuned)
+	if(variant == BaVariant::kNoLinks
+			|| variant == BaVariant::kWithDepthNoLinks
+			|| variant == BaVariant::kWithDepthNoLinksTuned
+			|| variant == BaVariant::kWithLidarDepthNoLinksTuned)
 	{
 		// Drop the noisy neighbor links so g2o BA is reduced to a pure
 		// reprojection problem (like Ceres/CVSBA).
 		g.links.clear();
 	}
-	if(variant == BaVariant::kG2OWithDepth
-			|| variant == BaVariant::kG2OWithDepthNoLinks
-			|| variant == BaVariant::kG2OWithDepthNoLinksTuned
-			|| variant == BaVariant::kG2OWithLidarDepthNoLinksTuned)
+	if(variant == BaVariant::kWithDepth
+			|| variant == BaVariant::kWithDepthNoLinks
+			|| variant == BaVariant::kWithDepthNoLinksTuned
+			|| variant == BaVariant::kWithLidarDepthNoLinksTuned)
 	{
 		// Switch to a stereo camera model: baseline = 0.15 m (a common
 		// medium-baseline value e.g. ZED Mini / RealSense D435i). Tx =
@@ -1858,7 +1858,7 @@ TEST_P(BundleAdjustmentTest, CircleCamerasRecoverPosesAndPoints)
 		// a small constant sigma on depth (range-independent, as a depth
 		// sensor / fused range source would produce). DisparityVariance is
 		// set tight in the params block to reflect this higher precision.
-		const bool lidarDepth = (variant == BaVariant::kG2OWithLidarDepthNoLinksTuned);
+		const bool lidarDepth = (variant == BaVariant::kWithLidarDepthNoLinksTuned);
 		std::mt19937 dispRng(13);
 		std::normal_distribution<double> dispNoise (0.0, 1.0);   // 1 px on disparity (stereo / RGB-D)
 		std::normal_distribution<double> depthLidarNoise(0.0, 0.01);  // 1 cm on depth (LiDAR / structured-light)
@@ -1945,12 +1945,12 @@ TEST_P(BundleAdjustmentTest, CircleCamerasRecoverPosesAndPoints)
 	float pointDistMax  = 0.025f;
 	if(backend == Optimizer::kTypeG2O)
 	{
-		if(variant == BaVariant::kG2ONoLinks)
+		if(variant == BaVariant::kNoLinks)
 		{
 			poseDistMax  = 0.015f;
 			pointDistMax = 0.015f;
 		}
-		else if(variant == BaVariant::kG2OWithDepth)
+		else if(variant == BaVariant::kWithDepth)
 		{
 			poseDistMax  = 0.02f;
 			// With realistic stereo noise (1 px on disparity), point recovery
@@ -1963,12 +1963,12 @@ TEST_P(BundleAdjustmentTest, CircleCamerasRecoverPosesAndPoints)
 			// 8x tighter points.
 			pointDistMax = roundPixels ? 0.025f : 0.15f;
 		}
-		else if(variant == BaVariant::kG2OWithDepthNoLinks)
+		else if(variant == BaVariant::kWithDepthNoLinks)
 		{
 			poseDistMax  = 0.015f;
 			pointDistMax = roundPixels ? 0.025f : 0.15f;
 		}
-		else if(variant == BaVariant::kG2OWithDepthNoLinksTuned)
+		else if(variant == BaVariant::kWithDepthNoLinksTuned)
 		{
 			// Calibrated per-axis info matrix + no noisy chain. Tightest
 			// case of all the WithDepth variants: both pose AND point
@@ -1976,7 +1976,7 @@ TEST_P(BundleAdjustmentTest, CircleCamerasRecoverPosesAndPoints)
 			poseDistMax  = 0.015f;
 			pointDistMax = 0.015f;
 		}
-		else if(variant == BaVariant::kG2OWithLidarDepthNoLinksTuned)
+		else if(variant == BaVariant::kWithLidarDepthNoLinksTuned)
 		{
 			// Accurate depth (1 cm sigma) + tight DisparityVariance: the
 			// tightest of all WithDepth variants on both pose and point.
@@ -1990,6 +1990,47 @@ TEST_P(BundleAdjustmentTest, CircleCamerasRecoverPosesAndPoints)
 			pointDistMax = 0.09f;
 		}
 	}
+	else if(backend == Optimizer::kTypeGTSAM)
+	{
+		// GTSAM soft-fixes the root via a tight PriorFactor (no native
+		// "setFixed" like g2o), so the gauge is slightly looser; the LM
+		// solver also stops at a relativeErrorTol that leaves a tiny bit of
+		// residual on the longest-range points. Both effects are sub-cm on
+		// the pose, ~few-cm on point cloud points that are 7-8 m from the
+		// root. Stereo variants converge to the same bounds as g2o because
+		// the depth constraint pins the gauge.
+		if(variant == BaVariant::kWithDepthNoLinksTuned)
+		{
+			poseDistMax  = 0.015f;
+			pointDistMax = 0.015f;
+		}
+		else if(variant == BaVariant::kWithLidarDepthNoLinksTuned)
+		{
+			poseDistMax  = 0.005f;
+			pointDistMax = 0.005f;
+		}
+		else if(variant == BaVariant::kWithDepth || variant == BaVariant::kWithDepthNoLinks)
+		{
+			poseDistMax  = 0.03f;
+			pointDistMax = roundPixels ? 0.025f : 0.15f;
+		}
+		else if(variant == BaVariant::kNoLinks)
+		{
+			// Pure mono reprojection. Mono BA has a 7-DOF gauge -- the
+			// recovered geometry is only correct up to scale -- but we
+			// solve for that scale against truth below before checking
+			// bounds, so the bounds match the clean-converged case.
+			poseDistMax  = 0.015f;
+			pointDistMax = 0.02f;
+		}
+		else
+		{
+			// kDefault: mono BA + noisy chain. Scale handled below; the
+			// chain noise still pulls poses ~few cm.
+			poseDistMax  = 0.04f;
+			pointDistMax = 0.04f;
+		}
+	}
 	else if(backend == Optimizer::kTypeCVSBA)
 	{
 		poseDistMax  = 0.06f;
@@ -2003,6 +2044,48 @@ TEST_P(BundleAdjustmentTest, CircleCamerasRecoverPosesAndPoints)
 		pointDistMax = std::max(pointDistMax, 0.02f);
 	}
 
+	// Mono BA gauge: fixing one pose leaves a 1-DOF scale ambiguity that the
+	// optimizer is free to land anywhere on. Solve for the scale that maps
+	// recovered geometry onto truth (closed-form LS on camera positions in
+	// the root-relative frame:
+	//   s = Σ(d_out · d_truth) / Σ(d_out · d_out)
+	// ) and apply it to BOTH poses and points before comparing. This is
+	// the test-side counterpart to the brute-force scale scan in
+	// tools/Report/main.cpp; here we know the relationship is quadratic
+	// so the closed form is exact.
+	//
+	// Applied when:
+	//   * the variant is pure-mono (no depth in observations), OR
+	//   * the BACKEND is mono-only -- rtabmap's CVSBA path uses cvsba's
+	//     Sba::run() which takes 2D image points only and so cannot use
+	//     depth; it runs mono BA even when handed stereo-style
+	//     observations and is gauge-ambiguous regardless of the variant.
+	// g2o, GTSAM, and Ceres switch to a stereo cost function when
+	// depth+baseline are available, so on stereo variants their scale is
+	// pinned by geometry and we leave it alone.
+	const bool monoVariant = (variant == BaVariant::kDefault || variant == BaVariant::kNoLinks);
+	const bool monoOnlyBackend = (backend == Optimizer::kTypeCVSBA);
+	const bool monoBA = monoVariant || monoOnlyBackend;
+	float scale = 1.0f;
+	if(monoBA)
+	{
+		double num = 0.0;
+		double den = 0.0;
+		for(const auto & kv : g.truePoses)
+		{
+			if(kv.first == 1) continue;
+			if(!outPoses.count(kv.first)) continue;
+			const Transform truthRel = truthRootInv * kv.second;
+			const Transform outRel   = outRootInv   * outPoses.at(kv.first);
+			num += outRel.x()*truthRel.x() + outRel.y()*truthRel.y() + outRel.z()*truthRel.z();
+			den += outRel.x()*outRel.x()   + outRel.y()*outRel.y()   + outRel.z()*outRel.z();
+		}
+		if(den > 1e-12)
+		{
+			scale = static_cast<float>(num / den);
+		}
+	}
+
 	// Recovered poses (relative to root).
 	for(const auto & kv : g.truePoses)
 	{
@@ -2010,7 +2093,13 @@ TEST_P(BundleAdjustmentTest, CircleCamerasRecoverPosesAndPoints)
 		if(id == 1) continue;  // root is its own reference (delta = identity in both frames)
 		ASSERT_TRUE(outPoses.count(id));
 		const Transform truthRel = truthRootInv * kv.second;
-		const Transform outRel   = outRootInv   * outPoses.at(id);
+		Transform outRel         = outRootInv   * outPoses.at(id);
+		if(monoBA)
+		{
+			outRel.x() *= scale;
+			outRel.y() *= scale;
+			outRel.z() *= scale;
+		}
 		EXPECT_LT(outRel.getDistance(truthRel), poseDistMax)
 				<< optimizerTypeName(backend) << " pose " << id
 				<< " got(rel)=" << outRel.prettyPrint()
@@ -2026,7 +2115,13 @@ TEST_P(BundleAdjustmentTest, CircleCamerasRecoverPosesAndPoints)
 		const int id = kv.first;
 		ASSERT_TRUE(outPoints.count(id));
 		const cv::Point3f truthRel = util3d::transformPoint(kv.second, truthRootInv);
-		const cv::Point3f outRel   = util3d::transformPoint(outPoints.at(id), outRootInv);
+		cv::Point3f outRel         = util3d::transformPoint(outPoints.at(id), outRootInv);
+		if(monoBA)
+		{
+			outRel.x *= scale;
+			outRel.y *= scale;
+			outRel.z *= scale;
+		}
 		const cv::Point3f diff     = outRel - truthRel;
 		const float d = std::sqrt(diff.x*diff.x + diff.y*diff.y + diff.z*diff.z);
 		EXPECT_LT(d, pointDistMax)
@@ -2042,20 +2137,38 @@ INSTANTIATE_TEST_SUITE_P(
 		::testing::Values(
 				// Continuous-pixel observations (the test's original setup).
 				std::make_tuple(Optimizer::kTypeG2O,   BaVariant::kDefault,             false),
-				std::make_tuple(Optimizer::kTypeG2O,   BaVariant::kG2ONoLinks,          false),
-				std::make_tuple(Optimizer::kTypeG2O,   BaVariant::kG2OWithDepth,        false),
-				std::make_tuple(Optimizer::kTypeG2O,   BaVariant::kG2OWithDepthNoLinks, false),
-				std::make_tuple(Optimizer::kTypeG2O,   BaVariant::kG2OWithDepthNoLinksTuned,      false),
-				std::make_tuple(Optimizer::kTypeG2O,   BaVariant::kG2OWithLidarDepthNoLinksTuned, false),
+				std::make_tuple(Optimizer::kTypeG2O,   BaVariant::kNoLinks,          false),
+				std::make_tuple(Optimizer::kTypeG2O,   BaVariant::kWithDepth,        false),
+				std::make_tuple(Optimizer::kTypeG2O,   BaVariant::kWithDepthNoLinks, false),
+				std::make_tuple(Optimizer::kTypeG2O,   BaVariant::kWithDepthNoLinksTuned,      false),
+				std::make_tuple(Optimizer::kTypeG2O,   BaVariant::kWithLidarDepthNoLinksTuned, false),
+				std::make_tuple(Optimizer::kTypeGTSAM, BaVariant::kDefault,             false),
+				std::make_tuple(Optimizer::kTypeGTSAM, BaVariant::kNoLinks,          false),
+				std::make_tuple(Optimizer::kTypeGTSAM, BaVariant::kWithDepth,        false),
+				std::make_tuple(Optimizer::kTypeGTSAM, BaVariant::kWithDepthNoLinks, false),
+				std::make_tuple(Optimizer::kTypeGTSAM, BaVariant::kWithDepthNoLinksTuned,      false),
+				std::make_tuple(Optimizer::kTypeGTSAM, BaVariant::kWithLidarDepthNoLinksTuned, false),
 				std::make_tuple(Optimizer::kTypeCeres, BaVariant::kDefault,             false),
+				std::make_tuple(Optimizer::kTypeCeres, BaVariant::kNoLinks,          false),
+				std::make_tuple(Optimizer::kTypeCeres, BaVariant::kWithDepth,        false),
+				std::make_tuple(Optimizer::kTypeCeres, BaVariant::kWithDepthNoLinks, false),
+				std::make_tuple(Optimizer::kTypeCeres, BaVariant::kWithDepthNoLinksTuned,      false),
+				std::make_tuple(Optimizer::kTypeCeres, BaVariant::kWithLidarDepthNoLinksTuned, false),
 				std::make_tuple(Optimizer::kTypeCVSBA, BaVariant::kDefault,             false),
 				// Same setups but with keypoints rounded to integer pixel
 				// coordinates (simulates a real detector).
 				std::make_tuple(Optimizer::kTypeG2O,   BaVariant::kDefault,             true),
-				std::make_tuple(Optimizer::kTypeG2O,   BaVariant::kG2ONoLinks,          true),
-				std::make_tuple(Optimizer::kTypeG2O,   BaVariant::kG2OWithDepth,        true),
-				std::make_tuple(Optimizer::kTypeG2O,   BaVariant::kG2OWithDepthNoLinks, true),
+				std::make_tuple(Optimizer::kTypeG2O,   BaVariant::kNoLinks,          true),
+				std::make_tuple(Optimizer::kTypeG2O,   BaVariant::kWithDepth,        true),
+				std::make_tuple(Optimizer::kTypeG2O,   BaVariant::kWithDepthNoLinks, true),
+				std::make_tuple(Optimizer::kTypeGTSAM, BaVariant::kDefault,             true),
+				std::make_tuple(Optimizer::kTypeGTSAM, BaVariant::kNoLinks,          true),
+				std::make_tuple(Optimizer::kTypeGTSAM, BaVariant::kWithDepth,        true),
+				std::make_tuple(Optimizer::kTypeGTSAM, BaVariant::kWithDepthNoLinks, true),
 				std::make_tuple(Optimizer::kTypeCeres, BaVariant::kDefault,             true),
+				std::make_tuple(Optimizer::kTypeCeres, BaVariant::kNoLinks,             true),
+				std::make_tuple(Optimizer::kTypeCeres, BaVariant::kWithDepth,           true),
+				std::make_tuple(Optimizer::kTypeCeres, BaVariant::kWithDepthNoLinks,    true),
 				std::make_tuple(Optimizer::kTypeCVSBA, BaVariant::kDefault,             true)),
 		[](const ::testing::TestParamInfo<BaParam> & info)
 		{
@@ -2066,6 +2179,82 @@ INSTANTIATE_TEST_SUITE_P(
 			return name;
 		});
 
+// ---------------------------------------------------------------------------
+// PlanarBundleAdjustmentTest -- verifies that isSlam2d() in BA locks the
+// recovered trajectory to its initial Z plane. g2o has supported this since
+// forever via EdgeSBACamPrior; GTSAM and Ceres got matching planar
+// constraints in this rev.
+// ---------------------------------------------------------------------------
+
+class PlanarBundleAdjustmentTest : public ::testing::TestWithParam<Optimizer::Type>
+{
+protected:
+	void SetUp() override
+	{
+		if(!Optimizer::isAvailable(GetParam()))
+		{
+			GTEST_SKIP() << optimizerTypeName(GetParam()) << " not built in";
+		}
+	}
+};
+
+TEST_P(PlanarBundleAdjustmentTest, RecoveredTrajectoryStaysOnInitialPlane)
+{
+	const Optimizer::Type backend = GetParam();
+
+	ParametersMap params;
+	params[Parameters::kOptimizerStrategy()]   = uNumber2Str(static_cast<int>(backend));
+	params[Parameters::kOptimizerIterations()] = "200";
+	params[Parameters::kRegForce3DoF()]        = "true";  // → isSlam2d() == true
+	std::unique_ptr<Optimizer> opt(Optimizer::create(params));
+	ASSERT_NE(opt.get(), nullptr);
+	ASSERT_TRUE(opt->isSlam2d());
+
+	BundleGraph g = buildBundleGraph(/*noisy=*/true, /*roundPixels=*/false);
+	// Planar test: initial poses reflect a real 2D-SLAM gauge -- the robot
+	// lives on a single horizontal plane at some non-zero height (z=0.5 m
+	// here, e.g. a camera mounted on a robot half a meter off the floor)
+	// with no roll/pitch. Strip the z/roll/pitch noise from the noisy
+	// initial poses and keep only the x/y/yaw noise. Using a non-zero
+	// reference z verifies the constraint locks to the INITIAL plane,
+	// not to z=0 by accident.
+	const float planeZ = 0.5f;
+	for(auto & kv : g.initialPoses)
+	{
+		float x, y, z, roll, pitch, yaw;
+		kv.second.getTranslationAndEulerAngles(x, y, z, roll, pitch, yaw);
+		kv.second = Transform(x, y, planeZ, 0.0f, 0.0f, yaw);
+	}
+
+	std::map<int, cv::Point3f> outPoints = g.initialPoints3D;
+	std::map<int, Transform> outPoses = opt->optimizeBA(
+			/*rootId=*/1, g.initialPoses, g.links, g.models, outPoints, g.wordReferences);
+
+	ASSERT_FALSE(outPoses.empty()) << "optimizeBA returned no poses";
+	ASSERT_EQ(outPoses.size(), g.truePoses.size());
+
+	// With the planar constraint locking every camera to its initial
+	// z = planeZ (weight 1e9 = sub-mm tolerance), no recovered pose
+	// should drift off the plane. Without the constraint, a 3D BA on
+	// this dataset can drift up to a few mm in z due to reprojection /
+	// chain residuals.
+	for(const auto & kv : outPoses)
+	{
+		EXPECT_NEAR(kv.second.z(), planeZ, 0.0005f)  // 0.5 mm
+				<< optimizerTypeName(backend) << " pose " << kv.first
+				<< " drifted off the z=" << planeZ << " plane: z=" << kv.second.z();
+	}
+}
+
+INSTANTIATE_TEST_SUITE_P(
+		Backends,
+		PlanarBundleAdjustmentTest,
+		::testing::Values(Optimizer::kTypeG2O, Optimizer::kTypeGTSAM, Optimizer::kTypeCeres),
+		[](const ::testing::TestParamInfo<Optimizer::Type> & info)
+		{
+			return optimizerTypeName(info.param);
+		});
+
 TEST(OptimizerTest, CvsbaPoseGraphOptimizeReturnsEmpty)
 {
 	// CVSBA only overrides optimizeBA() -- it doesn't implement pose-graph