refactor, debugging code

corelib/include/rtabmap/core/Statistics.h

@@ -83,6 +83,7 @@ class RTABMAP_CORE_EXPORT Statistics
 	RTABMAP_STATS(Loop, Optimization_max_error_removed_from_id, );
 	RTABMAP_STATS(Loop, Optimization_max_error_removed_to_id, );
 	RTABMAP_STATS(Loop, Optimization_max_error_removed_count, );
+	RTABMAP_STATS(Loop, Optimization_factors, );
 	RTABMAP_STATS(Loop, Linear_variance,);
 	RTABMAP_STATS(Loop, Angular_variance,);
 	RTABMAP_STATS(Loop, Landmark_detected,);

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

@@ -59,6 +59,17 @@ public:
 			double * finalError = 0,
 			int * iterationsDone = 0);
 
+	// True when iSAM2 (incremental) backend is active.
+	bool isIncremental() const {return isam2_ != 0;}
+
+	// Number of factors we believe are live in iSAM2.
+	std::size_t getTrackedFactorsCount() const {
+		return trackedFactors_.size() + (lastRootFactorIndex_.first != 0 ? 1 : 0);
+	}
+
+	// Live factor count reported by iSAM2 itself (should match getTrackedFactorsCount()).
+	std::size_t getISAM2LiveFactorsCount() const;
+
 private:
 	int internalOptimizerType_;
 
@@ -66,12 +77,7 @@ private:
 	int lastSwitchId_;
 	std::set<int> addedPoses_;
 	std::map<int, bool> isLandmarkWithRotation_; // persists across iSAM2 incremental calls
-	// Persistent map of non-self-referring constraints currently live in iSAM2,
-	// keyed by (min(from,to), max(from,to), type) -> factor index. Same
-	// (from,to) with a different Link::Type counts as a distinct constraint;
-	// direction flips of the same logical edge are folded together by the
-	// min/max normalization.
-	std::map<std::tuple<int, int, int>, std::uint64_t> trackedFactors_;
+	std::map<std::tuple<int, int, int>, std::uint64_t> trackedFactors_; // iSAM2 tracked constraints
 	std::pair<int, std::uint64_t> lastRootFactorIndex_;
 };
 

corelib/src/Rtabmap.cpp

@@ -29,6 +29,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/Version.h"
 #include "rtabmap/core/Features2d.h"
 #include "rtabmap/core/Optimizer.h"
+#include "rtabmap/core/optimizer/OptimizerGTSAM.h"
 #include "rtabmap/core/Graph.h"
 #include "rtabmap/core/Signature.h"
 
@@ -4231,6 +4232,12 @@ bool Rtabmap::process(
 			statistics_.addStatistic(Statistics::kLoopLandmark_detected_node_ref(), landmarksDetected.empty() || landmarksDetected.begin()->second.empty()?0:*landmarksDetected.begin()->second.begin());
 			statistics_.addStatistic(Statistics::kLoopVisual_inliers_mean_dist(), loopClosureVisualInliersMeanDist);
 			statistics_.addStatistic(Statistics::kLoopVisual_inliers_distribution(), loopClosureVisualInliersDistribution);
+			OptimizerGTSAM * gtsamOpt = dynamic_cast<OptimizerGTSAM*>(_graphOptimizer);
+			if(gtsamOpt != 0 && gtsamOpt->isIncremental())
+			{
+				statistics_.addStatistic(Statistics::kLoopOptimization_factors(), (float)gtsamOpt->getTrackedFactorsCount());
+				statistics_.addStatistic("Loop/Optimization_internal_factors", (float)gtsamOpt->getISAM2LiveFactorsCount());
+			}
 
 			statistics_.addStatistic(Statistics::kProximityTime_detections(), proximityDetectionsInTimeFound);
 			statistics_.addStatistic(Statistics::kProximitySpace_detections_added_visually(), proximityDetectionsAddedVisually);

corelib/src/optimizer/OptimizerGTSAM.cpp

@@ -90,6 +90,27 @@ bool OptimizerGTSAM::available()
 #endif
 }
 
+std::size_t OptimizerGTSAM::getISAM2LiveFactorsCount() const
+{
+#ifdef RTABMAP_GTSAM
+	if(isam2_ == 0)
+	{
+		return 0;
+	}
+	// iSAM2 keeps removed factors as null entries to preserve factor indices,
+	// so we have to skip nulls to get the actual live count.
+	const gtsam::NonlinearFactorGraph & factors = isam2_->getFactorsUnsafe();
+	std::size_t live = 0;
+	for(const gtsam::NonlinearFactorGraph::sharedFactor & f : factors)
+	{
+		if(f) ++live;
+	}
+	return live;
+#else
+	return 0;
+#endif
+}
+
 void OptimizerGTSAM::parseParameters(const ParametersMap & parameters)
 {
 	Optimizer::parseParameters(parameters);
@@ -941,6 +962,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 				{
 					UDEBUG("Update iSAM with the new factors");
 					result = isam2_->update(graph, initialEstimate, removeFactorIndices);
+
 #if BOOST_VERSION >= 106800
 					UASSERT(result.errorBefore.has_value());
 					UASSERT(result.errorAfter.has_value());