Adding doc and tests (#1492)

* added doc and tests for util2d.h

* updated cmake-ros ci

* Added util3d.h doc and tests

* util3d_transforms.h: Added doc and tests

* util3d_filtering.h: started doc and test

* util3d_filtering.h: more tests and doc

* Added more doc/tests

* finished util3d_filtering doc and tests

* added test for util2d::depthBleedingFiltering

* Added util3d_registration tests

* Added util3d_features.h doc/tests

* added doc/tests for util3d_correspondences.h

* added doc/gtest for util3d_mapping.h (missing hpp functions)

* finished testing util3d_mapping.hpp

* Added util3d_motion_estimation.h tests (2D->3D done)

* finished util3d_motion_estimation.h tests

* minimal util3d_surface.h

* Added Transform and VisualWord tests

* Added doc for CameraModel and StereoCameraModel

* Added more logs in ros ci

* Passing tests on fical

* improved all devcontainer

* added devcontainer kilted, fixed source setup.bash, removed ldconfig in ros-cmake workflow

* cleanup

* source ros

* Added utilite tests

* Added testing to appveyor, github actions cancellable on re-commit on same branch

* appveyor testing without all targets

* appveyor: specifying ALL_BUILD target

* Fixed Util2dTest.NMSImageBoundsRespected test

* Fixing PCL Indices error on old pcl

* Added VWDictionary tests and doc. Fixed LSH not working (fix from https://github.com/flann-lib/flann/pull/472

* fixing some appveyor CI errors, added test to check dictionary serialization against all type

* Added StereoDense, StereoBM and StereoSGBM doc and tests

* Added Stereo tests

* Added CameraModel and StereoCameraModel tests

* Added doc and test for Statistics

* Added doc/tests for Signature

* Added doc/test for SensorEvent, added doc for SensorCaptureInfo

* Added doc to SensorData

* Added SensorData tests

* Added SensorCapture and SensorCaptureThread doc and tests

* fixed sensordata test

* updated SSC test and doc

* Added doc and tests for BayesFilter class

* Enabled testing on mac, updated windows testing like on linux

* added test_link

* fixed unresolved on windows

* fixed ThreadHandle error on macos ci

* Added GPS and GeodeticCoords tests

* Added tests for compression

* Added Odometry tests (base class only)

* Added DBDriver tests

* Added coverage report

* uniformized test names

* fixing concurancy and coverage ci

* dont built tools, examples and app for coverage build

* fixed report tool rebuilt without qt compilation error

* updated coverage option

* updated coverage config

* added doc CI job

* fixing windows and mac ci errors

* Added DBDriverSqlite3 tests

* Added IMU tests

* Added Graph tests

* fixing flaky macos test

* Added IMUThread and IMUFilter tests

* Added Landmarks tests

* Added LASWriter tests

* fixing seed flaky test

* fixing flaky macos timing tests

* Added LocalGrid tests

* Added LocalGridMaker tests

* fixing ci errors

* Added GlobalMap tests

* Added doc for EnvSensor

* Added Features2D tests

* Added Registration tests

* Added RegistrationVis tests

* Added doc for Rtabmap and Memory classes

* Added Memory and Rtabmap tests

* making some tests less flaky

* lcov 1.14 support

* updated compatible tool arguments

* Added integration tests (RGB-D, Stereo, Lidar2d, Lidar3d)

* More octomap checks

* Refactored how/when python interpretor is created to simplify library usage

* Added python tests

* fixed some flaky tests

* suppressed some third party related warnings

* fixed ceres tests

* more flaky fixes

* Fixing tests without libpointmatcher

* Added RANSAC rejection filter to PCL ICP

* fixing multi platform flakiness

* Added test to detect regression

* Fixing windows pcl link error

* fixed some macos flakiness

* bigger 2D2D registration error on opencv 4.6.0

* flakiness

* fixing flaky tests on windows and mac

* flaky thread test on slow mac VM

* windows slow test

* fixing more ci erros

* fxing temp dir on windows

* Added Optimizer tests and discovered some bugs (fixed)

* fixing flaky tests in mac and windows

* Added Optimizer doc

* 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.

* fixing build without gtsam

* fixing home dir

* fixing python ci isssues

* Added multicam ba tests

* Added Ceres multicam BA support

* Aligned BundleAdjustment parameters with Optimizer/Strategy to avoid confusion in the code

* Added BA integration test

* Added robust graph optimization integration test

* Added loop3it test

* Added stereo20Hz test

* Added smartfactor gtsam

* Fixed bugged check and warn if python didn't return any descriptors

* Fixing gtsam version build issues

* fixing tilt on windows ci

* loosing ceres integration test for ci

* mac ci flakiness

* updating missing param in gui

* updating test bound for mac

* added appearance-based tests, set min gftt quality to quality level

* testing more stuff

* improving features2d tests

* ci flakiness

* fixing flaky ci

* ci fixes

* flaky fixes

* Added RegistrationIcp tests

* Added icp integration test with real-worl corridor like env

* intermediate nodes

* fixing enum

* Updated test to catch #1714

* Fixed 2d corridor failing on pcl

* flaky pnp test

* flaky brisk test

* Set rtabmap_integration test as long

* updating loop closure test

* flaky ci tests

* TEsting roundtrip g2o/toro save/load

* loosing test bound

* fixed cuda capable checks

* flaky tests

* Debugging test hanging

* more debugging stuff

* updating limit

* windows: disabled cuda on ci to avoid incompatible driver issue. Fixing a bad test mem allocation

* trying fixing cuda hanging issue

* fixing ci flakyness

* flaky tests

* Updated BOW flaky tests by checking min precision/recall instead of recall@100precision. Fixed signature test

* CameraModel::load() test initRectificationMap param

* test dbdriver load dictionary idsOnly

* Memory: test keepLinkedInDb param

* added dummyDictionary tests

* test intermediate nodes count

* Added MarkerDetector tests

* reverted breaking change of UMutex and USemaphore

* Features2d: fixed compiltion warnings with clang about override

* clang warnings

* fixing test build with pcl 1.8

* g2o and gtsam build errors on android

* opencv5 test fixes

* disabled testing for ios and android builds

* normalized endline characters for easier diff

* added LF CRLF rule

* bump 0.23.10. fixing doc version

* Publish rtabmap website doc from ci

* fixing MSCVC build error

* macos icp flaky test

* fixing ceres macos test bound

* ficing more flaky tests

* fixing opencv5 related test errors. Also fixed an actual bug in ENU_WGS84ToGeocentric_WGS84()

* added comment about mrpt change

* removed rosdoc2 (will add it for rtabmap_ros later)

* fixing website style

* updated download links

* locally deployable website with api

* sweep doxygen issues

* improved/revised doxygen main pages

* removed examples empty page

* Updated doxygen style

* more concise doxygen groups

* added api link on main readme

* fixing utilite test error

* fixing CommonFilteringGroundNormalsUp test

* updated precisionRecall test bounds for Freak and brief descriptors

* fixing scale check in ba tests

* disabled tests on windows cuda build (missing dlls amd runner cannot test cuda anyway)

* ceres: missing suitesparse dep in windows ci

* adjusting recall thr for fast/freak

* ficing more flaky tests

* fixing flaky tests

* disabled coverage in ros ci

* Enable integration tests for ros ci jobs

* loosing up some threshold for failing tests

* trigger cache

* fixing test data in ros ci. Updated flaky test for mac

* slaking some test limit

* Fixed rtabmap-detectMoreLoopClosures inverted output value

* loosing up sift recall on mac

* optimizer re-ordered distribution for reproducible results (mac g2o)

* macos dump test crash log

* combining all tests to save time on shared library reload. Also fixed Logs with missing arguments.

* Added ENABLE_FORMAT_ERRORS cmake option

* do test only one time

* fixed all format warnings

* format security android build errors

* less verbose tests

* updated ImuUThread test

* fixed a log

* Fixed libpointmatcher 2d normals eigen issue

* Fixing libpointmatcher conversion issues

* fixing libpointmatcher test on windows ci

* cleanup comments, relax some test thr

* disabled sequoia-intel ci build (too flaky, would need extensive testing directly on that machine)

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,22 +39,31 @@ 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/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>
 #include <gtsam/navigation/AttitudeFactor.h>
 #include <optimizer/gtsam/XYFactor.h>
 #include <optimizer/gtsam/XYZFactor.h>
+#include <optimizer/gtsam/PlanarBodyZFactor.h>
 #include <gtsam/nonlinear/ISAM2.h>
 
 #ifdef RTABMAP_VERTIGO
@@ -67,6 +77,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 +109,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 +1139,519 @@ 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
+
+#ifdef RTABMAP_GTSAM
+// Build a gtsam::Symbol for a 3D point. Word ids can be negative,
+// but gtsam symbol cannot.
+static inline gtsam::Symbol point3dSymbol(int id)
+{
+    return id < 0
+        ? gtsam::Symbol('L', static_cast<std::uint64_t>(-id))
+        : gtsam::Symbol('l', static_cast<std::uint64_t>(id));
+}
+#endif
+
+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 initialEstimate;
+
+	// 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);
+			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()));
+			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());
+	// 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 =
+				gtsam::noiseModel::mEstimator::Huber::Create(robustKernelDelta_);
+		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;
+		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;
+		}
+
+		// 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);
+		SmartMono::shared_ptr smartFactor;
+		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 = SmartMono::shared_ptr(new 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)
+		{
+			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(!initialEstimate.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));
+
+			if(smartFactor)
+			{
+				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
+			{
+				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);
+				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));
+			}
+		}
+
+		if(smartFactor && smartFactor->size() >= 2)
+		{
+			graph.add(smartFactor);
+			smartByWord[wordId] = smartFactor;
+		}
+	}
+
+	// 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.
+		gtsam::LevenbergMarquardtParams params;
+		if(epsilon() > 0.0)
+		{
+			params.relativeErrorTol = epsilon();
+			params.absoluteErrorTol = epsilon();
+		}
+		params.maxIterations    = iterations();
+		// 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(new gtsam::PCGSolverParameters());
+		gtsam::PreconditionerParameters::shared_ptr preconditioner(
+				new gtsam::BlockJacobiPreconditionerParameters());
+#if GTSAM_VERSION_NUMERIC >= 40300
+		// 4.3+: setter removed, fields renamed (epsilon_abs_ -> epsilon_abs).
+		pcg->preconditioner = preconditioner;
+		pcg->epsilon_abs = 1e-10;
+		pcg->epsilon_rel = 1e-10;
+#else
+		// Assign the member directly instead of calling setPreconditionerParams():
+		// the setter does exactly this but was only added after 4.0, and the
+		// Android build pins GTSAM 4.0.0.
+		pcg->preconditioner_ = preconditioner;
+		pcg->epsilon_abs_ = 1e-10;
+		pcg->epsilon_rel_ = 1e-10;
+#endif
+		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(initialEstimate), 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)
+	{
+		// 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())
+		{
+			auto 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))
+		{
+			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 */