Implemented OptimizerG2O::optimimzeBA(). Updated PostProcessingDialog (g2o sba option). Fixed words descriptors not filled in rtabmap::getMap3D(). CameraModelD(): return 5 null coeff distorsions if not set

corelib/include/rtabmap/core/CameraModel.h

@@ -99,7 +99,7 @@ public:
 	cv::Mat K_raw() const {return K_;} //intrinsic camera matrix (before rectification)
 	cv::Mat D_raw() const {return D_;} //intrinsic distorsion matrix (before rectification)
 	cv::Mat K() const {return !P_.empty()?P_.colRange(0,3):K_;} // if P exists, return rectified version
-	cv::Mat D() const {return P_.empty()&&!D_.empty()?D_:cv::Mat::zeros(1,4,CV_64FC1);} // if P exists, return rectified version
+	cv::Mat D() const {return P_.empty()&&!D_.empty()?D_:cv::Mat::zeros(1,5,CV_64FC1);} // if P exists, return rectified version
 	cv::Mat R() const {return R_;} //rectification matrix
 	cv::Mat P() const {return P_;} //projection matrix
 

corelib/include/rtabmap/core/Memory.h

@@ -150,7 +150,8 @@ public:
 	SensorData getNodeData(int nodeId, bool uncompressedData = false, bool keepLoadedDataInMemory = true);
 	void getNodeWords(int nodeId,
 			std::multimap<int, cv::KeyPoint> & words,
-			std::multimap<int, cv::Point3f> & words3);
+			std::multimap<int, cv::Point3f> & words3,
+			std::multimap<int, cv::Mat> & wordsDescriptors);
 	SensorData getSignatureDataConst(int locationId) const;
 	std::set<int> getAllSignatureIds() const;
 	bool memoryChanged() const {return _memoryChanged;}

corelib/include/rtabmap/core/Optimizer.h

@@ -99,6 +99,13 @@ public:
 
 	virtual void parseParameters(const ParametersMap & parameters);
 
+	void computeBACorrespondences(
+			const std::map<int, Transform> & poses,
+			const std::multimap<int, Link> & links,
+			const std::map<int, Signature> & signatures,
+			std::map<int, cv::Point3f> & points3DMap,
+			std::map<int, std::map<int, cv::Point2f> > & wordReferences); // <ID words, IDs frames + keypoint>
+
 protected:
 	Optimizer(
 			int iterations         = Parameters::defaultOptimizerIterations(),

corelib/include/rtabmap/core/OptimizerCVSBA.h

@@ -44,29 +44,18 @@ public:
 			int iterations         = Parameters::defaultOptimizerIterations(),
 			bool slam2d            = Parameters::defaultOptimizerSlam2D(),
 			bool covarianceIgnored = Parameters::defaultOptimizerVarianceIgnored()) :
-		Optimizer(iterations, slam2d, covarianceIgnored),
-		inlierDistance_(0.02),
-		minInliers_(10){}
+		Optimizer(iterations, slam2d, covarianceIgnored) {}
 	OptimizerCVSBA(const ParametersMap & parameters) :
-		Optimizer(parameters),
-		inlierDistance_(0.02),
-		minInliers_(10){}
+		Optimizer(parameters) {}
 	virtual ~OptimizerCVSBA() {}
 
 	virtual Type type() const {return kTypeCVSBA;}
 
-	void setInlierDistance(float inlierDistance) {inlierDistance_ = inlierDistance;}
-	void setMinInliers(int minInliers) {minInliers_ = minInliers;}
-
 	virtual 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);
-
-private:
-	float inlierDistance_;
-	float minInliers_;
 };
 
 } /* namespace rtabmap */

corelib/include/rtabmap/core/OptimizerG2O.h

@@ -68,6 +68,12 @@ 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, Signature> & signatures);
+
 private:
 	int solver_;
 	int optimizer_;