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 <[email protected]>
Co-authored-by: matlabbe <[email protected]>

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;
 };
 
 }