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/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_;