0.15.2: Fixed Transform's not orthogonal rotation matrix causing problems with GTSAM (normalize rotation each time transforms are multiplied, and for backward compatibility automatically normalize rotation of all transforms loaded from old databases prior to 0.15.2). Added Vis/CorGuessMatchToProjection, Icp/PMMatcherKnn and Icp/PMMatcherEpsilon parameters. CameraRGB: fixed max scans number when filtering. Link: transfering maximum covariance when merging. OptimizerGTSAM: updated how covariance are copied based on GTSAM official example. util3d::loadBINCloud(): fixed dim parameter not used. DatabaseViewer: Added option in graph view to ignore intermediate nodes when optimizing the graph.

corelib/include/rtabmap/core/CameraRGB.h

@@ -96,10 +96,6 @@ public:
 		_scanNormalsK = normalsK;
 		_scanNormalsRadius = normalsRadius;
 		_scanVoxelSize = voxelSize;
-		if(_scanDownsampleStep>1)
-		{
-			_scanMaxPts /= _scanDownsampleStep;
-		}
 	}
 
 	void setDepthFromScan(bool enabled, int fillHoles = 1, bool fillHolesFromBorder = false)

corelib/include/rtabmap/core/OdometryF2M.h

@@ -80,6 +80,7 @@ private:
 	std::map<int, int> bundlePoseReferences_;
 	int bundleSeq_;
 	Optimizer * sba_;
+	ParametersMap parameters_;
 };
 
 }

corelib/include/rtabmap/core/Parameters.h

@@ -510,6 +510,7 @@ class RTABMAP_EXP Parameters
     RTABMAP_PARAM(Vis, CorNNType,                int, 1,    uFormat("[%s=0] kNNFlannNaive=0, kNNFlannKdTree=1, kNNFlannLSH=2, kNNBruteForce=3, kNNBruteForceGPU=4. Used for features matching approach.", kVisCorType().c_str()));
     RTABMAP_PARAM(Vis, CorNNDR,                  float, 0.6,  uFormat("[%s=0] NNDR: nearest neighbor distance ratio. Used for features matching approach.", kVisCorType().c_str()));
     RTABMAP_PARAM(Vis, CorGuessWinSize,          int, 20,     uFormat("[%s=0] Matching window size (pixels) around projected points when a guess transform is provided to find correspondences. 0 means disabled.", kVisCorType().c_str()));
+    RTABMAP_PARAM(Vis, CorGuessMatchToProjection, bool, true, uFormat("[%s=0] Match frame's corners to source's projected points (when guess transform is provided) instead of projected points to frame's corners.", kVisCorType().c_str()));
     RTABMAP_PARAM(Vis, CorFlowWinSize,           int, 16,     uFormat("[%s=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.", kVisCorType().c_str()));
     RTABMAP_PARAM(Vis, CorFlowIterations,        int, 30,     uFormat("[%s=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.", kVisCorType().c_str()));
     RTABMAP_PARAM(Vis, CorFlowEps,               float, 0.01, uFormat("[%s=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.", kVisCorType().c_str()));
@@ -533,6 +534,8 @@ class RTABMAP_EXP Parameters
     // libpointmatcher
     RTABMAP_PARAM(Icp, PM,                       bool, false,   "Use libpointmatcher for ICP registration instead of PCL's implementation.");
     RTABMAP_PARAM_STR(Icp, PMConfig,             "",            uFormat("Configuration file (*.yaml) used by libpointmatcher. Note that data filters set for libpointmatcher are done after filtering done by rtabmap (i.e., %s, %s), so make sure to disable those in rtabmap if you want to use only those from libpointmatcher. Parameters %s, %s and %s are also ignored if configuration file is set.", kIcpVoxelSize().c_str(), kIcpDownsamplingStep().c_str(), kIcpIterations().c_str(), kIcpEpsilon().c_str(), kIcpMaxCorrespondenceDistance().c_str()).c_str());
+    RTABMAP_PARAM(Icp, PMMatcherKnn,             int, 1,        "KDTreeMatcher/knn: number of nearest neighbors to consider it the reference. For convenience when configuration file is not set.");
+    RTABMAP_PARAM(Icp, PMMatcherEpsilon,         float, 0.0,    "KDTreeMatcher/epsilon: approximation to use for the nearest-neighbor search. For convenience when configuration file is not set.");
     RTABMAP_PARAM(Icp, PMOutlierRatio,           float, 0.85,   "TrimmedDistOutlierFilter/ratio: For convenience when configuration file is not set. For kinect-like point cloud, use 0.65.");
 
     // Stereo disparity

corelib/include/rtabmap/core/RegistrationIcp.h

@@ -69,6 +69,8 @@ private:
 	float _pointToPlaneMinComplexity;
 	bool _libpointmatcher;
 	std::string _libpointmatcherConfig;
+	int _libpointmatcherKnn;
+	float _libpointmatcherEpsilon;
 	float _libpointmatcherOutlierRatio;
 	void * _libpointmatcherICP;
 };

corelib/include/rtabmap/core/RegistrationInfo.h

@@ -35,6 +35,7 @@ class RegistrationInfo
 {
 public:
 	RegistrationInfo() :
+		totalTime(0.0),
 		inliers(0),
 		matches(0),
 		icpInliersRatio(0),
@@ -48,6 +49,7 @@ public:
 	RegistrationInfo copyWithoutData() const
 	{
 		RegistrationInfo output;
+		output.totalTime = totalTime;
 		output.covariance = covariance.clone();
 		output.rejectedMsg = rejectedMsg;
 		output.inliers = inliers;
@@ -61,6 +63,7 @@ public:
 
 	cv::Mat covariance;
 	std::string rejectedMsg;
+	double totalTime;
 
 	// RegistrationVis
 	int inliers;

corelib/include/rtabmap/core/RegistrationVis.h

@@ -81,6 +81,7 @@ private:
 	int _flowMaxLevel;
 	float _nndr;
 	int _guessWinSize;
+	bool _guessMatchToProjection;
 	int _bundleAdjustment;
 
 	ParametersMap _featureParameters;

corelib/include/rtabmap/core/Transform.h

@@ -112,6 +112,7 @@ public:
 	float getDistance(const Transform & t) const;
 	float getDistanceSquared(const Transform & t) const;
 	Transform interpolate(float t, const Transform & other) const;
+	void normalizeRotation();
 	std::string prettyPrint() const;
 
 	Transform operator*(const Transform & t) const;