0.15.3: util3d::computeVarianceAndCorrespondences(): added max angle parameters for normal type. util3d::downsample(): added Normal types support. OccupancyGrid: fixed 2d scan with 5 channels not handled correctly on update.

corelib/include/rtabmap/core/util3d_filtering.h

@@ -51,6 +51,12 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP downsample(
 pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP downsample(
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
 		int step);
+pcl::PointCloud<pcl::PointNormal>::Ptr RTABMAP_EXP downsample(
+		const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
+		int step);
+pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP downsample(
+		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
+		int step);
 
 pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP voxelize(
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,

corelib/include/rtabmap/core/util3d_registration.h

@@ -63,6 +63,7 @@ void RTABMAP_EXP computeVarianceAndCorrespondences(
 		const pcl::PointCloud<pcl::PointNormal>::ConstPtr & cloudA,
 		const pcl::PointCloud<pcl::PointNormal>::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(

corelib/src/OccupancyGrid.cpp

@@ -641,7 +641,7 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn)
 						const float * vi = pair.first.ptr<float>(0,i);
 						float * vo = ground.ptr<float>(0,i);
 						cv::Point3f vt;
-						if(pair.first.channels() > 2)
+						if(pair.first.channels() != 2 && pair.first.channels() != 5)
 						{
 							vt = util3d::transformPoint(cv::Point3f(vi[0], vi[1], vi[2]), iter->second);
 						}
@@ -677,7 +677,7 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn)
 						const float * vi = pair.second.ptr<float>(0,i);
 						float * vo = obstacles.ptr<float>(0,i);
 						cv::Point3f vt;
-						if(pair.second.channels() > 2)
+						if(pair.first.channels() != 2 && pair.first.channels() != 5)
 						{
 							vt = util3d::transformPoint(cv::Point3f(vi[0], vi[1], vi[2]), iter->second);
 						}
@@ -801,7 +801,7 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn)
 					{
 						float * ptf = iter->second.ptr<float>(0,i);
 						cv::Point2i pt((ptf[0]-xMin)/cellSize_, (ptf[1]-yMin)/cellSize_);
-						UASSERT_MSG(pt.y < map.rows && pt.x < map.cols,
+						UASSERT_MSG(pt.y >=0 && pt.y < map.rows && pt.x >= 0 && pt.x < map.cols,
 								uFormat("%d: pt=(%d,%d) map=%dx%d rawPt=(%f,%f) xMin=%f yMin=%f channels=%dvs%d",
 										kter->first, pt.x, pt.y, map.cols, map.rows, ptf[0], ptf[1], xMin, yMin, iter->second.channels(), mapInfo.channels()-1).c_str());
 						char & value = map.at<char>(pt.y, pt.x);
@@ -911,7 +911,7 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn)
 					{
 						float * ptf = jter->second.ptr<float>(0,i);
 						cv::Point2i pt((ptf[0]-xMin)/cellSize_, (ptf[1]-yMin)/cellSize_);
-						UASSERT_MSG(pt.y < map.rows && pt.x < map.cols,
+						UASSERT_MSG(pt.y>=0 && pt.y < map.rows && pt.x>=0 && pt.x < map.cols,
 									uFormat("%d: pt=(%d,%d) map=%dx%d rawPt=(%f,%f) xMin=%f yMin=%f channels=%dvs%d",
 											kter->first, pt.x, pt.y, map.cols, map.rows, ptf[0], ptf[1], xMin, yMin, jter->second.channels(), mapInfo.channels()-1).c_str());
 						char & value = map.at<char>(pt.y, pt.x);

corelib/src/RegistrationIcp.cpp

@@ -523,6 +523,7 @@ Transform RegistrationIcp::computeTransformationImpl(
 								fromCloudNormalsRegistered,
 								toCloudNormals,
 								_maxCorrespondenceDistance,
+								_maxRotation,
 								variance,
 								correspondences);
 					}
@@ -717,6 +718,7 @@ Transform RegistrationIcp::computeTransformationImpl(
 										fromCloudNormalsRegistered,
 										toCloudNormals,
 										_maxCorrespondenceDistance,
+										_maxRotation,
 										variance,
 										correspondences);
 							}
@@ -855,14 +857,28 @@ Transform RegistrationIcp::computeTransformationImpl(
 							t.getEulerAngles(roll, pitch, yaw);
 							t = Transform(v[0], v[1], v[2], roll, pitch, yaw);
 							icpT = guess * t.inverse() * guessInv;
-						}
 
-						util3d::computeVarianceAndCorrespondences(
-								fromCloudRegistered,
-								toCloudFiltered,
-								_maxCorrespondenceDistance,
-								variance,
-								correspondences);
+							// we were using normals, so compute correspondences using normals
+							pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormalsRegistered = util3d::laserScanToPointCloudNormal(fromScan, icpT * fromLocalTransform);
+							pcl::PointCloud<pcl::PointNormal>::Ptr toCloudNormals = util3d::laserScanToPointCloudNormal(toScan, guess * toLocalTransform);
+
+							util3d::computeVarianceAndCorrespondences(
+									fromCloudNormalsRegistered,
+									toCloudNormals,
+									_maxCorrespondenceDistance,
+									_maxRotation,
+									variance,
+									correspondences);
+						}
+						else
+						{
+							util3d::computeVarianceAndCorrespondences(
+									fromCloudRegistered,
+									toCloudFiltered,
+									_maxCorrespondenceDistance,
+									variance,
+									correspondences);
+						}
 					}
 				}
 			}
@@ -926,8 +942,8 @@ Transform RegistrationIcp::computeTransformationImpl(
 
 					if(correspondencesRatio < _correspondenceRatio)
 					{
-						msg = uFormat("Cannot compute transform (cor=%d corrRatio=%f/%f)",
-								correspondences, correspondencesRatio, _correspondenceRatio);
+						msg = uFormat("Cannot compute transform (cor=%d corrRatio=%f/%f maxLaserScans=%d)",
+								correspondences, correspondencesRatio, _correspondenceRatio, maxLaserScans);
 						UINFO(msg.c_str());
 					}
 					else

corelib/src/util3d_filtering.cpp

@@ -135,6 +135,53 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr downsample(
 	}
 	return output;
 }
+pcl::PointCloud<pcl::PointNormal>::Ptr downsample(
+		const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
+		int step)
+{
+	UASSERT(step > 0);
+	pcl::PointCloud<pcl::PointNormal>::Ptr output(new pcl::PointCloud<pcl::PointNormal>);
+	if(step <= 1 || (int)cloud->size() <= step)
+	{
+		// no sampling
+		*output = *cloud;
+	}
+	else
+	{
+		int finalSize = int(cloud->size())/step;
+		output->resize(finalSize);
+		int oi = 0;
+		for(unsigned int i=0; i<cloud->size()-step+1; i+=step)
+		{
+			(*output)[oi++] = cloud->at(i);
+		}
+	}
+	return output;
+}
+pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr downsample(
+		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
+		int step)
+{
+	UASSERT(step > 0);
+	pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
+	if(step <= 1 || (int)cloud->size()<=step)
+	{
+		// no sampling
+		*output = *cloud;
+	}
+	else
+	{
+		int finalSize = int(cloud->size())/step;
+		output->resize(finalSize);
+		int oi = 0;
+		for(int i=0; i<(int)cloud->size()-step+1; i+=step)
+		{
+			(*output)[oi++] = cloud->at(i);
+		}
+	}
+	return output;
+}
+
 
 pcl::PointCloud<pcl::PointXYZ>::Ptr voxelize(
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,

corelib/src/util3d_registration.cpp

@@ -36,6 +36,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <pcl/registration/transformation_estimation_svd.h>
 #include <pcl/sample_consensus/sac_model_registration.h>
 #include <pcl/sample_consensus/ransac.h>
+#include <pcl/common/common.h>
 #include <rtabmap/utilite/ULogger.h>
 
 namespace rtabmap
@@ -237,6 +238,7 @@ void computeVarianceAndCorrespondences(
 		const pcl::PointCloud<pcl::PointNormal>::ConstPtr & cloudA,
 		const pcl::PointCloud<pcl::PointNormal>::ConstPtr & cloudB,
 		double maxCorrespondenceDistance,
+		double maxCorrespondenceAngle,
 		double & variance,
 		int & correspondencesOut)
 {
@@ -244,26 +246,53 @@ void computeVarianceAndCorrespondences(
 	correspondencesOut = 0;
 	pcl::registration::CorrespondenceEstimation<pcl::PointNormal, pcl::PointNormal>::Ptr est;
 	est.reset(new pcl::registration::CorrespondenceEstimation<pcl::PointNormal, pcl::PointNormal>);
-	est->setInputTarget(cloudA->size()>cloudB->size()?cloudA:cloudB);
-	est->setInputSource(cloudA->size()>cloudB->size()?cloudB:cloudA);
+	const pcl::PointCloud<pcl::PointNormal>::ConstPtr & target = cloudA->size()>cloudB->size()?cloudA:cloudB;
+	const pcl::PointCloud<pcl::PointNormal>::ConstPtr & source = cloudA->size()>cloudB->size()?cloudB:cloudA;
+	est->setInputTarget(target);
+	est->setInputSource(source);
 	pcl::Correspondences correspondences;
 	est->determineCorrespondences(correspondences, maxCorrespondenceDistance);
 
-	if(correspondences.size()>=3)
+	if(correspondences.size())
 	{
 		std::vector<double> distances(correspondences.size());
+		correspondencesOut = 0;
 		for(unsigned int i=0; i<correspondences.size(); ++i)
 		{
 			distances[i] = correspondences[i].distance;
+			if(maxCorrespondenceAngle <= 0.0)
+			{
+				++correspondencesOut;
+			}
+			else
+			{
+				Eigen::Vector4f v1(
+						target->at(correspondences[i].index_match).normal_x,
+						target->at(correspondences[i].index_match).normal_y,
+						target->at(correspondences[i].index_match).normal_z,
+						0);
+				Eigen::Vector4f v2(
+						source->at(correspondences[i].index_query).normal_x,
+						source->at(correspondences[i].index_query).normal_y,
+						source->at(correspondences[i].index_query).normal_z,
+						0);
+				float angle = pcl::getAngle3D(v1, v2);
+				if(angle < maxCorrespondenceAngle)
+				{
+					++correspondencesOut;
+				}
+			}
 		}
+		if(correspondencesOut)
+		{
+			distances.resize(correspondencesOut);
 
-		//variance
-		std::sort(distances.begin (), distances.end ());
-		double median_error_sqr = distances[distances.size () >> 1];
-		variance = (2.1981 * median_error_sqr);
+			//variance
+			std::sort(distances.begin (), distances.end ());
+			double median_error_sqr = distances[distances.size () >> 1];
+			variance = (2.1981 * median_error_sqr);
+		}
 	}
-
-	correspondencesOut = (int)correspondences.size();
 }
 
 void computeVarianceAndCorrespondences(