Added RGBD/ProximityGlobalScanMap for proximity detection using the whole scan map in localization mode. Added Icp RMS statistics (CCCorelib). Updated how localization is corrected with gravity when available (simply use roll and pitch of current gravity constraint).

corelib/include/rtabmap/core/LaserScan.h

@@ -136,8 +136,19 @@ public:
 	int getRGBOffset() const {return hasRGB()?(is2d()?2:3):-1;}
 	int getNormalsOffset() const {return hasNormals()?(2 + (is2d()?0:1) + ((hasRGB() || hasIntensity())?1:0)):-1;}
 
+	float & field(unsigned int pointIndex, unsigned int channelOffset);
+
 	void clear() {data_ = cv::Mat();}
 
+	/**
+	 * Concatenate scan's data, localTransform is ignored.
+	 */
+	LaserScan & operator+=(const LaserScan &);
+	/**
+	 * Concatenate scan's data, localTransform is ignored.
+	 */
+	LaserScan operator+(const LaserScan &);
+
 private:
 	void init(const cv::Mat & data,
 			Format format,

corelib/include/rtabmap/core/Memory.h

@@ -242,6 +242,7 @@ public:
 
 	Transform computeTransform(Signature & fromS, Signature & toS, Transform guess, RegistrationInfo * info = 0, bool useKnownCorrespondencesIfPossible = false) const;
 	Transform computeTransform(int fromId, int toId, Transform guess, RegistrationInfo * info = 0, bool useKnownCorrespondencesIfPossible = false);
+	Transform computeIcpTransform(const Signature & fromS, const Signature & toS, Transform guess, RegistrationInfo * info = 0) const;
 	Transform computeIcpTransformMulti(
 			int newId,
 			int oldId,

corelib/include/rtabmap/core/Parameters.h

@@ -385,6 +385,7 @@ class RTABMAP_EXP Parameters
     RTABMAP_PARAM(RGBD, ProximityPathRawPosesUsed,    bool, true,  "When comparing to a local path for one-to-many proximity detection, merge the scans using the odometry poses (with neighbor link optimizations) instead of the ones in the optimized local graph.");
     RTABMAP_PARAM(RGBD, ProximityAngle,               float, 45,   "Maximum angle (degrees) for one-to-one proximity detection.");
     RTABMAP_PARAM(RGBD, ProximityOdomGuess,           bool, false, "Use odometry as motion guess for one-to-one proximity detection.");
+    RTABMAP_PARAM(RGBD, ProximityGlobalScanMap,       bool, false, uFormat("Create a global assembled map from laser scans for one-to-many proximity detection, replacing the original one-to-many proximity detection (i.e., detection against local paths). Only used in localization mode (%s=false), otherwise original one-to-many proximity detection is done. Note also that if graph is modified (i.e., memory management is enabled or robot jumps from one disjoint session to another in same database), the global scan map is cleared and one-to-many proximity detection is reverted to original approach.", kMemIncrementalMemory().c_str(), kRGBDProximityPathRawPosesUsed().c_str()));
 
     // Graph optimization
 #ifdef RTABMAP_GTSAM

corelib/include/rtabmap/core/RegistrationInfo.h

@@ -37,6 +37,7 @@ public:
 	RegistrationInfo() :
 		totalTime(0.0),
 		inliers(0),
+		inliersRatio(0),
 		inliersMeanDistance(0.0f),
 		inliersDistribution(0.0f),
 		matches(0),
@@ -45,7 +46,8 @@ public:
 		icpRotation(0.0f),
 		icpStructuralComplexity(0.0f),
 		icpStructuralDistribution(0.0f),
-		icpCorrespondences(0)
+		icpCorrespondences(0),
+		icpRMS(0)
 
 	{
 	}
@@ -90,6 +92,7 @@ public:
 	float icpStructuralComplexity;
 	float icpStructuralDistribution;
 	int   icpCorrespondences;
+	float icpRMS;
 };
 
 }

corelib/include/rtabmap/core/Rtabmap.h

@@ -248,6 +248,8 @@ private:
 	void updateGoalIndex();
 	bool computePath(int targetNode, std::map<int, Transform> nodes, const std::multimap<int, rtabmap::Link> & constraints);
 
+	void createGlobalScanMap();
+
 	void setupLogFiles(bool overwrite = false);
 	void flushStatisticLogs();
 
@@ -306,6 +308,7 @@ private:
 	bool _loopCovLimited;
 	bool _loopGPS;
 	int _maxOdomCacheSize;
+	bool _createGlobalScanMap;
 
 	std::pair<int, float> _loopClosureHypothesis;
 	std::pair<int, float> _highestHypothesis;
@@ -341,6 +344,8 @@ private:
 	int _lastLocalizationNodeId; // for localization mode
 	std::map<int, std::pair<cv::Point3d, Transform> > _gpsGeocentricCache;
 	bool _currentSessionHasGPS;
+	LaserScan _globalScanMap;
+	std::map<int, Transform> _globalScanMapPoses;
 	std::map<int, Transform> _odomCachePoses;       // used in localization mode to reject loop closures
 	std::multimap<int, Link> _odomCacheConstraints; // used in localization mode to reject loop closures
 	std::map<int, Transform> _odomCacheAddLink; // used in localization mode when adding external link

corelib/include/rtabmap/core/Statistics.h

@@ -122,7 +122,8 @@ class RTABMAP_EXP Statistics
 	RTABMAP_STATS(Proximity, Space_visual_paths_checked,);
 	RTABMAP_STATS(Proximity, Space_scan_paths_checked,);
 	RTABMAP_STATS(Proximity, Space_detections_added_visually,);
-	RTABMAP_STATS(Proximity, Space_detections_added_icp_only,);
+	RTABMAP_STATS(Proximity, Space_detections_added_icp_multi,);
+	RTABMAP_STATS(Proximity, Space_detections_added_icp_global,);
 
 	RTABMAP_STATS(NeighborLinkRefining, Accepted,);
 	RTABMAP_STATS(NeighborLinkRefining, Inliers,);

corelib/include/rtabmap/core/Transform.h

@@ -103,6 +103,7 @@ public:
 	Transform rotation() const;
 	Transform translation() const;
 	Transform to3DoF() const;
+	Transform to4DoF() const;
 
 	cv::Mat rotationMatrix() const;
 	cv::Mat translationMatrix() const;

corelib/include/rtabmap/core/util3d_surface.h

@@ -437,6 +437,12 @@ void RTABMAP_EXP adjustNormalsToViewPoints(
 		const std::vector<int> & rawCameraIndices,
 		pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud);
 
+void RTABMAP_EXP adjustNormalsToViewPoints(
+		const std::map<int, Transform> & viewpoints,
+		const LaserScan & rawScan,
+		const std::vector<int> & viewpointIds,
+		LaserScan & scan);
+
 pcl::PolygonMesh::Ptr RTABMAP_EXP meshDecimation(const pcl::PolygonMesh::Ptr & mesh, float factor);
 
 template<typename pointT>