OptimizerG2O: track BA outliers per observation (#1741)

* OptimizerG2O: track BA outliers per observation

Track rejected BA projections by word and pose, preserving a landmark's optimized estimate whenever at least one observation remains active. Add a deterministic g2o regression and focused Linux CTest coverage.

* Changed some error logs to warning

---------

Co-authored-by: happyman <xiaochengwei@zkzcrobot.com>
Co-authored-by: matlabbe <matlabbe@gmail.com>

corelib/include/rtabmap/core/Optimizer.h

@@ -32,6 +32,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <map>
 #include <list>
+#include <set>
 #include <rtabmap/core/Link.h>
 #include <rtabmap/core/Parameters.h>
 #include <rtabmap/core/Signature.h>
@@ -64,6 +65,8 @@ public:
 	int cameraIndex;      ///< Index into the frame's camera model list for multi-camera rigs.
 };
 
+typedef std::map<int, std::set<int> > BAOutliers; // <word ID, rejected pose IDs>, matching wordReferences
+
 /**
  * @class Optimizer
  * @brief Abstract base for pose-graph and bundle-adjustment optimizers.
@@ -257,7 +260,7 @@ public:
 			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);
+			BAOutliers * outliers = 0);
 
 	/**
 	 * @brief BA wrapper that derives camera models and correspondences from signatures.
@@ -303,7 +306,7 @@ public:
 			const CameraModel & model,
 			std::map<int, cv::Point3f> & points3DMap,
 			const std::map<int, std::map<int, FeatureBA> > & wordReferences,
-			std::set<int> * outliers = 0);
+			BAOutliers * outliers = 0);
 
 	/**
 	 * @brief Build BA correspondences (3D points + per-frame observations) from signatures.

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

@@ -56,7 +56,7 @@ public:
 			const std::map<int, std::vector<CameraModel> > & models,
 			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);
+			BAOutliers * outliers = 0);
 };
 
 } /* namespace rtabmap */

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

@@ -71,7 +71,7 @@ public:
 			const std::map<int, std::vector<CameraModel> > & models, // in case of stereo, Tx should be set
 			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);
+			BAOutliers * outliers = 0);
 
 private:
 	double pixelVariance_;

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

@@ -71,7 +71,7 @@ public:
 			const std::map<int, std::vector<CameraModel> > & models, // in case of stereo, Tx should be set
 			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);
+			BAOutliers * outliers = 0);
 
 	bool saveGraph(
 		const std::string & fileName,

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

@@ -65,7 +65,7 @@ public:
 			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);
+			BAOutliers * outliers = 0);
 
 private:
 	int internalOptimizerType_;

corelib/src/Memory.cpp

@@ -3711,7 +3711,7 @@ Transform Memory::computeTransform(
 
 				UDEBUG("sba...start");
 				// set root negative to fix all other poses
-				std::set<int> sbaOutliers;
+				BAOutliers sbaOutliers;
 				UTimer bundleTimer;
 				OptimizerG2O sba(parameters_);
 				sba.setIterations(5);
@@ -3719,24 +3719,34 @@ Transform Memory::computeTransform(
 				bundlePoses = sba.optimizeBA(-toS.id(), bundlePoses, bundleLinks, bundleModels, points3DMap, wordReferences, &sbaOutliers);
 				UDEBUG("sba...end");
 
-				UDEBUG("bundleTime=%fs (poses=%d wordRef=%d outliers=%d)", bundleTime.ticks(), (int)bundlePoses.size(), totalWordReferences, (int)sbaOutliers.size());
+				int sbaOutliersCount = 0;
+				for(unsigned int i=0; i<info->inliersIDs.size(); ++i)
+				{
+					BAOutliers::const_iterator iter = sbaOutliers.find(info->inliersIDs[i]);
+					if(iter != sbaOutliers.end() && iter->second.find(toS.id()) != iter->second.end())
+					{
+						++sbaOutliersCount;
+					}
+				}
+				UDEBUG("bundleTime=%fs (poses=%d wordRef=%d outliers=%d)", bundleTime.ticks(), (int)bundlePoses.size(), totalWordReferences, sbaOutliersCount);
 
 				UDEBUG("Local Bundle Adjustment Before: %s", transform.prettyPrint().c_str());
 				if(!bundlePoses.rbegin()->second.isNull())
 				{
-					if(sbaOutliers.size())
+					if(sbaOutliersCount)
 					{
 						std::vector<int> newInliers(info->inliersIDs.size());
 						int oi=0;
 						for(unsigned int i=0; i<info->inliersIDs.size(); ++i)
 						{
-							if(sbaOutliers.find(info->inliersIDs[i]) == sbaOutliers.end())
+							BAOutliers::const_iterator iter = sbaOutliers.find(info->inliersIDs[i]);
+							if(iter == sbaOutliers.end() || iter->second.find(toS.id()) == iter->second.end())
 							{
 								newInliers[oi++] = info->inliersIDs[i];
 							}
 						}
 						newInliers.resize(oi);
-						UDEBUG("BA outliers ratio %f", float(sbaOutliers.size())/float(info->inliersIDs.size()));
+						UDEBUG("BA outliers ratio %f", float(sbaOutliersCount)/float(info->inliersIDs.size()));
 						info->inliers = (int)newInliers.size();
 						info->inliersIDs = newInliers;
 					}

corelib/src/Optimizer.cpp

@@ -476,8 +476,12 @@ std::map<int, Transform> Optimizer::optimizeBA(
 		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)
+		BAOutliers * outliers)
 {
+	if(outliers)
+	{
+		outliers->clear();
+	}
 	UERROR("Optimizer %d doesn't implement optimizeBA() method.", (int)this->type());
 	return std::map<int, Transform>();
 }
@@ -563,7 +567,7 @@ Transform Optimizer::optimizeBA(
 		const CameraModel & model,
 		std::map<int, cv::Point3f> & points3DMap,
 		const std::map<int, std::map<int, FeatureBA> > & wordReferences,
-		std::set<int> * outliers)
+		BAOutliers * outliers)
 {
 	std::map<int, Transform> poses;
 	poses.insert(std::make_pair(link.from(), Transform::getIdentity()));

corelib/src/RegistrationVis.cpp

@@ -1964,7 +1964,7 @@ Transform RegistrationVis::computeTransformationImpl(
 			models.insert(std::make_pair(2, cameraModelsTo));
 
 			std::map<int, std::map<int, FeatureBA> > wordReferences;
-			std::set<int> sbaOutliers;
+			BAOutliers sbaOutliers;
 			UDEBUG("");
 			for(unsigned int i=0; i<inliers.size(); ++i)
 			{
@@ -2041,25 +2041,35 @@ Transform RegistrationVis::computeTransformationImpl(
 			{
 				UDEBUG("Pose optimization: %s -> %s", transform.prettyPrint().c_str(), optimizedPoses.rbegin()->second.prettyPrint().c_str());
 
-				if(sbaOutliers.size())
+				int sbaOutliersCount = 0;
+				for(unsigned int i=0; i<inliers.size(); ++i)
+				{
+					BAOutliers::const_iterator iter = sbaOutliers.find(inliers[i]);
+					if(iter != sbaOutliers.end() && iter->second.find(2) != iter->second.end())
+					{
+						++sbaOutliersCount;
+					}
+				}
+				if(sbaOutliersCount)
 				{
 					std::vector<int> newInliers(inliers.size());
 					int oi=0;
 					for(unsigned int i=0; i<inliers.size(); ++i)
 					{
-						if(sbaOutliers.find(inliers[i]) == sbaOutliers.end())
+						BAOutliers::const_iterator iter = sbaOutliers.find(inliers[i]);
+						if(iter == sbaOutliers.end() || iter->second.find(2) == iter->second.end())
 						{
 							newInliers[oi++] = inliers[i];
 						}
 					}
 					newInliers.resize(oi);
-					UDEBUG("BA outliers ratio %f", float(sbaOutliers.size())/float(inliers.size()));
+					UDEBUG("BA outliers ratio %f", float(sbaOutliersCount)/float(inliers.size()));
 					inliers = newInliers;
 				}
 				if((int)inliers.size() < _minInliers)
 				{
 					msg = uFormat("Not enough inliers after bundle adjustment %d/%d (matches=%d) between %d and %d",
-							(int)inliers.size(), _minInliers, (int)((int)inliers.size()+sbaOutliers.size()), fromSignature.id(), toSignature.id());
+							(int)inliers.size(), _minInliers, (int)(int)inliers.size()+sbaOutliersCount, fromSignature.id(), toSignature.id());
 					transform.setNull();
 				}
 				else

corelib/src/odometry/OdometryF2M.cpp

@@ -26,7 +26,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
 #include "rtabmap/core/OdometryInfo.h"
-#include "rtabmap/core/Memory.h"
 #include "rtabmap/core/Signature.h"
 #include "rtabmap/core/RegistrationVis.h"
 #include "rtabmap/core/util3d.h"
@@ -41,6 +40,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/utilite/ULogger.h"
 #include "rtabmap/utilite/UTimer.h"
 #include "rtabmap/utilite/UMath.h"
+#include "rtabmap/utilite/UStl.h"
 #include "rtabmap/utilite/UConversion.h"
 #if CV_MAJOR_VERSION < 5
 #include <opencv2/calib3d/calib3d.hpp>
@@ -473,14 +473,23 @@ Transform OdometryF2M::computeTransform(
 
 							UDEBUG("sba...start");
 							// set root negative to fix all other poses
-							std::set<int> sbaOutliers;
+							BAOutliers sbaOutliers;
 							UTimer bundleTimer;
 							bundlePoses = sba_->optimizeBA(-lastFrame_->id(), bundlePoses, bundleLinks, bundleModels, points3DMap, wordReferences, &sbaOutliers);
 							bundleTime = bundleTimer.ticks();
 							UDEBUG("sba...end");
-							totalBundleOutliers = (int)sbaOutliers.size();
+							int sbaOutliersCount = 0;
+							for(unsigned int i=0; i<regInfo.inliersIDs.size(); ++i)
+							{
+								BAOutliers::const_iterator iter = sbaOutliers.find(regInfo.inliersIDs[i]);
+								if(iter != sbaOutliers.end() && iter->second.find(lastFrame_->id()) != iter->second.end())
+								{
+									++sbaOutliersCount;
+								}
+							}
+							totalBundleOutliers = sbaOutliersCount;
 
-							UDEBUG("bundleTime=%fs (poses=%d wordRef=%d outliers=%d)", bundleTime, (int)bundlePoses.size(), (int)bundleWordReferences_.size(), (int)sbaOutliers.size());
+							UDEBUG("bundleTime=%fs (poses=%d wordRef=%d outliers=%d)", bundleTime, (int)bundlePoses.size(), (int)bundleWordReferences_.size(), sbaOutliersCount);
 							if(info)
 							{
 								info->localBundlePoses = bundlePoses;
@@ -497,14 +506,15 @@ Transform OdometryF2M::computeTransform(
 									{
 										info->localBundleOutliersPerCam = std::vector<int>(lastFrameModels.size(),0);
 									}
-									if(sbaOutliers.size())
+									if(sbaOutliersCount)
 									{
 										regInfo.inliersPerCam = std::vector<int>(lastFrameModels.size(),0);
 										std::vector<int> newInliers(regInfo.inliersIDs.size());
 										int oi=0;
 										for(unsigned int i=0; i<regInfo.inliersIDs.size(); ++i)
 										{
-											if(sbaOutliers.find(regInfo.inliersIDs[i]) == sbaOutliers.end())
+											BAOutliers::const_iterator iter = sbaOutliers.find(regInfo.inliersIDs[i]);
+											if(iter == sbaOutliers.end() || iter->second.find(lastFrame_->id()) == iter->second.end())
 											{
 												newInliers[oi++] = regInfo.inliersIDs[i];
 												regInfo.inliersPerCam[wordReferences.at(regInfo.inliersIDs[i]).at(lastFrame_->id()).cameraIndex] += 1;
@@ -515,7 +525,7 @@ Transform OdometryF2M::computeTransform(
 											}
 										}
 										newInliers.resize(oi);
-										UDEBUG("BA outliers ratio %f", float(sbaOutliers.size())/float(regInfo.inliersIDs.size()));
+										UDEBUG("BA outliers ratio %f", float(sbaOutliersCount)/float(regInfo.inliersIDs.size()));
 										regInfo.inliers = (int)newInliers.size();
 										regInfo.inliersIDs = newInliers;
 									}

corelib/src/odometry/OdometryMono.cpp

@@ -476,9 +476,8 @@ Transform OdometryMono::computeTransform(SensorData & data, const Transform & gu
 								}
 							}
 
-							std::set<int> outliers;
 							UWARN("Bundle adjustment begin");
-							poses = ba->optimizeBA(poses.begin()->first, poses, links, models, localMap_, wordReferences, &outliers);
+							poses = ba->optimizeBA(poses.begin()->first, poses, links, models, localMap_, wordReferences);
 							UWARN("Bundle adjustment end");
 							if(!poses.empty())
 							{

corelib/src/optimizer/OptimizerCVSBA.cpp

@@ -60,8 +60,12 @@ std::map<int, Transform> OptimizerCVSBA::optimizeBA(
 		const std::map<int, std::vector<CameraModel> > & models,
 		std::map<int, cv::Point3f> & points3DMap,
 		const std::map<int, std::map<int, FeatureBA> > & wordReferences, // <ID words, IDs frames + keypoint/Disparity>)
-		std::set<int> * outliers)
+		BAOutliers * outliers)
 {
+	if(outliers)
+	{
+		outliers->clear();
+	}
 #ifdef RTABMAP_CVSBA
 	// run sba optimization
 	cvsba::Sba sba;

corelib/src/optimizer/OptimizerCeres.cpp

@@ -24,6 +24,7 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
+#include <algorithm>
 #include "rtabmap/core/Graph.h"
 
 #if CV_MAJOR_VERSION < 5
@@ -135,7 +136,10 @@ std::map<int, Transform> OptimizerCeres::optimize(
 	if(edgeConstraints.size()>=1 && poses.size()>=2 && iterations() > 0)
 	{
 		//Build problem
-		ceres::Problem problem;
+		// enable_fast_removal: the outlier pass below removes blocks by id.
+	ceres::Problem::Options problemOptions;
+	problemOptions.enable_fast_removal = true;
+	ceres::Problem problem(problemOptions);
 		std::map<int, ceres::examples::Pose2d> poses2d;
 		ceres::examples::MapOfPoses poses3d;
 
@@ -411,8 +415,12 @@ std::map<int, Transform> OptimizerCeres::optimizeBA(
 		const std::map<int, std::vector<CameraModel> > & models,
 		std::map<int, cv::Point3f> & points3DMap,
 		const std::map<int, std::map<int, FeatureBA> > & wordReferences, // <ID words, IDs frames + keypoint/Disparity>)
-		std::set<int> * outliers)
+		BAOutliers * outliers)
 {
+	if(outliers)
+	{
+		outliers->clear();
+	}
 #ifdef RTABMAP_CERES
 	// run sba optimization
 
@@ -509,6 +517,8 @@ std::map<int, Transform> OptimizerCeres::optimizeBA(
 	// 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);
+	// <word, pose> per observation: names the rejections, counts views left.
+	std::vector<std::pair<int,int> > obsWordPose(baProblem.num_observations_);
 
 	oi = 0;
 	for(std::map<int, std::map<int, FeatureBA> >::const_iterator iter=wordReferences.begin();
@@ -533,6 +543,7 @@ std::map<int, Transform> OptimizerCeres::optimizeBA(
 
 			baProblem.camera_index_[oi] = camIt->second;
 			baProblem.point_index_[oi] = pointIdToIndex.at(iter->first);
+			obsWordPose[oi] = std::make_pair(iter->first, poseId);
 			baProblem.observations_[4*oi] = jter->second.kpt.pt.x - m.cx();
 			baProblem.observations_[4*oi+1] = jter->second.kpt.pt.y - m.cy();
 			baProblem.observations_[4*oi+2] = m.fx();
@@ -574,6 +585,10 @@ std::map<int, Transform> OptimizerCeres::optimizeBA(
 	const double inv_sigma_d  = 1.0 / std::sqrt(disparityVariance_);
 	int monoObsCount   = 0;
 	int stereoObsCount = 0;
+	// Per observation: its residual block, and 2 (mono) or 3 (stereo) residuals,
+	// to slice Problem::Evaluate's flat vector into per-observation chi2.
+	std::vector<ceres::ResidualBlockId> obsBlockIds(baProblem.num_observations(), nullptr);
+	std::vector<int> obsResidualCount(baProblem.num_observations(), 2);
 	for (int i = 0; i < baProblem.num_observations(); ++i) {
 		const double u  = observations[4 * i];
 		const double v  = observations[4 * i + 1];
@@ -590,19 +605,26 @@ std::map<int, Transform> OptimizerCeres::optimizeBA(
 					u, v, observed_disparity[i], fx, fy, baseline_fx[i],
 					inv_sigma_uv, inv_sigma_d);
 			++stereoObsCount;
+			obsResidualCount[i] = 3;
 		}
 		else
 		{
 			// Mono (2 residuals: u, v).
-			cost_function = ceres::SnavelyReprojectionError::Create(u, v, fx, fy);
+			cost_function = ceres::SnavelyReprojectionError::Create(u, v, fx, fy, inv_sigma_uv);
 			++monoObsCount;
+			obsResidualCount[i] = 2;
 		}
 		// 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.
+		// Huber reads delta in |r| units but Optimizer/RobustKernelDelta is a chi^2
+		// threshold, so the knee deliberately sits above the rejection threshold: pass 1
+		// then only caps gross outliers, which keeps its estimate a good basis for
+		// deciding what to reject. Matching them throttles legitimate noise and costs
+		// accuracy on weakly constrained far points.
 		ceres::LossFunction* loss_function =
 				robustKernelDelta_ > 0.0 ? new ceres::HuberLoss(robustKernelDelta_) : nullptr;
-		problem.AddResidualBlock(cost_function,
+		obsBlockIds[i] = problem.AddResidualBlock(cost_function,
 								 loss_function,
 								 baProblem.mutable_camera_for_observation(i),
 								 baProblem.mutable_point_for_observation(i));
@@ -733,11 +755,45 @@ std::map<int, Transform> OptimizerCeres::optimizeBA(
 		UDEBUG("Ceres BA: %d multi-cam rigid edges", rigEdgeCount);
 	}
 
-	// 2D / planar BA mode: lock each non-root pose's primary (cam 0)
+	// Fixed cameras: rootId >= 0 fixes that pose, rootId < 0 fixes all but -rootId
+	// (the optimizeBA() contract). Without it the gauge is free, so poses the
+	// caller pinned drift away and take the landmarks with them.
+	int fixedCamCount = 0;
+	for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
+	{
+		const bool fixNode = (rootId >= 0 && iter->first == rootId) ||
+		                     (rootId <  0 && iter->first != -rootId);
+		if(!fixNode)
+		{
+			continue;
+		}
+		// All cameras of the rig, like g2o: the rig edges are stiff but not
+		// rigid, so pinning cam 0 alone leaves the others slightly free.
+		std::map<int, std::vector<CameraModel> >::const_iterator iterModel = models.find(iter->first);
+		const size_t rigSize = iterModel != models.end() ? iterModel->second.size() : 0;
+		for(size_t c = 0; c < rigSize; ++c)
+		{
+			std::map<std::pair<int,int>, int>::const_iterator camIt =
+					camIdxByKey.find(std::make_pair(iter->first, (int)c));
+			if(camIt == camIdxByKey.end())
+			{
+				continue;
+			}
+			double * cam_block = baProblem.cameras_ + camIt->second * 6;
+			// A pose with no observations and no links never entered the problem.
+			if(problem.HasParameterBlock(cam_block))
+			{
+				problem.SetParameterBlockConstant(cam_block);
+				++fixedCamCount;
+			}
+		}
+	}
+	UDEBUG("Ceres BA: %d fixed camera block(s) (rootId=%d)", fixedCamCount, rootId);
+
+	// 2D / planar BA mode: lock each non-fixed pose's primary (cam 0)
 	// vertex to its initial body-z (lateral motion + yaw stay free).
 	// Other cameras of a multi-cam rig follow via the rigid edges above.
-	// Root pose's cam 0 is fixed entirely so the gauge has no remaining
-	// z-DOF. Mirrors the g2o EdgeSBACamPrior path.
+	// Mirrors the g2o EdgeSBACamPrior path.
 	if(isSlam2d())
 	{
 		const double sqrtInfo = std::sqrt(1e9);  // matches g2o pinfo(2,2) = 1e9
@@ -755,7 +811,7 @@ std::map<int, Transform> OptimizerCeres::optimizeBA(
 			                     (rootId <  0 && iter->first != -rootId);
 			if(fixNode)
 			{
-				problem.SetParameterBlockConstant(cam_block);
+				// Already constant from the loop above.
 				continue;
 			}
 			// Unary planar constraint on the BODY z (the camera vertex is in
@@ -800,6 +856,11 @@ std::map<int, Transform> OptimizerCeres::optimizeBA(
 	// before the constraint is fully satisfied.
 	options.parameter_tolerance = 0.0;
 	options.gradient_tolerance  = 0.0;
+	// Pass 1 only needs to get close enough for bad residuals to stand out; pass 2
+	// re-solves with the full budget. 5 matches the g2o backend. With no kernel
+	// there is only one pass, so it gets everything.
+	const bool rejectOutliers = robustKernelDelta_ > 0.0 && baProblem.num_observations() > 0;
+	options.max_num_iterations = rejectOutliers ? std::min(5, iterations()) : iterations();
 	ceres::Solver::Summary summary;
 	ceres::Solve(options, &problem, &summary);
 	if(ULogger::level() == ULogger::kDebug)
@@ -813,6 +874,88 @@ std::map<int, Transform> OptimizerCeres::optimizeBA(
 		return poses;
 	}
 
+	// Hard rejection, like g2o and GTSAM. HuberLoss only down-weights: past the
+	// delta its gradient is constant, not zero, so an outlier keeps pulling the
+	// landmark however long the solve runs. Drop those blocks and re-solve. Runs
+	// even when the caller wants no report -- rejection is what fixes the estimate.
+	std::set<int> pointsToRestore;
+	if(rejectOutliers)
+	{
+		// apply_loss_function=false: threshold the raw chi^2, not the Huber cost.
+		ceres::Problem::EvaluateOptions evalOptions;
+		evalOptions.residual_blocks     = obsBlockIds;
+		evalOptions.apply_loss_function = false;
+		double residualCost = 0.0;
+		std::vector<double> residuals;
+		if(problem.Evaluate(evalOptions, &residualCost, &residuals, 0, 0))
+		{
+			// chi^2 > delta, the documented meaning of Optimizer/RobustKernelDelta.
+			std::map<int, int> observationsPerWord;
+			std::map<int, int> rejectedPerWord;
+			int rejectedCount = 0;
+			size_t offset = 0;
+			for(int i=0; i<baProblem.num_observations(); ++i)
+			{
+				const int wordId = obsWordPose[i].first;
+				++observationsPerWord[wordId];
+				double chi2 = 0.0;
+				for(int k=0; k<obsResidualCount[i] && offset+k < residuals.size(); ++k)
+				{
+					chi2 += residuals[offset+k] * residuals[offset+k];
+				}
+				offset += obsResidualCount[i];
+				if(chi2 > robustKernelDelta_)
+				{
+					if(outliers)
+					{
+						(*outliers)[wordId].insert(obsWordPose[i].second);
+					}
+					++rejectedPerWord[wordId];
+					++rejectedCount;
+					problem.RemoveResidualBlock(obsBlockIds[i]);
+				}
+			}
+			// A landmark with every view rejected is unconstrained: freeze it, and
+			// leave the caller's input estimate alone on readback.
+			for(std::map<int, int>::const_iterator iter=rejectedPerWord.begin(); iter!=rejectedPerWord.end(); ++iter)
+			{
+				if(iter->second == observationsPerWord.at(iter->first))
+				{
+					pointsToRestore.insert(iter->first);
+					double * point_block = baProblem.points_ + pointIdToIndex.at(iter->first) * 3;
+					if(problem.HasParameterBlock(point_block))
+					{
+						problem.SetParameterBlockConstant(point_block);
+					}
+				}
+			}
+			// Always run pass 2, even with nothing rejected: pass 1 was truncated.
+			// The parameter arrays hold pass 1's values, so it warm-starts free.
+			UDEBUG("Ceres BA: re-solving without %d rejected observation(s) over %d word(s), %d point(s) restored...",
+					rejectedCount, (int)rejectedPerWord.size(), (int)pointsToRestore.size());
+			options.max_num_iterations = iterations();
+			ceres::Solver::Summary reSummary;
+			ceres::Solve(options, &problem, &reSummary);
+			if(!reSummary.IsSolutionUsable())
+			{
+				// The first-pass solution still carries the outliers' pull, so it
+				// is not worth handing back. Empty on failure, per the
+				// Optimizer::optimizeBA() contract.
+				UWARN("ceres: re-solve without the %d rejected observation(s) is "
+					  "unusable, aborting optimization!", rejectedCount);
+				return std::map<int, Transform>();
+			}
+		}
+		else
+		{
+			// Without residuals there is no way to reject anything, and the first
+			// pass was deliberately truncated, so all we could hand back is an
+			// under-converged solution that still has its outliers in it.
+			UWARN("ceres: could not evaluate reprojection residuals, aborting optimization!");
+			return std::map<int, Transform>();
+		}
+	}
+
 	//update poses (read back from cam 0 of each rig -- the other cameras
 	//are rigidly constrained to it).
 	std::map<int, Transform> newPoses = poses;
@@ -864,13 +1007,17 @@ std::map<int, Transform> OptimizerCeres::optimizeBA(
 
 	}
 
-	//update 3D points
+	//update 3D points; the fully-rejected ones keep the caller's estimate.
 	oi = 0;
 	for(std::map<int, cv::Point3f>::iterator kter = points3DMap.begin(); kter!=points3DMap.end(); ++kter)
 	{
-		kter->second.x = baProblem.points_[oi++];
-		kter->second.y = baProblem.points_[oi++];
-		kter->second.z = baProblem.points_[oi++];
+		if(pointsToRestore.find(kter->first) == pointsToRestore.end())
+		{
+			kter->second.x = baProblem.points_[oi];
+			kter->second.y = baProblem.points_[oi+1];
+			kter->second.z = baProblem.points_[oi+2];
+		}
+		oi += 3;
 	}
 
 	return newPoses;

corelib/src/optimizer/OptimizerG2O.cpp

@@ -25,6 +25,7 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
+#include <algorithm>
 #include <rtabmap/utilite/ULogger.h>
 #include <rtabmap/utilite/UStl.h>
 #include <rtabmap/utilite/UMath.h>
@@ -1438,9 +1439,13 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 		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)
+		BAOutliers * outliers)
 {
 	std::map<int, Transform> optimizedPoses;
+	if(outliers)
+	{
+		outliers->clear();
+	}
 #if defined(RTABMAP_G2O) || defined(RTABMAP_ORB_SLAM)
 	UDEBUG("Optimizing graph...");
 
@@ -1544,6 +1549,7 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 
 
 		UDEBUG("fill %ld poses to g2o... (rootId=%d hasGravityConstraints=%d isSlam2d=%d)", poses.size(), rootId, hasGravityConstraints?1:0, isSlam2d()?1:0);
+		int freeCamVertices = 0;
 		for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
 		{
 			if(iter->first > 0)
@@ -1650,10 +1656,26 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 							iterModel->second[i].Tx(),
 							iterModel->second[i].Tx()<0.0?-iterModel->second[i].Tx()/iterModel->second[i].fx():baseline_,
 							camPose.prettyPrint().c_str());*/
+
+					if(!vCam->fixed())
+					{
+						++freeCamVertices;
+					}
 				}
 			}
 		}
 
+		// Every camera fixed leaves BlockSolver_6_3's pose block empty and g2o
+		// dereferences it unconditionally (fillCSparse -> SIGSEGV). Reachable via
+		// a negative rootId whose -rootId isn't in poses, so refuse instead.
+		if(freeCamVertices == 0)
+		{
+			UERROR("BA has no free camera vertex (rootId=%d, poses=%d): every pose "
+				   "is fixed, there is nothing for g2o to solve. Not optimizing.",
+					rootId, (int)poses.size());
+			return optimizedPoses;
+		}
+
 		UDEBUG("fill edges to g2o...");
 		for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
 		{
@@ -1809,7 +1831,13 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 			negVertexOffset += wordReferences.rbegin()->first;
 		}
 		UDEBUG("stepVertexId=%d, negVertexOffset=%d", stepVertexId, negVertexOffset);
-		std::list<g2o::OptimizableGraph::Edge*> edges;
+		struct EdgeReference
+		{
+			g2o::OptimizableGraph::Edge* edge;
+			int wordId;
+			int poseId;
+		};
+		std::list<EdgeReference> edges;
 		for(std::map<int, std::map<int, FeatureBA> >::const_iterator iter = wordReferences.begin(); iter!=wordReferences.end(); ++iter)
 		{
 			int id = iter->first;
@@ -1932,12 +1960,24 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 						if(robustKernelDelta_ > 0.0)
 						{
 							g2o::RobustKernelHuber* kernel = new g2o::RobustKernelHuber;
+							// Huber reads delta in |r| units but
+							// Optimizer/RobustKernelDelta is a chi^2 threshold, so the
+							// knee deliberately sits above the rejection threshold:
+							// pass 1 then only caps gross outliers, which keeps its
+							// estimate a good basis for deciding what to reject.
+							// Matching them throttles legitimate noise and costs
+							// accuracy on weakly constrained far points.
 							kernel->setDelta(robustKernelDelta_);
 							e->setRobustKernel(kernel);
 						}
 
-						optimizer.addEdge(e);
-						edges.push_back(e);
+						if(!optimizer.addEdge(e))
+						{
+							delete e;
+							UERROR("Failed adding BA observation for word %d in pose %d.", id, poseId);
+							return optimizedPoses;
+						}
+						edges.push_back(EdgeReference{e, id, poseId});
 					}
 				}
 			}
@@ -1957,7 +1997,9 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 
 		for(int i=0; i<(robustKernelDelta_>0.0?2:1); ++i)
 		{
-			it += optimizer.optimize(i==0&&robustKernelDelta_>0.0?5:iterations());
+			// Pass 1 only needs to expose the bad residuals; pass 2 gets the full
+			// budget. std::min so a caller asking for fewer than 5 gets that.
+			it += optimizer.optimize(i==0&&robustKernelDelta_>0.0?std::min(5, iterations()):iterations());
 
 			// early stop condition
 			optimizer.computeActiveErrors();
@@ -1980,45 +2022,24 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 
 			if(robustKernelDelta_>0.0)
 			{
-				for(std::list<g2o::OptimizableGraph::Edge*>::iterator iter=edges.begin(); iter!=edges.end();++iter)
+				for(std::list<EdgeReference>::iterator iter=edges.begin(); iter!=edges.end();++iter)
 				{
-					if((*iter)->level() == 0 && (*iter)->chi2() > (*iter)->robustKernel()->delta())
+					if(iter->edge->level() == 0 && iter->edge->chi2() > iter->edge->robustKernel()->delta())
 					{
-						(*iter)->setLevel(1);
+						iter->edge->setLevel(1);
 						++outliersCount;
 						double d = 0.0;
-#ifdef RTABMAP_ORB_SLAM
-						if(dynamic_cast<g2o::EdgeStereoSE3ProjectXYZ*>(*iter) != 0)
+	#ifdef RTABMAP_ORB_SLAM
+						if(dynamic_cast<g2o::EdgeStereoSE3ProjectXYZ*>(iter->edge) != 0)
 						{
-							d = ((g2o::EdgeStereoSE3ProjectXYZ*)(*iter))->measurement()[0]-((g2o::EdgeStereoSE3ProjectXYZ*)(*iter))->measurement()[2];
+							d = ((g2o::EdgeStereoSE3ProjectXYZ*)iter->edge)->measurement()[0]-((g2o::EdgeStereoSE3ProjectXYZ*)iter->edge)->measurement()[2];
 						}
-						//UDEBUG("Ignoring edge (%d<->%d) d=%f var=%f kernel=%f chi2=%f", (*iter)->vertex(0)->id()-stepVertexId, (*iter)->vertex(1)->id(), d, 1.0/((g2o::EdgeStereoSE3ProjectXYZ*)(*iter))->information()(0,0), (*iter)->robustKernel()->delta(), (*iter)->chi2());
-#else
-						if(dynamic_cast<g2o::EdgeProjectP2SC*>(*iter) != 0)
+	#else
+						if(dynamic_cast<g2o::EdgeProjectP2SC*>(iter->edge) != 0)
 						{
-							d = ((g2o::EdgeProjectP2SC*)(*iter))->measurement()[0]-((g2o::EdgeProjectP2SC*)(*iter))->measurement()[2];
-						}
-						//UDEBUG("Ignoring edge (%d<->%d) d=%f var=%f kernel=%f chi2=%f", (*iter)->vertex(0)->id()-stepVertexId, (*iter)->vertex(1)->id(), d, 1.0/((g2o::EdgeProjectP2SC*)(*iter))->information()(0,0), (*iter)->robustKernel()->delta(), (*iter)->chi2());
-#endif
-
-						int id=-1;
-						if((*iter)->vertex(0)->id() > negVertexOffset)
-						{
-							id = negVertexOffset - (*iter)->vertex(0)->id();
-						}
-						else
-						{
-							id = (*iter)->vertex(0)->id() - stepVertexId;
-						}
-						UASSERT_MSG(points3DMap.find(id) != points3DMap.end(), uFormat("word id=%d points3DMap=%ld vertex id=%d (negVertexOffset=%d stepVertexId=%d)",
-							id, points3DMap.size(), (*iter)->vertex(0)->id(), negVertexOffset, stepVertexId).c_str());
-						cv::Point3f pt3d = points3DMap.at(id);
-						((g2o::VertexSBAPointXYZ*)(*iter)->vertex(0))->setEstimate(Eigen::Vector3d(pt3d.x, pt3d.y, pt3d.z));
-
-						if(outliers)
-						{
-							outliers->insert((*iter)->vertex(0)->id()-stepVertexId);
+							d = ((g2o::EdgeProjectP2SC*)iter->edge)->measurement()[0]-((g2o::EdgeProjectP2SC*)iter->edge)->measurement()[2];
 						}
+	#endif
 						if(d < 5.0)
 						{
 							outliersCountFar++;
@@ -2031,6 +2052,35 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 				UDEBUG("outliers=%d outliersCountFar=%d", outliersCount, outliersCountFar);
 			}
 		}
+
+		std::map<int, int> edgesPerWord;
+		std::map<int, int> outlierEdgesPerWord;
+		for(std::list<EdgeReference>::const_iterator iter=edges.begin(); iter!=edges.end(); ++iter)
+		{
+			++edgesPerWord[iter->wordId];
+			if(iter->edge->level() != 0)
+			{
+				++outlierEdgesPerWord[iter->wordId];
+				if(outliers)
+				{
+					(*outliers)[iter->wordId].insert(iter->poseId);
+				}
+			}
+		}
+		std::set<int> pointsToRestore;
+		for(std::map<int, int>::const_iterator iter=outlierEdgesPerWord.begin(); iter!=outlierEdgesPerWord.end(); ++iter)
+		{
+			if(iter->second == edgesPerWord.at(iter->first))
+			{
+				pointsToRestore.insert(iter->first);
+			}
+		}
+		// Landmarks keeping at least one active projection are re-optimized; only
+		// the fully rejected ones fall back to the caller's estimate.
+		UDEBUG("words=%d, with rejected observations=%d (partially=%d, fully=%d)",
+				(int)edgesPerWord.size(), (int)outlierEdgesPerWord.size(),
+				(int)(outlierEdgesPerWord.size() - pointsToRestore.size()),
+				(int)pointsToRestore.size());
 		UDEBUG("g2o optimizing end (%d iterations done, error=%f, outliers=%d/%d (delta=%f) time = %f s)", it, optimizer.activeRobustChi2(), outliersCount, (int)edges.size(), robustKernelDelta_, timer.ticks());
 
 		if(optimizer.activeRobustChi2() > 1000000000000.0)
@@ -2120,9 +2170,13 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 
 			if(v)
 			{
-				cv::Point3f p(v->estimate()[0], v->estimate()[1], v->estimate()[2]);
-				//UDEBUG("%d from=%f,%f,%f to=%f,%f,%f", iter->first, iter->second.x, iter->second.y, iter->second.z, p.x, p.y, p.z);
-				iter->second = p;
+				// Keep an optimized landmark when at least one projection remains active.
+				// Otherwise, leave its input estimate untouched.
+				if(pointsToRestore.find(id) == pointsToRestore.end())
+				{
+					cv::Point3f p(v->estimate()[0], v->estimate()[1], v->estimate()[2]);
+					iter->second = p;
+				}
 			}
 			else
 			{

corelib/src/optimizer/OptimizerGTSAM.cpp

@@ -31,7 +31,9 @@ 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 <algorithm>
 #include <rtabmap/core/util3d.h>
+#include <rtabmap/core/util3d_transforms.h>
 #include <set>
 
 #include <rtabmap/core/optimizer/OptimizerGTSAM.h>
@@ -48,7 +50,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <gtsam/slam/BetweenFactor.h>
 #include <gtsam/slam/ProjectionFactor.h>
 #include <gtsam/slam/StereoFactor.h>
-#include <gtsam/slam/SmartProjectionPoseFactor.h>
 #include <gtsam/sam/BearingFactor.h>
 #include <gtsam/sam/BearingRangeFactor.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
@@ -1161,9 +1162,13 @@ std::map<int, Transform> OptimizerGTSAM::optimizeBA(
 		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)
+		BAOutliers * outliers)
 {
 	std::map<int, Transform> optimizedPoses;
+	if(outliers)
+	{
+		outliers->clear();
+	}
 #ifdef RTABMAP_GTSAM
 	UDEBUG("Optimizing BA graph...");
 
@@ -1322,11 +1327,24 @@ std::map<int, Transform> OptimizerGTSAM::optimizeBA(
 	}
 
 	// 4) 3D points + reprojection observations.
+	//
+	// Every landmark gets an explicit Point3 variable and one factor per
+	// observation, mono and stereo alike, matching g2o and Ceres. The GTSAM-native
+	// choice for mono would be a SmartProjectionPoseFactor, but it marginalizes
+	// the point out of the graph: no per-observation residual to threshold, and
+	// its readback triangulation is a plain DLT one gross outlier drags off by
+	// metres.
 	UDEBUG("GTSAM BA: adding %d 3D points and observations...", (int)points3DMap.size());
+
+	// Maps each observation factor back to its <word, pose> for the sweep below.
+	struct ObsFactor
+	{
+		size_t factorIndex;
+		int wordId;
+		int poseId;
+	};
+	std::vector<ObsFactor> obsFactors;
 	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 =
@@ -1338,32 +1356,33 @@ std::map<int, Transform> OptimizerGTSAM::optimizeBA(
 	const double sigmaDisparity = std::sqrt(disparityVariance_);
 	gtsam::SharedNoiseModel stereoNoiseModel = gtsam::noiseModel::Diagonal::Sigmas(
 			(gtsam::Vector(3) << sigmaPixel, sigmaDisparity, sigmaPixel).finished());
-	// 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::SharedNoiseModel monoNoiseModel =
 			gtsam::noiseModel::Isotropic::Sigma(2, sigmaPixel);
-	gtsam::SharedNoiseModel monoNoiseModel = monoIsotropicNoise;
 	if(robustKernelDelta_ > 0.0)
 	{
+		// Huber reads delta in |r| units but Optimizer/RobustKernelDelta is a chi^2
+		// threshold, so the knee deliberately sits above the rejection threshold: pass 1
+		// then only caps gross outliers, which keeps its estimate a good basis for
+		// deciding what to reject. Matching them throttles legitimate noise and costs
+		// accuracy on weakly constrained far points.
 		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;
+
+	// Pose variables, priors and links are identical in both passes; only the
+	// landmark part is rebuilt once observations are rejected.
+	const gtsam::NonlinearFactorGraph poseGraph = graph;
+	const gtsam::Values poseValues = initialEstimate;
+
+	// Adds every landmark and observation except those in `excluded`. A landmark
+	// with none left is dropped rather than added unconstrained, so readback keeps
+	// the caller's input estimate -- what a fully rejected point has to keep.
+	auto addLandmarks = [&](const BAOutliers & excluded)
+	{
+	obsFactors.clear();
+	insertedPoints.clear();
 	for(std::map<int, std::map<int, FeatureBA> >::const_iterator iter = wordReferences.begin(); iter!=wordReferences.end(); ++iter)
 	{
 		const int wordId = iter->first;
@@ -1378,117 +1397,75 @@ std::map<int, Transform> OptimizerGTSAM::optimizeBA(
 			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)
+		BAOutliers::const_iterator excludedIter = excluded.find(wordId);
+
+		// Collect usable observations first: none left means no variable at all.
+		std::vector<std::map<int, FeatureBA>::const_iterator> kept;
+		for(std::map<int, FeatureBA>::const_iterator jter = iter->second.begin(); jter != iter->second.end(); ++jter)
 		{
-			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))
+			const int poseId = jter->first;
+			const std::pair<int,int> camKey(poseId, jter->second.cameraIndex);
+			const gtsam::Symbol xkey('x', poseId * GTSAM_BA_MULTICAM_OFFSET + jter->second.cameraIndex);
+			if(poses.find(poseId) == poses.end() ||
+			   calMono.find(camKey) == calMono.end() ||
+			   !poseValues.exists(xkey) ||
+			   (excludedIter != excluded.end() && excludedIter->second.count(poseId)))
 			{
-				anyStereoForWord = true;
-				break;
+				continue;
 			}
+			kept.push_back(jter);
+		}
+		if(kept.empty())
+		{
+			continue;
 		}
 
 		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);
-		}
+		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)
+		for(size_t k=0; k<kept.size(); ++k)
 		{
-			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 int poseId    = kept[k]->first;
+			const FeatureBA & f = kept[k]->second;
+			const std::pair<int,int> camKey(poseId, f.cameraIndex);
+			const gtsam::Symbol xkey('x', poseId * GTSAM_BA_MULTICAM_OFFSET + f.cameraIndex);
 			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));
+			const size_t factorIdx = graph.size();
 
-			if(smartFactor)
-			{
-				smartFactor->add(gtsam::Point2(f.kpt.pt.x, f.kpt.pt.y), xkey);
-			}
-			else if(isStereo)
+			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);
+							wordId, poseId, f.cameraIndex, 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));
+						gtsam::Point2(f.kpt.pt.x, f.kpt.pt.y), monoNoiseModel, xkey, pkey, K));
 			}
-		}
-
-		if(smartFactor && smartFactor->size() >= 2)
-		{
-			graph.add(smartFactor);
-			smartByWord[wordId] = smartFactor;
+			obsFactors.push_back(ObsFactor{factorIdx, wordId, poseId});
 		}
 	}
+	};
+	addLandmarks(BAOutliers());
 
-	// 5) Optimize.
+	// 5) Optimize. Wrapped so the rejection pass can re-solve; false = gave up.
 	UTimer timer;
 	gtsam::Values result;
 	double finalError = std::numeric_limits<double>::quiet_NaN();
+	auto solveGraph = [&](int maxIterations) -> bool
+	{
 	try
 	{
 		// Always use Levenberg-Marquardt for BA, ignoring GTSAM/Optimizer.
@@ -1503,14 +1480,11 @@ std::map<int, Transform> OptimizerGTSAM::optimizeBA(
 			params.relativeErrorTol = epsilon();
 			params.absoluteErrorTol = epsilon();
 		}
-		params.maxIterations    = iterations();
+		params.maxIterations    = maxIterations;
 		// 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.
+		// Cholesky, which is faster here. The inner tolerances must be tight
+		// enough that the iterative solver doesn't bottom out before LM
+		// converges; 1e-10 comes from GTSAM's SFMExample_SmartFactorPCG.
 		params.linearSolverType = gtsam::NonlinearOptimizerParams::Iterative;
 		gtsam::PCGSolverParameters::shared_ptr pcg(new gtsam::PCGSolverParameters());
 		gtsam::PreconditionerParameters::shared_ptr preconditioner(
@@ -1530,7 +1504,7 @@ std::map<int, Transform> OptimizerGTSAM::optimizeBA(
 #endif
 		params.iterativeParams = pcg;
 		gtsam::NonlinearOptimizer * optimizer = new gtsam::LevenbergMarquardtOptimizer(graph, initialEstimate, params);
-		UDEBUG("GTSAM BA optimizing (max iterations=%d, robustKernel=%f)...", iterations(), robustKernelDelta_);
+		UDEBUG("GTSAM BA optimizing (max iterations=%d, robustKernel=%f)...", maxIterations, robustKernelDelta_);
 		result = optimizer->optimize();
 		finalError = optimizer->error();
 		UDEBUG("GTSAM BA done (initialError=%f finalError=%f time=%fs)", graph.error(initialEstimate), finalError, timer.ticks());
@@ -1539,40 +1513,84 @@ std::map<int, Transform> OptimizerGTSAM::optimizeBA(
 	catch(const gtsam::IndeterminantLinearSystemException & e)
 	{
 		UERROR("GTSAM BA: indeterminant linear system: %s", e.what());
-		return optimizedPoses;
+		return false;
 	}
 	catch(const std::exception & e)
 	{
 		UERROR("GTSAM BA failed: %s", e.what());
-		return optimizedPoses;
+		return false;
 	}
 
 	if(uIsNan(finalError))
 	{
 		UERROR("GTSAM BA produced a NaN error.");
+		return false;
+	}
+	return true;
+	};
+
+	// Pass 1 only needs to get close enough for bad residuals to stand out; pass 2
+	// re-solves with the full budget. 5 matches the g2o backend.
+	const bool rejectOutliers = robustKernelDelta_ > 0.0;
+	if(!solveGraph(rejectOutliers ? std::min(5, iterations()) : iterations()))
+	{
+		UWARN("GTSAM BA: solve failed, aborting optimization!");
 		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)
+	// 5b) Hard rejection, like g2o. The Huber kernel only down-weights, and that
+	//     residual pull keeps biasing the landmark however many iterations run.
+	//     Runs even when the caller wants no report -- rejection is what fixes
+	//     the estimate -- and pass 2 runs even with nothing rejected, since pass 1
+	//     was truncated.
+	BAOutliers rejected;
+	if(rejectOutliers)
 	{
-		const double thresholdSq = robustKernelDelta_ * robustKernelDelta_;
-		for(std::vector<std::pair<size_t, int> >::const_iterator iter = obsFactors.begin(); iter != obsFactors.end(); ++iter)
+		// chi^2 > delta, the documented meaning of Optimizer/RobustKernelDelta,
+		// matching OptimizerG2O. error() returns 0.5*rho(|r|), and rho == chi^2
+		// below the kernel knee -- which the whole rejection band sits under -- so
+		// 2*error is the raw chi^2 here. Past the knee rho still exceeds delta.
+		int rejectedCount = 0;
+		for(std::vector<ObsFactor>::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)
+			if(iter->factorIndex >= graph.size()) continue;
+			if(2.0 * graph.at(iter->factorIndex)->error(result) > robustKernelDelta_)
 			{
-				outliers->insert(iter->second);
+				rejected[iter->wordId].insert(iter->poseId);
+				++rejectedCount;
 			}
 		}
-		UDEBUG("GTSAM BA: %d outlier observations flagged.", (int)outliers->size());
+		UDEBUG("GTSAM BA: re-solving without %d rejected observation(s) over %d word(s)...",
+				rejectedCount, (int)rejected.size());
+		const gtsam::Values firstPass = result;
+		graph = poseGraph;
+		initialEstimate = poseValues;
+		addLandmarks(rejected);
+		// Warm-start from pass 1 where the variable survived, like g2o.
+		const auto warmStartKeys = initialEstimate.keys();
+		for(const gtsam::Key & key : warmStartKeys)
+		{
+			if(firstPass.exists(key))
+			{
+				initialEstimate.update(key, firstPass.at(key));
+			}
+		}
+		if(!solveGraph(iterations()))
+		{
+			// Rejection left something unsolvable (a landmark down to one ray).
+			// The first-pass solution still carries the outliers' pull, so it is
+			// not worth handing back -- fail like the other solver paths do.
+			UWARN("GTSAM BA: re-solve without the %d rejected observation(s) failed, "
+				  "aborting optimization!", rejectedCount);
+			return optimizedPoses;
+		}
+	}
+	if(outliers)
+	{
+		*outliers = rejected;
 	}
 
-	// 7) Read back poses (camera frame -> body frame via localTransform^-1).
+	// 6) 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)
@@ -1615,24 +1633,10 @@ std::map<int, Transform> OptimizerGTSAM::optimizeBA(
 		optimizedPoses.insert(std::make_pair(iter->first, t));
 	}
 
-	// 8) Read back 3D points.
+	// 7) Read back 3D points. Fully rejected landmarks were never added as
+	//    variables, so their input estimate stands.
 	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))
 		{

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

@@ -19,8 +19,10 @@ namespace ceres {
 // parameterized using 6 parameters: 3 for rotation, 3 for translation. The principal point is not modeled
 // (i.e. it is assumed be located at the image center).
 struct SnavelyReprojectionError {
-  SnavelyReprojectionError(double observed_x, double observed_y, double fx, double fy)
-      : observed_x(observed_x), observed_y(observed_y), fx(fx), fy(fy) {}
+  SnavelyReprojectionError(double observed_x, double observed_y, double fx, double fy,
+                           double inv_sigma_uv)
+      : observed_x(observed_x), observed_y(observed_y), fx(fx), fy(fy),
+        inv_sigma_uv(inv_sigma_uv) {}
 
   template <typename T>
   bool operator()(const T* const camera,
@@ -43,9 +45,10 @@ struct SnavelyReprojectionError {
     T predicted_x = fx * xp;
     T predicted_y = fy * yp;
 
-    // The error is the difference between the predicted and observed position.
-    residuals[0] = predicted_x - observed_x;
-    residuals[1] = predicted_y - observed_y;
+    // Whitened by 1/sigma so Optimizer/PixelVariance means the same here as in
+    // the stereo functor and the g2o / GTSAM backends.
+    residuals[0] = inv_sigma_uv * (predicted_x - observed_x);
+    residuals[1] = inv_sigma_uv * (predicted_y - observed_y);
 
     return true;
   }
@@ -54,14 +57,16 @@ struct SnavelyReprojectionError {
   static ceres::CostFunction* Create(const double observed_x,
                                      const double observed_y,
 									 const double fx,
-									 const double fy) {
+									 const double fy,
+									 const double inv_sigma_uv) {
     return (new ceres::AutoDiffCostFunction<SnavelyReprojectionError, 2, 6, 3>(
-                new SnavelyReprojectionError(observed_x, observed_y, fx, fy)));
+                new SnavelyReprojectionError(observed_x, observed_y, fx, fy, inv_sigma_uv)));
   }
   double observed_x;
   double observed_y;
   double fx;
   double fy;
+  double inv_sigma_uv;
 };
 
 }

corelib/test/test_optimizer.cpp

@@ -2309,15 +2309,18 @@ INSTANTIATE_TEST_SUITE_P(
 		});
 
 // ---------------------------------------------------------------------------
-// Regression test: wordReferences with negative ids. Memory.cpp assigns
-// sequential negative ids (-1, -2, -3, ...) to features that aren't
-// quantized into the visual vocabulary, and OdometryF2M forwards them
-// straight into optimizeBA(). gtsam::Symbol packs the index into 56
-// unsigned bits and used to overflow on these ("Symbol index is too
-// large"); g2o remaps them via negVertexOffset; Ceres handles them too.
-// Build the standard BA scenario, then negate every point id so the
-// optimizer sees what F2M's wordReferences actually looks like in
-// production.
+// Regression test: wordReferences with negative ids. optimizeBA() puts no sign
+// restriction on them, and each backend remaps differently: g2o shifts landmark
+// vertices by negVertexOffset, GTSAM packs the index into a gtsam::Symbol's 56
+// unsigned bits and can overflow there ("Symbol index is too large"), Ceres
+// indexes its own array. Negate every point id of the standard scenario.
+//
+// No in-tree caller feeds negative ids in today, so this guards the API
+// contract rather than a live path: Memory.cpp's negatives are frame-local
+// (negIndex resets per signature) and never leave its own signatures, while
+// both BA entry points renumber first -- computeBACorrespondences() keys on
+// keypoint identity, and OdometryF2M's ids come from RegistrationVis, which
+// only generates non-negative ones.
 // ---------------------------------------------------------------------------
 class NegativeWordIdBaTest : public ::testing::TestWithParam<Optimizer::Type>
 {
@@ -2896,3 +2899,337 @@ TEST(OptimizerTest, CvsbaPoseGraphOptimizeReturnsEmpty)
 	std::map<int, Transform> out = opt->optimize(1, in, inLinks);
 	EXPECT_TRUE(out.empty()) << "CVSBA should not handle pose-graph optimize()";
 }
+
+
+// ---------------------------------------------------------------------------
+// BAOutlierRejectionTest -- how each BA backend handles gross observation
+// outliers, and what it reports back through the BAOutliers out-parameter.
+//
+// Every backend that implements optimizeBA() is held to the same contract:
+// reject the bad observations, report them per <word, pose>, keep refining the
+// landmarks that still have good views, and leave a landmark whose every view
+// is rejected at the caller's input estimate. Backends that only down-weight
+// outliers through an m-estimator do not meet that contract, and these tests
+// are meant to fail for them until they do.
+//
+// The scene is deliberately rigid on the pose side. rootId is negative, which
+// all three backends read as "fix every pose except -rootId", so the camera
+// geometry is locked and the problem reduces to structure refinement -- that is
+// what makes the landmark assertions exact rather than approximate. It still
+// needs one free pose, since OptimizerG2O refuses an all-fixed BA (see
+// EveryPoseFixedIsRefusedInsteadOfCrashing). Pose 10 is it: a duplicate of pose
+// 4 welded to it by an identity link with information 1e6, so the link residual
+// starts at zero and the free pose can't perturb anything. It carries no
+// observations, which doubles as coverage for unobserved poses surviving BA.
+//
+// Two landmarks, each a different outlier shape:
+//   word -42: 9 observations, 8 exact and pose 4's displaced by (+80,-80) px.
+//       Plenty of good data left, so the point must still land on truth. The id
+//       is negative so g2o has to map the landmark vertex back through
+//       negVertexOffset to attribute the outlier -- NegativeWordIdBaTest above
+//       covers negative ids for the solve, but never passes an outliers pointer.
+//   word 7: 2 observations (poses 1 and 9), both displaced by +/-100 px. Every
+//       observation is an outlier, so nothing is left to constrain the point
+//       and the caller's input estimate has to survive untouched.
+//
+// The 45 cm pose spacing matters: at 5 m depth with fx=100, a 4 m baseline puts
+// depth uncertainty near 6 cm/px. Tighter spacing makes depth so weakly
+// observable that it tests conditioning rather than outlier handling.
+//
+// CVSBA is excluded: it ignores rootId entirely (it has no fixed-camera
+// concept), so the locked-geometry premise doesn't hold for it, and no CI job
+// builds it.
+// ---------------------------------------------------------------------------
+
+namespace {
+
+// A measurement of `point` as seen from `pose`, i.e. the exact projection of
+// the ground-truth point. (offsetU, offsetV) displaces it in pixels to
+// manufacture a gross outlier.
+FeatureBA baObservation(
+		const cv::Point3f & point,
+		const Transform & pose,
+		const CameraModel & model,
+		float offsetU = 0.0f,
+		float offsetV = 0.0f)
+{
+	const Transform cameraPose = pose * model.localTransform();
+	const cv::Point3f pointInCamera = util3d::transformPoint(point, cameraPose.inverse());
+	float u = 0.0f;
+	float v = 0.0f;
+	model.reproject(pointInCamera.x, pointInCamera.y, pointInCamera.z, u, v);
+	return FeatureBA(cv::KeyPoint(u+offsetU, v+offsetV, 1.0f));
+}
+
+// The rejected poses reported for one word, or an empty set if the word wasn't
+// flagged at all -- lets a missing word be compared like a wrong one instead of
+// aborting the test on a bad at().
+std::set<int> outliersFor(const BAOutliers & outliers, int wordId)
+{
+	BAOutliers::const_iterator iter = outliers.find(wordId);
+	return iter != outliers.end() ? iter->second : std::set<int>();
+}
+
+float pointDistance(const cv::Point3f & a, const cv::Point3f & b)
+{
+	const cv::Point3f d = a - b;
+	return std::sqrt(d.x*d.x + d.y*d.y + d.z*d.z);
+}
+
+// The observations of `wordId` that were *not* reported as outliers.
+std::set<int> keptFor(const BAOutliers & all, const BAOutliers & rejected, int wordId)
+{
+	std::set<int> kept = outliersFor(all, wordId);
+	for(int poseId : outliersFor(rejected, wordId))
+	{
+		kept.erase(poseId);
+	}
+	return kept;
+}
+
+std::string formatOutliers(const BAOutliers & outliers)
+{
+	std::ostringstream stream;
+	for(BAOutliers::const_iterator iter=outliers.begin(); iter!=outliers.end(); ++iter)
+	{
+		stream << iter->first << ":";
+		for(std::set<int>::const_iterator jter=iter->second.begin(); jter!=iter->second.end(); ++jter)
+		{
+			stream << " " << *jter;
+		}
+		stream << "; ";
+	}
+	return stream.str();
+}
+
+class BAOutlierRejectionTest : public ::testing::TestWithParam<Optimizer::Type>
+{
+protected:
+	const int          partialWordId_   = -42;
+	const int          rejectedWordId_  = 7;
+	const cv::Point3f  partialTruth_    {5.0f, -0.1f, 0.1f};
+	const cv::Point3f  partialInitial_  {5.0f, -0.2f, 0.1f};
+	const cv::Point3f  rejectedTruth_   {5.0f,  0.0f, 0.0f};
+	const cv::Point3f  rejectedInitial_ {5.0f, -0.5f, 0.0f};
+
+	void SetUp() override
+	{
+		if(!Optimizer::isAvailable(GetParam()))
+		{
+			GTEST_SKIP() << optimizerTypeName(GetParam()) << " not built in";
+		}
+		ParametersMap parameters;
+		parameters.insert(ParametersPair(Parameters::kOptimizerRobustKernelDelta(), "8"));
+		opt_.reset(Optimizer::create(GetParam(), parameters));
+		ASSERT_NE(opt_.get(), nullptr);
+		opt_->setIterations(50);
+
+		model_ = CameraModel(100.0, 100.0, 320.0, 240.0,
+				CameraModel::opticalRotation(), 0.0, cv::Size(640, 480));
+	}
+
+	// Builds the scene described above. With withOutliers=false every
+	// observation is the exact projection of its ground-truth point, which
+	// isolates the outlier effect from the scene's own conditioning.
+	void buildScene(bool withOutliers, float partialOffsetPx = 80.0f)
+	{
+		for(int id=1; id<=10; ++id)
+		{
+			poses_.insert({id, Transform(0.0f, 0.45f*(id-1), 0.0f, 0.0f, 0.0f, 0.0f)});
+			models_.insert({id, std::vector<CameraModel>(1, model_)});
+		}
+		poses_.at(10) = poses_.at(4);
+		cv::Mat information = cv::Mat::eye(6, 6, CV_64FC1) * 1000000.0;
+		links_.insert({4, Link(4, 10, Link::kNeighbor, Transform::getIdentity(), information)});
+
+		points3DMap_.insert({partialWordId_,  partialInitial_});
+		points3DMap_.insert({rejectedWordId_, rejectedInitial_});
+
+		for(int id=1; id<=9; ++id)
+		{
+			const bool corrupt = withOutliers && id == 4;
+			wordReferences_[partialWordId_].insert({id, baObservation(
+					partialTruth_, poses_.at(id), model_,
+					corrupt ?  partialOffsetPx : 0.0f,
+					corrupt ? -partialOffsetPx : 0.0f)});
+			allObservations_[partialWordId_].insert(id);
+			if(corrupt)
+			{
+				corrupted_[partialWordId_].insert(id);
+			}
+		}
+		// Poses 1 and 9 -- the ends of the trajectory -- so that the two
+		// observations still triangulate well on their own. A short-baseline
+		// pair would fail the clean control for conditioning reasons alone.
+		const int rejectedPoses[2] = {1, 9};
+		for(int i=0; i<2; ++i)
+		{
+			const float offsetU = withOutliers ? (i == 0 ? 100.0f : -100.0f) : 0.0f;
+			wordReferences_[rejectedWordId_].insert({rejectedPoses[i], baObservation(
+					rejectedTruth_, poses_.at(rejectedPoses[i]), model_, offsetU)});
+			allObservations_[rejectedWordId_].insert(rejectedPoses[i]);
+			if(withOutliers)
+			{
+				corrupted_[rejectedWordId_].insert(rejectedPoses[i]);
+			}
+		}
+	}
+
+	// Runs BA over the scene. points3DMap_ is refined in place.
+	std::map<int, Transform> runBA(BAOutliers & outliers, int rootId = -10)
+	{
+		return opt_->optimizeBA(rootId, poses_, links_, models_, points3DMap_, wordReferences_, &outliers);
+	}
+
+	void expectPointNear(int wordId, const cv::Point3f & expected, float tolerance)
+	{
+		const cv::Point3f p = points3DMap_.at(wordId);
+		EXPECT_NEAR(p.x, expected.x, tolerance) << optimizerTypeName(GetParam()) << " word " << wordId;
+		EXPECT_NEAR(p.y, expected.y, tolerance) << optimizerTypeName(GetParam()) << " word " << wordId;
+		EXPECT_NEAR(p.z, expected.z, tolerance) << optimizerTypeName(GetParam()) << " word " << wordId;
+	}
+
+	// Every observation the scene contains, and exactly the subset buildScene()
+	// displaced, both as <word, poses>. Expectations are derived from these rather
+	// than restated as literals, so editing the scene cannot leave the assertions
+	// describing the old one.
+	BAOutliers                                     allObservations_;
+	BAOutliers                                     corrupted_;
+
+	std::unique_ptr<Optimizer>                     opt_;
+	CameraModel                                    model_;
+	std::map<int, Transform>                       poses_;
+	std::multimap<int, Link>                       links_;
+	std::map<int, std::vector<CameraModel> >       models_;
+	std::map<int, cv::Point3f>                     points3DMap_;
+	std::map<int, std::map<int, FeatureBA> >       wordReferences_;
+};
+
+}  // namespace
+
+TEST_P(BAOutlierRejectionTest, EveryPoseFixedIsRefusedInsteadOfCrashing)
+{
+	// A negative rootId whose -rootId isn't in poses fixes every camera, leaving
+	// g2o's BlockSolver_6_3 pose block empty -- LinearSolverCSparse dereferences
+	// that unconditionally and segfaults, so the backend has to decline the
+	// problem. Reaching the end of this test is the regression check; the
+	// assertion also rules out a partially filled result.
+	buildScene(true);
+	BAOutliers outliers;
+	const std::map<int, Transform> optimized = runBA(outliers, -11);
+
+	EXPECT_TRUE(optimized.empty() || optimized.size() == poses_.size())
+			<< optimizerTypeName(GetParam()) << " returned " << optimized.size()
+			<< " of " << poses_.size() << " poses";
+}
+
+TEST_P(BAOutlierRejectionTest, CleanObservationsRecoverBothLandmarks)
+{
+	// Control: no outliers anywhere, so both landmarks must snap onto truth from
+	// their 0.1 m / 0.5 m off estimates. If this fails the scene is badly
+	// conditioned for the backend and the outlier results below say nothing.
+	buildScene(false);
+	BAOutliers outliers;
+	ASSERT_FALSE(runBA(outliers).empty());
+
+	expectPointNear(partialWordId_,  partialTruth_,  0.01f);
+	expectPointNear(rejectedWordId_, rejectedTruth_, 0.01f);
+	EXPECT_TRUE(outliers.empty())
+			<< optimizerTypeName(GetParam()) << " flagged outliers on a clean scene: "
+			<< formatOutliers(outliers);
+}
+
+TEST_P(BAOutlierRejectionTest, RejectsBadObservationsAndReportsThem)
+{
+	// One BA run, four independent post-conditions. EXPECTs rather than ASSERTs so
+	// a backend meeting some clauses and not others reports all of them at once.
+	buildScene(true);
+	BAOutliers outliers;
+	const std::map<int, Transform> optimized = runBA(outliers);
+	ASSERT_FALSE(optimized.empty())
+			<< optimizerTypeName(GetParam()) << " BA returned no poses at all";
+	const std::string dump = formatOutliers(outliers);
+
+	// Pose 10 has no projection at all -- it is held only by its link to
+	// pose 4. A backend must still hand it back rather than silently drop it.
+	EXPECT_EQ(optimized.size(), poses_.size())
+			<< optimizerTypeName(GetParam()) << " did not return all poses";
+	EXPECT_TRUE(optimized.find(10) != optimized.end())
+			<< optimizerTypeName(GetParam()) << " dropped the observation-less pose";
+
+	// Word -42: 8 exact observations against 1 gross outlier. Rejection should
+	// win, moving the point from y=-0.2 to the truth's y=-0.1 as in the clean
+	// case. This is the clause a backend fails when it only down-weights -- the
+	// m-estimator's constant residual pull biases the landmark -- or when flagging
+	// any one edge resets the whole landmark to its input estimate.
+	expectPointNear(partialWordId_, partialTruth_, 0.01f);
+
+	// First pin what the scene actually holds, so the check below can't be
+	// vacuous: word -42 seen from poses 1-9 with only pose 4 displaced, word 7
+	// seen from poses 1 and 9 with both displaced.
+	ASSERT_EQ(allObservations_[partialWordId_],  std::set<int>({1, 2, 3, 4, 5, 6, 7, 8, 9}));
+	ASSERT_EQ(allObservations_[rejectedWordId_], std::set<int>({1, 9}));
+	ASSERT_EQ(corrupted_[partialWordId_],        std::set<int>({4}));
+	ASSERT_EQ(corrupted_[rejectedWordId_],       std::set<int>({1, 9}));
+
+	// Then: exactly those, attributed to the right word *and* pose -- a flat set
+	// of word ids could not say which view was the bad one.
+	EXPECT_EQ(outliers, corrupted_) << "got " << dump << "want " << formatOutliers(corrupted_);
+
+	// And spelled out per word, so both halves are readable and a partial match
+	// says which side went wrong. Over-reporting costs the caller good data, so
+	// the eight clean views of word -42 have to survive; word 7 keeps none.
+	EXPECT_EQ(outliersFor(outliers, partialWordId_), std::set<int>({4})) << dump;
+	EXPECT_EQ(keptFor(allObservations_, outliers, partialWordId_),
+			std::set<int>({1, 2, 3, 5, 6, 7, 8, 9})) << dump;
+	EXPECT_EQ(outliersFor(outliers, rejectedWordId_), std::set<int>({1, 9})) << dump;
+	EXPECT_TRUE(keptFor(allObservations_, outliers, rejectedWordId_).empty()) << dump;
+
+	// Word 7: both observations are outliers, so nothing constrains the point once
+	// they go and whatever the solver left is meaningless. The write-back is
+	// skipped outright, so it must come back bit-identical, not merely close.
+	const cv::Point3f rejectedOut = points3DMap_.at(rejectedWordId_);
+	EXPECT_EQ(rejectedOut.x, rejectedInitial_.x) << optimizerTypeName(GetParam());
+	EXPECT_EQ(rejectedOut.y, rejectedInitial_.y) << optimizerTypeName(GetParam());
+	EXPECT_EQ(rejectedOut.z, rejectedInitial_.z) << optimizerTypeName(GetParam());
+
+	// The contrast that makes the clause above meaningful: word -42 still has
+	// eight good views, so it must NOT be restored. A backend that falls back on
+	// any rejection rather than only on total rejection leaves both landmarks at
+	// their input estimates, and only this check separates the two behaviours.
+	EXPECT_GT(pointDistance(points3DMap_.at(partialWordId_), partialInitial_), 0.05f)
+			<< optimizerTypeName(GetParam()) << " left the partially rejected landmark"
+			   " at its input estimate instead of optimizing it";
+	EXPECT_LT(pointDistance(points3DMap_.at(rejectedWordId_), rejectedInitial_), 1e-6f)
+			<< optimizerTypeName(GetParam()) << " moved the fully rejected landmark";
+}
+
+TEST_P(BAOutlierRejectionTest, RejectsOnDocumentedChi2Threshold)
+{
+	// The backends must agree on *where* the threshold is, not merely that they
+	// have one. Optimizer/RobustKernelDelta documents itself as a chi2 threshold,
+	// so the default 8 rejects at chi2 > 8, i.e. |r| > sqrt(8) ~ 2.83 sigma. The
+	// Huber knee deliberately sits higher, at 8 sigma (see the backends' notes).
+	//
+	// A (+4,-4) px displacement at sigma = 1 px gives chi2 = 32, inside the band
+	// (8, 64]: rejected on the documented chi2 reading, silently kept if a
+	// backend drifts to reading delta as |r|.
+	buildScene(true, 4.0f);
+	BAOutliers outliers;
+	ASSERT_FALSE(runBA(outliers).empty());
+
+	EXPECT_EQ(outliers, corrupted_)
+			<< optimizerTypeName(GetParam()) << ": " << formatOutliers(outliers);
+	EXPECT_EQ(keptFor(allObservations_, outliers, partialWordId_),
+			std::set<int>({1, 2, 3, 5, 6, 7, 8, 9}))
+			<< optimizerTypeName(GetParam()) << ": " << formatOutliers(outliers);
+}
+
+INSTANTIATE_TEST_SUITE_P(
+		BABackends,
+		BAOutlierRejectionTest,
+		::testing::Values(Optimizer::kTypeG2O, Optimizer::kTypeGTSAM, Optimizer::kTypeCeres),
+		[](const ::testing::TestParamInfo<Optimizer::Type> & info)
+		{
+			return optimizerTypeName(info.param);
+		});

corelib/test/test_rtabmap_integration.cpp

@@ -1097,7 +1097,8 @@ TEST_F(RtabmapIntegrationFixture, Stereo20Hz)
 				/*align2D=*/false);
 		std::cerr << "[" << v.label << "] trans rmse=" << tRmse << "m max="
 				  << tMax << "m, rot rmse=" << rRmse << "deg max="
-				  << rMax << "deg\n";
+				  << rMax << "deg, replay=" << result.replayWallSeconds
+				  << "s odom=" << result.odomTotalSeconds << "s\n";
 
 		// Golden is BA-optimized; the test runs only the real-time SLAM
 		// pipeline with the matching BA backend, so the natural gap to