Increased version to 0.20.7. OdometryF2M: added support for intensity field, removed ignored key frames when there is low scan complexity. RegistrationIcp: added Icp/PMMatcherIntensity, Icp/PointToPlaneGroundNormalsUp and Icp/PointToPlaneLowComplexityStrategy parameters. Rtabmap: when graph optimized from end, increased optimization error before warning that resulting map correction is not identity (this could happen with GTSAM as the root is not perfectly fixed). CloudViewer: added coordinate frame scaling option, added rainbow colormap option for scan intensity. DBViewer: fixed local proximity merged scans not shown modified after refining those links, show intensity, fixed constraints view not updated after rejecting a link. MainWindow: added intesity support with odometry scans.

corelib/include/rtabmap/core/CameraThread.h

@@ -72,6 +72,15 @@ public:
 	void enableIMUFiltering(int filteringStrategy=1, const ParametersMap & parameters = ParametersMap());
 	void disableIMUFiltering();
 
+	RTABMAP_DEPRECATED(void setScanParameters(
+			bool fromDepth,
+			int downsampleStep=1, // decimation of the depth image in case the scan is from depth image
+			float rangeMin=0.0f,
+			float rangeMax=0.0f,
+			float voxelSize = 0.0f,
+			int normalsK = 0,
+			int normalsRadius = 0.0f,
+			bool forceGroundNormalsUp = false) , "Use new version of this function with groundNormalsUp=0.8 for forceGroundNormalsUp=True and groundNormalsUp=0.0 for forceGroundNormalsUp=False.");
 	void setScanParameters(
 			bool fromDepth,
 			int downsampleStep=1, // decimation of the depth image in case the scan is from depth image
@@ -80,17 +89,7 @@ public:
 			float voxelSize = 0.0f,
 			int normalsK = 0,
 			int normalsRadius = 0.0f,
-			bool forceGroundNormalsUp = false)
-	{
-		_scanFromDepth = fromDepth;
-		_scanDownsampleStep=downsampleStep;
-		_scanRangeMin = rangeMin;
-		_scanRangeMax = rangeMax;
-		_scanVoxelSize = voxelSize;
-		_scanNormalsK = normalsK;
-		_scanNormalsRadius = normalsRadius;
-		_scanForceGroundNormalsUp = forceGroundNormalsUp;
-	}
+			float groundNormalsUp = 0.0f);
 
 	void postUpdate(SensorData * data, CameraInfo * info = 0) const;
 
@@ -119,7 +118,7 @@ private:
 	float _scanVoxelSize;
 	int _scanNormalsK;
 	float _scanNormalsRadius;
-	bool _scanForceGroundNormalsUp;
+	float _scanForceGroundNormalsUp;
 	StereoDense * _stereoDense;
 	clams::DiscreteDepthDistortionModel * _distortionModel;
 	bool _bilateralFiltering;

corelib/include/rtabmap/core/Parameters.h

@@ -643,13 +643,15 @@ class RTABMAP_EXP Parameters
     RTABMAP_PARAM(Icp, Epsilon,                   float, 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.1,   "Ratio of matching correspondences to accept the transform.");
 #ifdef RTABMAP_POINTMATCHER
-    RTABMAP_PARAM(Icp, PointToPlane,              bool, true,   "Use point to plane ICP.");
+    RTABMAP_PARAM(Icp, PointToPlane,                bool, true,   "Use point to plane ICP.");
 #else
-    RTABMAP_PARAM(Icp, PointToPlane,              bool, false,  "Use point to plane ICP.");
+    RTABMAP_PARAM(Icp, PointToPlane,                bool, false,  "Use point to plane ICP.");
 #endif
-    RTABMAP_PARAM(Icp, PointToPlaneK,             int, 5,       "Number of neighbors to compute normals for point to plane if the cloud doesn't have already normals.");
-    RTABMAP_PARAM(Icp, PointToPlaneRadius,        float, 1.0,   "Search radius to compute normals for point to plane if the cloud doesn't have already normals.");
-    RTABMAP_PARAM(Icp, PointToPlaneMinComplexity, float, 0.02,  "Minimum structural complexity (0.0=low, 1.0=high) of the scan to do point to plane registration, otherwise point to point registration is done instead.");
+    RTABMAP_PARAM(Icp, PointToPlaneK,               int, 5,       "Number of neighbors to compute normals for point to plane if the cloud doesn't have already normals.");
+    RTABMAP_PARAM(Icp, PointToPlaneRadius,          float, 1.0,   "Search radius to compute normals for point to plane if the cloud doesn't have already normals.");
+    RTABMAP_PARAM(Icp, PointToPlaneGroundNormalsUp, float, 0.0,   "Invert normals on ground if they are pointing down (useful for ring-like 3D LiDARs). 0 means disabled, 1 means only normals perfectly aligned with -z axis. This is only done with 3D scans.");
+    RTABMAP_PARAM(Icp, PointToPlaneMinComplexity,   float, 0.02,  uFormat("Minimum structural complexity (0.0=low, 1.0=high) of the scan to do PointToPlane registration, otherwise PointToPoint registration is done instead and strategy from %s is used. This check is done only when %s=true.", kIcpPointToPlaneLowComplexityStrategy().c_str(), kIcpPointToPlane().c_str()));
+    RTABMAP_PARAM(Icp, PointToPlaneLowComplexityStrategy, int, 1, uFormat("If structural complexity is below %s: set to 0 to so that the transform is automatically rejected, set to 1 to limit ICP correction in axes with most constraints (e.g., for a corridor-like environment, the resulting transform will be limited in y and yaw, x will taken from the guess), set to 2 to accept \"as is\" the transform computed by PointToPoint.", kIcpPointToPlaneMinComplexity().c_str()));
 
     // libpointmatcher
 #ifdef RTABMAP_POINTMATCHER
@@ -660,6 +662,7 @@ class RTABMAP_EXP Parameters
     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, 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.");
 
     // Stereo disparity

corelib/include/rtabmap/core/RegistrationIcp.h

@@ -69,11 +69,14 @@ private:
 	bool _pointToPlane;
 	int _pointToPlaneK;
 	float _pointToPlaneRadius;
+	float _pointToPlaneGroundNormalsUp;
 	float _pointToPlaneMinComplexity;
+	int _pointToPlaneLowComplexityStrategy;
 	bool _libpointmatcher;
 	std::string _libpointmatcherConfig;
 	int _libpointmatcherKnn;
 	float _libpointmatcherEpsilon;
+	bool _libpointmatcherIntensity;
 	float _libpointmatcherOutlierRatio;
 	void * _libpointmatcherICP;
 };

corelib/include/rtabmap/core/odometry/OdometryF2M.h

@@ -78,7 +78,7 @@ private:
 	Signature * map_;
 	Signature * lastFrame_;
 	int lastFrameOldestNewId_;
-	std::vector<std::pair<pcl::PointCloud<pcl::PointNormal>::Ptr, pcl::IndicesPtr> > scansBuffer_;
+	std::vector<std::pair<pcl::PointCloud<pcl::PointXYZINormal>::Ptr, pcl::IndicesPtr> > scansBuffer_;
 	bool initGravity_;
 
 	std::map<int, std::map<int, FeatureBA> > bundleWordReferences_; //<WordId, <FrameId, pt2D+depth>>

corelib/include/rtabmap/core/util3d_filtering.h

@@ -56,7 +56,16 @@ LaserScan RTABMAP_EXP commonFiltering(
 		float voxelSize = 0.0f,
 		int normalK = 0,
 		float normalRadius = 0.0f,
-		bool forceGroundNormalsUp = false);
+		float groundNormalsUp = 0.0f);
+RTABMAP_DEPRECATED(LaserScan RTABMAP_EXP commonFiltering(
+		const LaserScan & scan,
+		int downsamplingStep,
+		float rangeMin,
+		float rangeMax,
+		float voxelSize,
+		int normalK,
+		float normalRadius,
+		bool forceGroundNormalsUp), "Use version with groundNormalsUp as float. For forceGroundNormalsUp=true, set groundNormalsUp=0.8, otherwise set groundNormalsUp=0.0.");
 
 LaserScan RTABMAP_EXP rangeFiltering(
 		const LaserScan & scan,
@@ -288,6 +297,12 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cropBox(
 		const Eigen::Vector4f & max,
 		const Transform & transform = Transform::getIdentity(),
 		bool negative = false);
+pcl::PointCloud<pcl::PointXYZINormal>::Ptr RTABMAP_EXP cropBox(
+		const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
+		const Eigen::Vector4f & min,
+		const Eigen::Vector4f & max,
+		const Transform & transform = Transform::getIdentity(),
+		bool negative = false);
 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP cropBox(
 		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
 		const Eigen::Vector4f & min,
@@ -451,6 +466,12 @@ pcl::IndicesPtr RTABMAP_EXP subtractFiltering(
 /**
  * For convenience.
  */
+pcl::PointCloud<pcl::PointXYZINormal>::Ptr RTABMAP_EXP subtractFiltering(
+		const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
+		const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & substractCloud,
+		float radiusSearch,
+		float maxAngle = M_PI/4.0f,
+		int minNeighborsInRadius = 1);
 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP subtractFiltering(
 		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
 		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & substractCloud,
@@ -467,6 +488,14 @@ pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP subtractFiltering(
  * @param radiusSearch the radius in meter.
  * @return the indices of the points satisfying the parameters.
  */
+pcl::IndicesPtr RTABMAP_EXP subtractFiltering(
+		const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & substractCloud,
+		const pcl::IndicesPtr & substractIndices,
+		float radiusSearch,
+		float maxAngle = M_PI/4.0f,
+		int minNeighborsInRadius = 1);
 pcl::IndicesPtr RTABMAP_EXP subtractFiltering(
 		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
 		const pcl::IndicesPtr & indices,

corelib/include/rtabmap/core/util3d_registration.h

@@ -66,12 +66,25 @@ void RTABMAP_EXP computeVarianceAndCorrespondences(
 		double maxCorrespondenceAngle, // <=0 means that we don't care about normal angle difference
 		double & variance,
 		int & correspondencesOut);
+void RTABMAP_EXP computeVarianceAndCorrespondences(
+		const pcl::PointCloud<pcl::PointXYZINormal>::ConstPtr & cloudA,
+		const pcl::PointCloud<pcl::PointXYZINormal>::ConstPtr & cloudB,
+		double maxCorrespondenceDistance,
+		double maxCorrespondenceAngle, // <=0 means that we don't care about normal angle difference
+		double & variance,
+		int & correspondencesOut);
 void RTABMAP_EXP computeVarianceAndCorrespondences(
 		const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloudA,
 		const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloudB,
 		double maxCorrespondenceDistance,
 		double & variance,
 		int & correspondencesOut);
+void RTABMAP_EXP computeVarianceAndCorrespondences(
+		const pcl::PointCloud<pcl::PointXYZI>::ConstPtr & cloudA,
+		const pcl::PointCloud<pcl::PointXYZI>::ConstPtr & cloudB,
+		double maxCorrespondenceDistance,
+		double & variance,
+		int & correspondencesOut);
 
 Transform RTABMAP_EXP icp(
 		const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_source,
@@ -82,6 +95,15 @@ Transform RTABMAP_EXP icp(
 		pcl::PointCloud<pcl::PointXYZ> & cloud_source_registered,
 		float epsilon = 0.0f,
 		bool icp2D = false);
+Transform RTABMAP_EXP icp(
+		const pcl::PointCloud<pcl::PointXYZI>::ConstPtr & cloud_source,
+		const pcl::PointCloud<pcl::PointXYZI>::ConstPtr & cloud_target,
+		double maxCorrespondenceDistance,
+		int maximumIterations,
+		bool & hasConverged,
+		pcl::PointCloud<pcl::PointXYZI> & cloud_source_registered,
+		float epsilon = 0.0f,
+		bool icp2D = false);
 
 Transform RTABMAP_EXP icpPointToPlane(
 		const pcl::PointCloud<pcl::PointNormal>::ConstPtr & cloud_source,
@@ -92,6 +114,15 @@ Transform RTABMAP_EXP icpPointToPlane(
 		pcl::PointCloud<pcl::PointNormal> & cloud_source_registered,
 		float epsilon = 0.0f,
 		bool icp2D = false);
+Transform RTABMAP_EXP icpPointToPlane(
+		const pcl::PointCloud<pcl::PointXYZINormal>::ConstPtr & cloud_source,
+		const pcl::PointCloud<pcl::PointXYZINormal>::ConstPtr & cloud_target,
+		double maxCorrespondenceDistance,
+		int maximumIterations,
+		bool & hasConverged,
+		pcl::PointCloud<pcl::PointXYZINormal> & cloud_source_registered,
+		float epsilon = 0.0f,
+		bool icp2D = false);
 
 } // namespace util3d
 } // namespace rtabmap

corelib/include/rtabmap/core/util3d_surface.h

@@ -358,6 +358,12 @@ float RTABMAP_EXP computeNormalsComplexity(
 		bool is2d = false,
 		cv::Mat * pcaEigenVectors = 0,
 		cv::Mat * pcaEigenValues = 0);
+float RTABMAP_EXP computeNormalsComplexity(
+		const pcl::PointCloud<pcl::PointXYZINormal> & cloud,
+		const Transform & t = Transform::getIdentity(),
+		bool is2d = false,
+		cv::Mat * pcaEigenVectors = 0,
+		cv::Mat * pcaEigenValues = 0);
 float RTABMAP_EXP computeNormalsComplexity(
 		const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud,
 		const Transform & t = Transform::getIdentity(),
@@ -387,18 +393,39 @@ pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP mls(
 		float dilationVoxelSize = 1.0f,  // VOXEL_GRID_DILATION
 		int dilationIterations = 0);     // VOXEL_GRID_DILATION
 
-LaserScan RTABMAP_EXP adjustNormalsToViewPoint(
+RTABMAP_DEPRECATED(LaserScan RTABMAP_EXP adjustNormalsToViewPoint(
 		const LaserScan & scan,
 		const Eigen::Vector3f & viewpoint,
-		bool forceGroundNormalsUp);
+		bool forceGroundNormalsUp), "Use version with groundNormalsUp as float. For forceGroundNormalsUp=true, set groundNormalsUp to 0.8f, otherwise set groundNormalsUp to 0.0f.");
+LaserScan RTABMAP_EXP adjustNormalsToViewPoint(
+		const LaserScan & scan,
+		const Eigen::Vector3f & viewpoint = Eigen::Vector3f(0,0,0),
+		float groundNormalsUp = 0.0f);
+RTABMAP_DEPRECATED(void RTABMAP_EXP adjustNormalsToViewPoint(
+		pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
+		const Eigen::Vector3f & viewpoint,
+		bool forceGroundNormalsUp), "Use version with groundNormalsUp as float. For forceGroundNormalsUp=true, set groundNormalsUp to 0.8f, otherwise set groundNormalsUp to 0.0f.");
 void RTABMAP_EXP adjustNormalsToViewPoint(
 		pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
 		const Eigen::Vector3f & viewpoint = Eigen::Vector3f(0,0,0),
-		bool forceGroundNormalsUp = false);
+		float groundNormalsUp = 0.0f);
+RTABMAP_DEPRECATED(void RTABMAP_EXP adjustNormalsToViewPoint(
+		pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
+		const Eigen::Vector3f & viewpoint,
+		bool forceGroundNormalsUp), "Use version with groundNormalsUp as float. For forceGroundNormalsUp=true, set groundNormalsUp to 0.8f, otherwise set groundNormalsUp to 0.0f.");
 void RTABMAP_EXP adjustNormalsToViewPoint(
 		pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
 		const Eigen::Vector3f & viewpoint = Eigen::Vector3f(0,0,0),
-		bool forceGroundNormalsUp = false);
+		float groundNormalsUp = 0.0f);
+RTABMAP_DEPRECATED(void RTABMAP_EXP adjustNormalsToViewPoint(
+		pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
+		const Eigen::Vector3f & viewpoint,
+		bool forceGroundNormalsUp), "Use version with groundNormalsUp as float. For forceGroundNormalsUp=true, set groundNormalsUp to 0.8f, otherwise set groundNormalsUp to 0.0f.");
+void RTABMAP_EXP adjustNormalsToViewPoint(
+		pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
+		const Eigen::Vector3f & viewpoint = Eigen::Vector3f(0,0,0),
+		float groundNormalsUp = 0.0f);
+
 void RTABMAP_EXP adjustNormalsToViewPoints(
 		const std::map<int, Transform> & poses,
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & rawCloud,