Fixed broken 2D icp from 0.11.0

corelib/include/rtabmap/core/Parameters.h

@@ -402,8 +402,8 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(Icp, VoxelSize,                 float, 0.025, "Uniform sampling voxel size (0=disabled).");
 	RTABMAP_PARAM(Icp, DownsamplingStep,          int, 1, 	    "Downsampling step size (1=no sampling). This is done before uniform sampling.");
 	RTABMAP_PARAM(Icp, MaxCorrespondenceDistance, float, 0.05,  "Max distance for point correspondences.");
-	RTABMAP_PARAM(Icp, Iterations,                int, 10, 		"Max iterations.");
+	RTABMAP_PARAM(Icp, Iterations,                int, 30, 		"Max iterations.");
-	RTABMAP_PARAM(Icp, Epsilon,                   float, 0.001, "Set the transformation epsilon (maximum allowable difference between two consecutive transformations) in order for an optimization to be considered as having converged to the final solution.");
+	RTABMAP_PARAM(Icp, Epsilon,                   float, 0.0,   "Set the transformation epsilon (maximum allowable difference between two consecutive transformations) in order for an optimization to be considered as having converged to the final solution.");
 	RTABMAP_PARAM(Icp, CorrespondenceRatio,       float, 0.3,   "Ratio of matching correspondences to accept the transform.");
 	RTABMAP_PARAM(Icp, PointToPlane,              bool, false, 	"Use point to plane ICP.");
 	RTABMAP_PARAM(Icp, PointToPlaneNormalNeighbors, int, 20,    "Number of neighbors to compute normals for point to plane.");

corelib/src/DBDriverSqlite3.cpp

@@ -1779,6 +1779,7 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
 							(*iter)->sensorData().setCameraModels(models);
 							(*iter)->sensorData().setStereoCameraModel(stereoModel);
 						}
+						rc = sqlite3_step(ppStmt);
 					}
 					UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
 

corelib/src/Parameters.cpp

@@ -218,6 +218,8 @@ const std::map<std::string, std::pair<bool, std::string> > & Parameters::getRemo
 
 		removedParameters_.insert(std::make_pair("RGBD/LocalLoopDetectionByTime",              std::make_pair(true,  Parameters::kRGBDProximityByTime())));
 		removedParameters_.insert(std::make_pair("RGBD/LocalLoopDetectionBySpace",             std::make_pair(true,  Parameters::kRGBDProximityBySpace())));
+		removedParameters_.insert(std::make_pair("RGBD/LocalLoopDetectionTime",                std::make_pair(true,  Parameters::kRGBDProximityByTime())));
+		removedParameters_.insert(std::make_pair("RGBD/LocalLoopDetectionSpace",               std::make_pair(true,  Parameters::kRGBDProximityBySpace())));
 		removedParameters_.insert(std::make_pair("RGBD/LocalLoopDetectionPathScansMerged",     std::make_pair(true,  Parameters::kRGBDProximityPathScansMerged())));
 		removedParameters_.insert(std::make_pair("RGBD/LocalLoopDetectionMaxGraphDepth",       std::make_pair(true,  Parameters::kRGBDProximityMaxGraphDepth())));
 		removedParameters_.insert(std::make_pair("RGBD/LocalLoopDetectionPathFilteringRadius", std::make_pair(true,  Parameters::kRGBDProximityPathFilteringRadius())));

corelib/src/RegistrationIcp.cpp

@@ -223,7 +223,7 @@ Transform RegistrationIcp::computeTransformationImpl(
 						   hasConverged,
 						   *fromCloudRegistered,
 						   _epsilon,
-						   !this->force3DoF()); // icp2D
+						   this->force3DoF()); // icp2D
 				}
 
 				/*pcl::io::savePCDFile("fromCloud.pcd", *fromCloud);

corelib/src/Rtabmap.cpp

@@ -1002,7 +1002,7 @@ bool Rtabmap::process(
 				{
 					// set small variance
 					UDEBUG("Set small variance. The robot is not moving.");
-					_memory->updateLink(signature->id(), oldId, guess, 0.0001, 0.0001);
+					_memory->updateLink(oldId, signature->id(), guess, 0.0001, 0.0001);
 				}
 			}
 			else
@@ -1019,10 +1019,10 @@ bool Rtabmap::process(
 					if(!t.isNull())
 					{
 						UINFO("Scan matching: update neighbor link (%d->%d, variance=%f) from %s to %s",
-								signature->id(),
 								oldId,
+								signature->id(),
 								info.variance,
-								signature->getLinks().at(oldId).transform().prettyPrint().c_str(),
+								guess.prettyPrint().c_str(),
 								t.prettyPrint().c_str());
 						UASSERT(info.variance > 0.0);
 						_memory->updateLink(oldId, signature->id(), t, info.variance, info.variance);
@@ -1049,7 +1049,7 @@ bool Rtabmap::process(
 						if(info.variance > 0)
 						{
 							double sqrtVar = sqrt(info.variance);
-							_memory->updateLink(signature->id(), oldId, guess, sqrtVar, sqrtVar);
+							_memory->updateLink(oldId, signature->id(), guess, sqrtVar, sqrtVar);
 						}
 					}
 					statistics_.addStatistic(Statistics::kNeighborLinkRefiningAccepted(), !t.isNull()?1.0f:0);
@@ -1064,6 +1064,7 @@ bool Rtabmap::process(
 
 			UASSERT(oldS->hasLink(signature->id()));
 			UASSERT(uContains(_optimizedPoses, oldId));
+
 			newPose = _optimizedPoses.at(oldId) * oldS->getLinks().at(signature->id()).transform();
 			_mapCorrection = newPose * signature->getPose().inverse();
 			if(_mapCorrection.getNormSquared() > 0.001f && _optimizeFromGraphEnd)
@@ -1079,7 +1080,7 @@ bool Rtabmap::process(
 			newPose = _mapCorrection * signature->getPose();
 		}
 
-		UDEBUG("Added pose %s", newPose.prettyPrint().c_str());
+		UDEBUG("Added pose %s (odom=%s)", newPose.prettyPrint().c_str(), signature->getPose().prettyPrint().c_str());
 		// Update Poses and Constraints
 		_optimizedPoses.insert(std::make_pair(signature->id(), newPose));
 		_lastLocalizationPose = newPose; // keep in cache the latest corrected pose