Added Bundle Adjustment options to OdometryF2M (OdomF2M/BundleAdjustment) and RegistrationVis (Vis/BundleAdjustment). Added logger thread filter. RegistrationVis: Copy back corresponding input word IDs to output words when available. Memory::computeTransform() added option to use already computed corespondences if possible (proximity detection by time uses this). Updated Optimizer::optimizeBA() interface.

corelib/include/rtabmap/core/Optimizer.h

@@ -83,6 +83,8 @@ public:
 	void setEpsilon(double epsilon) {epsilon_ = epsilon;}
 	void setRobust(bool enabled) {robust_ = enabled;}
 
+	virtual void parseParameters(const ParametersMap & parameters);
+
 	// inherited classes should implement one of these methods
 	virtual std::map<int, Transform> optimize(
 			int rootId,
@@ -92,12 +94,24 @@ public:
 			double * finalError = 0,
 			int * iterationsDone = 0);
 	virtual std::map<int, Transform> optimizeBA(
+			int rootId,
+			const std::map<int, Transform> & poses,
+			const std::multimap<int, Link> & links,
+			const std::map<int, CameraModel> & models,
+			std::map<int, cv::Point3f> & points3DMap,
+			const std::map<int, std::map<int, cv::Point2f> > & wordReferences); // <ID words, IDs frames + keypoint>);
+
+	std::map<int, Transform> optimizeBA(
 			int rootId,
 			const std::map<int, Transform> & poses,
 			const std::multimap<int, Link> & links,
 			const std::map<int, Signature> & signatures);
 
-	virtual void parseParameters(const ParametersMap & parameters);
+	Transform optimizeBA(
+			const Link & link,
+			const CameraModel & model,
+			std::map<int, cv::Point3f> & points3DMap,
+			const std::map<int, std::map<int, cv::Point2f> > & wordReferences);
 
 	void computeBACorrespondences(
 			const std::map<int, Transform> & poses,
@@ -109,7 +123,7 @@ public:
 protected:
 	Optimizer(
 			int iterations         = Parameters::defaultOptimizerIterations(),
-			bool slam2d            = Parameters::defaultOptimizerSlam2D(),
+			bool slam2d            = Parameters::defaultRegForce3DoF(),
 			bool covarianceIgnored = Parameters::defaultOptimizerVarianceIgnored(),
 			double epsilon         = Parameters::defaultOptimizerEpsilon(),
 			bool robust            = Parameters::defaultOptimizerRobust());