Fixed optimized map cleared when optimized graph is smaller than WM (…

…poses should still refer to nodes in WM). Removed warning about max scan points smaller than actual scan (this can happen when assembling scans for proximity detection). Lowering default Icp/PMOutlierRatio to 0.85.

corelib/include/rtabmap/core/Parameters.h

@@ -671,7 +671,7 @@ class RTABMAP_EXP Parameters
     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, PMMatcherIntensity,       bool, false,   uFormat("KDTreeMatcher:  among nearest neighbors, keep only the one with the most similar intensity. This only work with %s>1.", kIcpPMMatcherKnn().c_str()));
-    RTABMAP_PARAM(Icp, PMOutlierRatio,           float, 0.95,   "TrimmedDistOutlierFilter/ratio: For convenience when configuration file is not set. For kinect-like point cloud, use 0.65.");
+    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
     RTABMAP_PARAM(Stereo, WinWidth,              int, 15,       "Window width.");

corelib/src/LaserScan.cpp

@@ -330,7 +330,7 @@ void LaserScan::init(
 		}
 		else if(!is2D && maxPoints_>0 && data_.cols > maxPoints_)
 		{
-			UWARN("The number of points (%d) in the scan is over the maximum "
+			UDEBUG("The number of points (%d) in the scan is over the maximum "
 				  "points (%d) defined by max points setting.",
 				  data_.cols, maxPoints_);
 		}

corelib/src/Memory.cpp

@@ -2062,20 +2062,13 @@ std::map<int, Transform> Memory::loadOptimizedPoses(Transform * lastlocalization
 		bool ok = true;
 		std::map<int, Transform> poses = _dbDriver->loadOptimizedPoses(lastlocalizationPose);
 		// Make sure optimized poses match the working directory! Otherwise return nothing.
-		if(poses.size() == _workingMem.size())
+		for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end() && ok; ++iter)
 		{
-			for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end() && ok; ++iter)
+			if(_workingMem.find(iter->first)==_workingMem.end())
 			{
-				if(_workingMem.find(iter->first)==_workingMem.end())
-				{
-					ok = false;
-				}
+				ok = false;
 			}
 		}
-		else
-		{
-			ok = false;
-		}
 		if(!ok)
 		{
 			UWARN("Optimized poses (%d) and working memory "
@@ -3217,7 +3210,7 @@ Transform Memory::computeIcpTransformMulti(
 		// scans are in base frame but for 2d scans, set the height so that correspondences matching works
 		assembledData.setLaserScan(
 				LaserScan(assembledScan,
-					fromScan.maxPoints()?fromScan.maxPoints():maxPoints,
+					maxPoints,
 					fromScan.rangeMax(),
 					fromScan.is2d()?Transform(0,0,fromScan.localTransform().z(),0,0,0):Transform::getIdentity()));