OdomF2M: added support to laser scan

corelib/include/rtabmap/core/OdometryF2F.h

@@ -51,6 +51,7 @@ private:
 private:
 	//Parameters:
 	float keyFrameThr_;
+	float scanKeyFrameThr_;
 
 	Registration * registrationPipeline_;
 	Signature refFrame_;

corelib/include/rtabmap/core/OdometryF2M.h

@@ -29,11 +29,13 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #define ODOMETRYF2M_H_
 
 #include <rtabmap/core/Odometry.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
 
 namespace rtabmap {
 
 class Signature;
-class RegistrationVis;
+class Registration;
 
 class RTABMAP_EXP OdometryF2M : public Odometry
 {
@@ -53,11 +55,15 @@ private:
 	int maximumMapSize_;
 	float keyFrameThr_;
 	int maxNewFeatures_;
+	float scanKeyFrameThr_;
+	int scanMaximumMapSize_;
+	float scanSubstractRadius_;
 	std::string fixedMapPath_;
 
-	RegistrationVis * regVis_;
+	Registration * regPipeline_;
 	Signature * map_;
 	Signature * lastFrame_;
+	std::map<int, pcl::PointCloud<pcl::PointNormal>::Ptr > scansBuffer_;
 };
 
 }

corelib/include/rtabmap/core/OdometryInfo.h

@@ -45,6 +45,7 @@ public:
 		variance(0.0f),
 		features(0),
 		localMapSize(0),
+		localScanMapSize(0),
 		timeEstimation(0.0f),
 		timeParticleFiltering(0.0f),
 		stamp(0),
@@ -63,6 +64,7 @@ public:
 		output.variance = variance;
 		output.features = features;
 		output.localMapSize = localMapSize;
+		output.localScanMapSize = localScanMapSize;
 		output.timeEstimation = timeEstimation;
 		output.timeParticleFiltering = timeParticleFiltering;
 		output.stamp = stamp;
@@ -80,6 +82,7 @@ public:
 	float variance;
 	int features;
 	int localMapSize;
+	int localScanMapSize;
 	float timeEstimation;
 	float timeParticleFiltering;
 	double stamp;
@@ -89,15 +92,16 @@ public:
 	Transform transformGroundTruth;
 	float distanceTravelled;
 
-	int type; // 0=BOW, 1=F2F, 2=ICP, 3=Mono
+	int type; // 0=F2M, 1=F2F
 
-	// BOW
+	// F2M
 	std::multimap<int, cv::KeyPoint> words;
 	std::vector<int> wordMatches;
 	std::vector<int> wordInliers;
 	std::map<int, cv::Point3f> localMap;
+	cv::Mat localScanMap;
 
-	// F2F && Mono
+	// F2F
 	std::vector<cv::Point2f> refCorners;
 	std::vector<cv::Point2f> newCorners;
 	std::vector<int> cornerInliers;

corelib/include/rtabmap/core/Parameters.h

@@ -360,11 +360,14 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(Odom, KalmanProcessNoise, 	float, 0.001,     "Process noise covariance value.");
 	RTABMAP_PARAM(Odom, KalmanMeasurementNoise, float, 0.01,      "Process measurement covariance value.");
 	RTABMAP_PARAM(Odom, GuessMotion,            bool, true,       "Guess next transformation from the last motion computed.");
-	RTABMAP_PARAM(Odom, KeyFrameThr,            float, 0.5,       "Create a new keyframe when the number of inliers drops under this ratio of features in last frame. Setting the value to 0 means that a keyframe is created for each processed frame.");
+	RTABMAP_PARAM(Odom, KeyFrameThr,            float, 0.3,       "[Visual] Create a new keyframe when the number of inliers drops under this ratio of features in last frame. Setting the value to 0 means that a keyframe is created for each processed frame.");
+	RTABMAP_PARAM(Odom, ScanKeyFrameThr,        float, 0.7,       "[Geometry] Create a new keyframe when the number of ICP inliers drops under this ratio of points in last frame's scan. Setting the value to 0 means that a keyframe is created for each processed frame.");
 
 	// Odometry Bag-of-words
-	RTABMAP_PARAM(OdomF2M, MaxSize,             int, 1000,   "Local map size: If > 0 (example 5000), the odometry will maintain a local map of X maximum words.");
+	RTABMAP_PARAM(OdomF2M, MaxSize,             int, 2000,    "[Visual] Local map size: If > 0 (example 5000), the odometry will maintain a local map of X maximum words.");
-	RTABMAP_PARAM(OdomF2M, MaxNewFeatures,      int, 0,      "Maximum features (sorted by keypoint response) added to local map from a new key-frame. 0 means no limit.");
+	RTABMAP_PARAM(OdomF2M, MaxNewFeatures,      int, 0,       "[Visual] Maximum features (sorted by keypoint response) added to local map from a new key-frame. 0 means no limit.");
+	RTABMAP_PARAM(OdomF2M, ScanMaxSize,         int, 2000,    "[Geometry] Maximum local scan map size.");
+	RTABMAP_PARAM(OdomF2M, ScanSubstractRadius, float, 0.05,  "[Geometry] Radius used to filter points of a new added scan to local map. This could match the voxel size of the scans.");
 	RTABMAP_PARAM_STR(OdomF2M, FixedMapPath,    "",           "Path to a fixed map (RTAB-Map's database) to be used for odometry. Odometry will be constraint to this map. RGB-only images can be used if odometry PnP estimation is used.")
 
 	// Odometry Mono

corelib/include/rtabmap/core/util3d_filtering.h

@@ -233,6 +233,34 @@ pcl::IndicesPtr RTABMAP_EXP subtractFiltering(
 		float radiusSearch,
 		int minNeighborsInRadius = 1);
 
+/**
+ * For convenience.
+ */
+pcl::PointCloud<pcl::PointNormal>::Ptr RTABMAP_EXP subtractFiltering(
+		const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
+		const pcl::PointCloud<pcl::PointNormal>::Ptr & substractCloud,
+		float radiusSearch,
+		float maxAngle = M_PI/4.0f,
+		int minNeighborsInRadius = 1);
+
+/**
+ * Subtract a cloud from another one using radius filtering.
+ * @param cloud the input cloud.
+ * @param indices the input indices of the cloud to check, if empty, all points in the cloud are checked.
+ * @param cloud the input cloud to subtract.
+ * @param indices the input indices of the subtracted cloud to check, if empty, all points in the cloud are checked.
+ * @param radiusSearch the radius in meter.
+ * @return the indices of the points satisfying the parameters.
+ */
+pcl::IndicesPtr RTABMAP_EXP subtractFiltering(
+		const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		const pcl::PointCloud<pcl::PointNormal>::Ptr & substractCloud,
+		const pcl::IndicesPtr & substractIndices,
+		float radiusSearch,
+		float maxAngle = M_PI/4.0f,
+		int minNeighborsInRadius = 1);
+
 /**
  * For convenience.
  */
@@ -240,7 +268,7 @@ pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP subtractFiltering(
 		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
 		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & substractCloud,
 		float radiusSearch,
-		float maxAngle,
+		float maxAngle = M_PI/4.0f,
 		int minNeighborsInRadius = 1);
 
 /**

corelib/include/rtabmap/core/util3d_registration.h

@@ -79,7 +79,7 @@ Transform RTABMAP_EXP icp(
 		int maximumIterations,
 		bool & hasConverged,
 		pcl::PointCloud<pcl::PointXYZ> & cloud_source_registered,
-		double epsilon = 0,
+		float epsilon = 0.0f,
 		bool icp2D = false);
 
 Transform RTABMAP_EXP icpPointToPlane(
@@ -88,7 +88,9 @@ Transform RTABMAP_EXP icpPointToPlane(
 		double maxCorrespondenceDistance,
 		int maximumIterations,
 		bool & hasConverged,
-		pcl::PointCloud<pcl::PointNormal> & cloud_source_registered);
+		pcl::PointCloud<pcl::PointNormal> & cloud_source_registered,
+		float epsilon = 0.0f,
+		bool icp2D = false);
 
 pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP getICPReadyCloud(
 		const cv::Mat & depth,

corelib/include/rtabmap/core/util3d_transforms.h

@@ -62,6 +62,9 @@ pcl::PointXYZ RTABMAP_EXP transformPoint(
 pcl::PointXYZRGB RTABMAP_EXP transformPoint(
 		const pcl::PointXYZRGB & pt,
 		const Transform & transform);
+pcl::PointNormal RTABMAP_EXP transformPoint(
+		const pcl::PointNormal & point,
+		const Transform & transform);
 
 } // namespace util3d
 } // namespace rtabmap

corelib/src/CameraThread.cpp

@@ -32,11 +32,14 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/util2d.h"
 #include "rtabmap/core/util3d.h"
 #include "rtabmap/core/util3d_surface.h"
+#include "rtabmap/core/util3d_filtering.h"
 #include "rtabmap/core/StereoDense.h"
 
 #include <rtabmap/utilite/UTimer.h>
 #include <rtabmap/utilite/ULogger.h>
 
+#include <pcl/io/io.h>
+
 namespace rtabmap
 {
 
@@ -170,7 +173,22 @@ void CameraThread::mainLoop()
 			{
 				UASSERT(_scanDecimation >= 1);
 				UTimer timer;
-				pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::cloudFromSensorData(data, _scanDecimation, _scanMaxDepth, _scanVoxelSize);
+				pcl::IndicesPtr validIndices(new std::vector<int>);
+				pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::cloudFromSensorData(data, _scanDecimation, _scanMaxDepth, 0.0f, 0, validIndices.get());
+				float maxPoints = (data.depthRaw().rows/_scanDecimation)*(data.depthRaw().cols/_scanDecimation);
+				if(_scanVoxelSize>0.0f)
+				{
+					cloud = util3d::voxelize(cloud, validIndices, _scanVoxelSize);
+					float ratio = float(cloud->size()) / float(validIndices->size());
+					maxPoints = ratio * maxPoints;
+				}
+				else if(!cloud->is_dense)
+				{
+					pcl::PointCloud<pcl::PointXYZ>::Ptr denseCloud(new pcl::PointCloud<pcl::PointXYZ>);
+					pcl::copyPointCloud(*cloud, *validIndices, *denseCloud);
+					cloud = denseCloud;
+				}
+
 				cv::Mat scan;
 				if(_scanNormalsK>0)
 				{
@@ -180,7 +198,7 @@ void CameraThread::mainLoop()
 				{
 					scan = util3d::laserScanFromPointCloud(*cloud);
 				}
-				data.setLaserScanRaw(scan, (data.depthRaw().rows/_scanDecimation)*(data.depthRaw().cols/_scanDecimation), _scanMaxDepth);
+				data.setLaserScanRaw(scan, (int)maxPoints, _scanMaxDepth);
 				info.timeScanFromDepth = timer.ticks();
 				UINFO("Computing scan from depth = %f s", info.timeScanFromDepth);
 			}

corelib/src/Memory.cpp

@@ -3107,7 +3107,7 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
 	}
 
 	std::vector<cv::Point3f> keypoints3D;
-	if(!_useOdometryFeatures || (data.keypoints().size() != data.descriptors().rows))
+	if(!_useOdometryFeatures || data.keypoints().empty() || data.keypoints().size() != data.descriptors().rows)
 	{
 		if(_feature2D->getMaxFeatures() >= 0 && !data.imageRaw().empty() && !isIntermediateNode)
 		{

corelib/src/Odometry.cpp

@@ -196,7 +196,7 @@ Transform Odometry::process(SensorData & data, OdometryInfo * info)
 	}
 
 	double dt = previousStamp_>0.0f?data.stamp() - previousStamp_:0.0;
-	Transform guess;
+	Transform guess = dt?Transform::getIdentity():Transform();
 	UASSERT(dt>0.0 || (dt == 0.0 && previousVelocityTransform_.isNull()));
 	if(!previousVelocityTransform_.isNull())
 	{

corelib/src/OdometryF2F.cpp

@@ -39,11 +39,14 @@ namespace rtabmap {
 OdometryF2F::OdometryF2F(const ParametersMap & parameters) :
 	Odometry(parameters),
 	keyFrameThr_(Parameters::defaultOdomKeyFrameThr()),
+	scanKeyFrameThr_(Parameters::defaultOdomScanKeyFrameThr()),
 	motionSinceLastKeyFrame_(Transform::getIdentity())
 {
 	registrationPipeline_ = Registration::create(parameters);
 	Parameters::parse(parameters, Parameters::kOdomKeyFrameThr(), keyFrameThr_);
+	Parameters::parse(parameters, Parameters::kOdomScanKeyFrameThr(), scanKeyFrameThr_);
 	UASSERT(keyFrameThr_>=0.0f && keyFrameThr_<=1.0f);
+	UASSERT(scanKeyFrameThr_>=0.0f && scanKeyFrameThr_<=1.0f);
 }
 
 OdometryF2F::~OdometryF2F()
@@ -136,7 +139,8 @@ Transform OdometryF2F::computeTransform(
 		motionSinceLastKeyFrame_ *= output;
 
 		// new key-frame?
-		if(keyFrameThr_==0 || float(regInfo.inliers) <= keyFrameThr_*float(refFrame_.sensorData().keypoints().size()))
+		if( (registrationPipeline_->isImageRequired() && (keyFrameThr_ == 0 || float(regInfo.inliers) <= keyFrameThr_*float(refFrame_.sensorData().keypoints().size()))) ||
+			(registrationPipeline_->isScanRequired() && (scanKeyFrameThr_ == 0 || regInfo.icpInliersRatio <= scanKeyFrameThr_)))
 		{
 			UDEBUG("Update key frame");
 			int features = newFrame.getWordsDescriptors().size();

corelib/src/OdometryF2M.cpp

@@ -34,6 +34,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/util3d_registration.h"
 #include "rtabmap/core/util3d_correspondences.h"
 #include "rtabmap/core/util3d_motion_estimation.h"
+#include "rtabmap/core/util3d_filtering.h"
 #include "rtabmap/core/Optimizer.h"
 #include "rtabmap/core/VWDictionary.h"
 #include "rtabmap/core/util3d.h"
@@ -57,8 +58,11 @@ OdometryF2M::OdometryF2M(const ParametersMap & parameters) :
 	maximumMapSize_(Parameters::defaultOdomF2MMaxSize()),
 	keyFrameThr_(Parameters::defaultOdomKeyFrameThr()),
 	maxNewFeatures_(Parameters::defaultOdomF2MMaxNewFeatures()),
+	scanKeyFrameThr_(Parameters::defaultOdomScanKeyFrameThr()),
+	scanMaximumMapSize_(Parameters::defaultOdomF2MScanMaxSize()),
+	scanSubstractRadius_(Parameters::defaultOdomF2MScanSubstractRadius()),
 	fixedMapPath_(Parameters::defaultOdomF2MFixedMapPath()),
-	regVis_(new RegistrationVis(parameters)),
+	regPipeline_(Registration::create(parameters)),
 	map_(new Signature(-1)),
 	lastFrame_(new Signature(1))
 {
@@ -66,9 +70,13 @@ OdometryF2M::OdometryF2M(const ParametersMap & parameters) :
 	Parameters::parse(parameters, Parameters::kOdomF2MMaxSize(), maximumMapSize_);
 	Parameters::parse(parameters, Parameters::kOdomKeyFrameThr(), keyFrameThr_);
 	Parameters::parse(parameters, Parameters::kOdomF2MMaxNewFeatures(), maxNewFeatures_);
+	Parameters::parse(parameters, Parameters::kOdomScanKeyFrameThr(), scanKeyFrameThr_);
+	Parameters::parse(parameters, Parameters::kOdomF2MScanMaxSize(), scanMaximumMapSize_);
+	Parameters::parse(parameters, Parameters::kOdomF2MScanSubstractRadius(), scanSubstractRadius_);
 	Parameters::parse(parameters, Parameters::kOdomF2MFixedMapPath(), fixedMapPath_);
 	UASSERT(maximumMapSize_ >= 0);
 	UASSERT(keyFrameThr_ >= 0.0f && keyFrameThr_<=1.0f);
+	UASSERT(scanKeyFrameThr_ >= 0.0f && scanKeyFrameThr_<=1.0f);
 	UASSERT(maxNewFeatures_ >= 0);
 
 	if(!fixedMapPath_.empty())
@@ -142,8 +150,9 @@ OdometryF2M::OdometryF2M(const ParametersMap & parameters) :
 				UERROR("No pose loaded from database \"%s\"", fixedMapPath_.c_str());
 			}
 		}
-		if((int)map_->getWords3().size() < regVis_->getMinInliers() || map_->getWords3().size() == 0)
+		if((int)map_->getWords3().size() < regPipeline_->getMinVisualCorrespondences() || map_->getWords3().size() == 0)
 		{
+			// TODO: support geometric-only maps?
 			UERROR("The loaded fixed map from \"%s\" is too small! Only %d unique features loaded. Odometry won't be computed!",
 					fixedMapPath_.c_str(), (int)map_->getWords3().size());
 		}
@@ -195,10 +204,11 @@ Transform OdometryF2M::computeTransform(
 	// Generate keypoints from the new data
 	if(lastFrame_->sensorData().isValid())
 	{
-		if(map_->getWords3().size() && lastFrame_->sensorData().isValid())
+		if((map_->getWords3().size() || !map_->sensorData().laserScanRaw().empty()) &&
+			lastFrame_->sensorData().isValid())
 		{
 			Signature tmpMap = *map_;
-			Transform transform = regVis_->computeTransformationMod(
+			Transform transform = regPipeline_->computeTransformationMod(
 					tmpMap,
 					*lastFrame_,
 					guess.isNull()?Transform():this->getPose()*guess,
@@ -221,20 +231,29 @@ Transform OdometryF2M::computeTransform(
 			}
 
 			if(!transform.isNull())
-			{
-				if(fixedMapPath_.empty() &&
-				   (keyFrameThr_==0 || float(regInfo.inliers) <= keyFrameThr_*float(lastFrame_->sensorData().keypoints().size())))
 			{
 				output = transform;
 
+				if(fixedMapPath_.empty())
+				{
+					bool modified = false;
+					Transform newFramePose = this->getPose()*output;
+
+					// fields to update
+					cv::Mat mapScan = tmpMap.sensorData().laserScanRaw();
+					std::multimap<int, cv::Point3f> mapPoints = tmpMap.getWords3();
+					std::multimap<int, cv::Mat> mapDescriptors = tmpMap.getWordsDescriptors();
+
+					//Visual
 					int added = 0;
 					int removed = 0;
+					UDEBUG("keyframeThr=%f inliers=%d features=%d", keyFrameThr_, regInfo.inliers, (int)lastFrame_->sensorData().keypoints().size());
+					if(regPipeline_->isImageRequired() &&
+						(keyFrameThr_==0 || float(regInfo.inliers) <= keyFrameThr_*float(lastFrame_->sensorData().keypoints().size())))
+					{
+						UDEBUG("Update local map");
 
 						// update local map
-					*map_ = tmpMap;
-					std::multimap<int, cv::Point3f> mapPoints = map_->getWords3();
-					std::multimap<int, cv::Mat> mapDescriptors = map_->getWordsDescriptors();
-					Transform t = this->getPose()*output;
 						UASSERT(mapPoints.size() == mapDescriptors.size());
 						UASSERT_MSG(lastFrame_->getWordsDescriptors().size() == lastFrame_->getWords3().size(), uFormat("%d vs %d", lastFrame_->getWordsDescriptors().size(), lastFrame_->getWords3().size()).c_str());
 
@@ -259,7 +278,7 @@ Transform OdometryF2M::computeTransform(
 							{
 								if(mapPoints.find(iter->second.first) == mapPoints.end() && util3d::isFinite(iter->second.second))
 								{
-								mapPoints.insert(std::make_pair(iter->second.first, util3d::transformPoint(iter->second.second, t)));
+									mapPoints.insert(std::make_pair(iter->second.first, util3d::transformPoint(iter->second.second, newFramePose)));
 									mapDescriptors.insert(std::make_pair(iter->second.first, lastFrame_->getWordsDescriptors().find(iter->second.first)->second));
 									++added;
 								}
@@ -288,43 +307,118 @@ Transform OdometryF2M::computeTransform(
 								}
 							}
 						}
+						modified = true;
+					}
 
+					// Geometric
+					UDEBUG("scankeyframeThr=%f icpInliersRatio=%f", scanKeyFrameThr_, regInfo.icpInliersRatio);
+					if(regPipeline_->isScanRequired() &&
+						(scanKeyFrameThr_==0 || regInfo.icpInliersRatio <= scanKeyFrameThr_))
+					{
+						UINFO("Update local scan map %d", lastFrame_->id());
+						pcl::PointCloud<pcl::PointNormal>::Ptr mapCloudNormals = util3d::laserScanToPointCloudNormal(mapScan);
+						pcl::PointCloud<pcl::PointNormal>::Ptr frameCloudNormals = util3d::laserScanToPointCloudNormal(lastFrame_->sensorData().laserScanRaw(), newFramePose);
+
+						if(mapCloudNormals->size() && scanSubstractRadius_ > 0.0f)
+						{
+							frameCloudNormals = util3d::subtractFiltering(frameCloudNormals, mapCloudNormals, scanSubstractRadius_, 0.0f);
+						}
+						if(frameCloudNormals->size())
+						{
+							scansBuffer_.insert(std::make_pair(lastFrame_->id(), frameCloudNormals));
+
+							//remove points if too big
+							UDEBUG("scansBuffer=%d, mapSize=%d maxPoints=%d", (int)scansBuffer_.size(), int(mapCloudNormals->size() + frameCloudNormals->size()), scanMaximumMapSize_);
+							if(scansBuffer_.size() > 1 && int(mapCloudNormals->size() + frameCloudNormals->size()) > scanMaximumMapSize_)
+							{
+								//asssemble
+								mapCloudNormals->clear();
+								std::list<int> toRemove;
+								for(std::map<int, pcl::PointCloud<pcl::PointNormal>::Ptr>::reverse_iterator iter=scansBuffer_.rbegin();
+									iter!=scansBuffer_.rend();
+									++iter)
+								{
+									if(mapCloudNormals->empty())
+									{
+										*mapCloudNormals = *iter->second;
+									}
+									else if((int)mapCloudNormals->size() < scanMaximumMapSize_)
+									{
+										*mapCloudNormals += *iter->second;
+									}
+									else
+									{
+										toRemove.push_back(iter->first);
+									}
+								}
+								for(std::list<int>::iterator iter=toRemove.begin(); iter!=toRemove.end(); ++iter)
+								{
+									scansBuffer_.erase(*iter);
+								}
+							}
+							else
+							{
+								//assemble
+								*mapCloudNormals += *frameCloudNormals;
+							}
+
+							mapScan = util3d::laserScanFromPointCloud(*mapCloudNormals);
+							modified=true;
+						}
+					}
+
+					if(modified)
+					{
+						*map_ = tmpMap;
+
+						map_->sensorData().setLaserScanRaw(mapScan, 0, 0);
 						map_->setWords3(mapPoints);
 						map_->setWordsDescriptors(mapDescriptors);
 
 						UINFO("Updated map: %d added %d removed (new map size=%d)", added, removed, (int)mapPoints.size());
 					}
+				}
 				else
 				{
 					// fixed local map, don't update with the new signature
-					output = transform;
 				}
 			}
 
-			if(this->isInfoDataFilled())
+			if(info)
 			{
 				// use tmpMap instead of map_ to make sure that correspondences with the new frame matches
 				info->localMapSize = (int)tmpMap.getWords3().size();
+				info->localScanMapSize = tmpMap.sensorData().laserScanRaw().cols;
+				if(this->isInfoDataFilled())
+				{
 					info->localMap = uMultimapToMap(tmpMap.getWords3());
+					info->localScanMap = tmpMap.sensorData().laserScanRaw();
+				}
 			}
 		}
 		else
 		{
 			// just generate keypoints for the new signature
+			if(regPipeline_->isImageRequired())
+			{
 				Signature dummy;
-			regVis_->computeTransformationMod(
+				regPipeline_->computeTransformationMod(
 						*lastFrame_,
 						dummy);
+			}
 
 			data.setFeatures(lastFrame_->sensorData().keypoints(), lastFrame_->sensorData().descriptors());
 
-			if(fixedMapPath_.empty() && (int)lastFrame_->getWords3().size() >= regVis_->getMinInliers())
+			if(fixedMapPath_.empty())
 			{
 				output.setIdentity();
 				// a very high variance tells that the new pose is not linked with the previous one
 				regInfo.variance = 9999;
 
-				Transform t = this->getPose(); // initial pose may be not identity...
+				Transform newFramePose = this->getPose(); // initial pose may be not identity...
+				if(regPipeline_->isImageRequired() &&
+				   (int)lastFrame_->getWords3().size() >= regPipeline_->getMinVisualCorrespondences())
+				{
 					std::multimap<int, cv::Point3f> transformedPoints;
 					std::multimap<int, cv::Mat> descriptors;
 					UASSERT(lastFrame_->getWords3().size() == lastFrame_->getWordsDescriptors().size());
@@ -335,21 +429,33 @@ Transform OdometryF2M::computeTransform(
 					{
 						if(util3d::isFinite(iter->second))
 						{
-						transformedPoints.insert(std::make_pair(iter->first, util3d::transformPoint(iter->second, t)));
+							transformedPoints.insert(std::make_pair(iter->first, util3d::transformPoint(iter->second, newFramePose)));
 							descriptors.insert(std::make_pair(iter->first, descIter->second));
 						}
 					}
-
 					map_->setWords3(transformedPoints);
 					map_->setWordsDescriptors(descriptors);
 					map_->sensorData().setCameraModels(lastFrame_->sensorData().cameraModels());
 					map_->sensorData().setStereoCameraModel(lastFrame_->sensorData().stereoCameraModel());
 				}
+				if(regPipeline_->isScanRequired())
+				{
+					pcl::PointCloud<pcl::PointNormal>::Ptr mapCloudNormals = util3d::laserScanToPointCloudNormal(lastFrame_->sensorData().laserScanRaw(), newFramePose);
+					scansBuffer_.insert(std::make_pair(lastFrame_->id(), mapCloudNormals));
+					map_->sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*mapCloudNormals), 0,0);
+				}
+			}
+
+			if(info)
+			{
+				info->localMapSize = (int)map_->getWords3().size();
+				info->localScanMapSize = map_->sensorData().laserScanRaw().cols;
 
 				if(this->isInfoDataFilled())
 				{
-				info->localMapSize = (int)map_->getWords3().size();
 					info->localMap = uMultimapToMap(map_->getWords3());
+					info->localScanMap = map_->sensorData().laserScanRaw();
+				}
 			}
 		}
 
@@ -357,16 +463,20 @@ Transform OdometryF2M::computeTransform(
 
 		nFeatures = lastFrame_->getWords().size();
 		if(this->isInfoDataFilled() && info)
+		{
+			if(regPipeline_->isImageRequired())
 			{
 				info->words = lastFrame_->getWords();
 			}
 		}
+	}
 
 	if(info)
 	{
 		info->variance = regInfo.variance;
 		info->inliers = regInfo.inliers;
 		info->matches = regInfo.matches;
+		info->icpInliersRatio = regInfo.icpInliersRatio;
 		info->features = nFeatures;
 
 		if(this->isInfoDataFilled())
@@ -376,14 +486,15 @@ Transform OdometryF2M::computeTransform(
 		}
 	}
 
-	UINFO("Odom update time = %fs lost=%s features=%d inliers=%d/%d variance=%f local_map=%d",
+	UINFO("Odom update time = %fs lost=%s features=%d inliers=%d/%d variance=%f local_map=%d local_scan_map=%d",
 			timer.elapsed(),
 			output.isNull()?"true":"false",
 			nFeatures,
 			regInfo.inliers,
 			regInfo.matches,
 			regInfo.variance,
-			(int)map_->getWords3().size());
+			regPipeline_->isImageRequired()?(int)map_->getWords3().size():0,
+			regPipeline_->isScanRequired()?(int)map_->sensorData().laserScanRaw().cols:0);
 	return output;
 }
 

corelib/src/RegistrationIcp.cpp

@@ -110,9 +110,9 @@ Transform RegistrationIcp::computeTransformationImpl(
 			dataTo.laserScanRaw().cols,
 			dataTo.laserScanRaw().channels());
 
-	// ICP with guess transform
+	if(!guess.isNull() && !dataFrom.laserScanRaw().empty() && !dataTo.laserScanRaw().empty())
-	if(!dataFrom.laserScanRaw().empty() && !dataTo.laserScanRaw().empty())
 	{
+		// ICP with guess transform
 		int maxLaserScans = dataTo.laserScanMaxPts();
 		cv::Mat fromScan = dataFrom.laserScanRaw();
 		cv::Mat toScan = dataTo.laserScanRaw();
@@ -132,8 +132,7 @@ Transform RegistrationIcp::computeTransformationImpl(
 			int correspondences = 0;
 			double variance = 1.0;
 
-			if( !force3DoF() &&
+			if( _pointToPlane &&
-				_pointToPlane &&
 				_voxelSize == 0.0f &&
 				fromScan.channels() == 6 &&
 				toScan.channels() == 6)
@@ -149,7 +148,9 @@ Transform RegistrationIcp::computeTransformationImpl(
 					   _maxCorrespondenceDistance,
 					   _maxIterations,
 					   hasConverged,
-					   *fromCloudNormalsRegistered);
+					   *fromCloudNormalsRegistered,
+					   _epsilon,
+					   this->force3DoF());
 				if(!icpT.isNull() && hasConverged)
 				{
 					util3d::computeVarianceAndCorrespondences(
@@ -158,6 +159,20 @@ Transform RegistrationIcp::computeTransformationImpl(
 							_maxCorrespondenceDistance,
 							variance,
 							correspondences);
+					/*
+					UWARN("icpT=%s", icpT.prettyPrint().c_str());
+					pcl::io::savePCDFile("fromCloud.pcd", *fromCloudNormals);
+					pcl::io::savePCDFile("toCloud.pcd", *toCloudNormals);
+					UWARN("saved fromCloud.pcd and toCloud.pcd");
+					if(!icpT.isNull())
+					{
+						pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudTmp = util3d::transformPointCloud(fromCloudNormals, icpT);
+						pcl::io::savePCDFile("fromCloudFinal.pcd", *fromCloudTmp);
+						pcl::io::savePCDFile("fromCloudFinal2.pcd", *fromCloudNormalsRegistered);
+						UWARN("saved fromCloudFinal.pcd");
+					}
+					*/
+
 				}
 			}
 			else
@@ -178,7 +193,7 @@ Transform RegistrationIcp::computeTransformationImpl(
 
 				bool correspondencesComputed = false;
 				pcl::PointCloud<pcl::PointXYZ>::Ptr fromCloudRegistered(new pcl::PointCloud<pcl::PointXYZ>());
-				if(!force3DoF() && _pointToPlane) // ICP Point To Plane, only in 3D
+				if(_pointToPlane) // ICP Point To Plane, only in 3D
 				{
 					pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormals = util3d::computeNormals(fromCloudFiltered, _pointToPlaneNormalNeighbors);
 					pcl::PointCloud<pcl::PointNormal>::Ptr toCloudNormals = util3d::computeNormals(toCloudFiltered, _pointToPlaneNormalNeighbors);
@@ -198,7 +213,9 @@ Transform RegistrationIcp::computeTransformationImpl(
 							   _maxCorrespondenceDistance,
 							   _maxIterations,
 							   hasConverged,
-							   *fromCloudNormalsRegistered);
+							   *fromCloudNormalsRegistered,
+							   _epsilon,
+							   this->force3DoF());
 						if(!filtered &&
 							!icpT.isNull() &&
 							hasConverged)
@@ -263,16 +280,13 @@ Transform RegistrationIcp::computeTransformationImpl(
 				hasConverged)
 			{
 				float ix,iy,iz, iroll,ipitch,iyaw;
-				icpT.getTranslationAndEulerAngles(ix,iy,iz,iroll,ipitch,iyaw);
+				Transform icpInTargetReferential = guess.inverse() * icpT.inverse() * guess; // actual local ICP refinement
+				icpInTargetReferential.getTranslationAndEulerAngles(ix,iy,iz,iroll,ipitch,iyaw);
 				if((_maxTranslation>0.0f &&
-					(fabs(ix) > _maxTranslation ||
+					uMax3(fabs(ix), fabs(iy), fabs(iz)) > _maxTranslation)
-					 fabs(iy) > _maxTranslation ||
-					 fabs(iz) > _maxTranslation))
 				   ||
 				   (_maxRotation>0.0f &&
-					 (fabs(iroll) > _maxRotation ||
+					uMax3(fabs(iroll), fabs(ipitch), fabs(iyaw)) > _maxRotation))
-					  fabs(ipitch) > _maxRotation ||
-					  fabs(iyaw) > _maxRotation)))
 				{
 					msg = uFormat("Cannot compute transform (ICP correction too large -> %f m %f rad, limits=%f m, %f rad)",
 							uMax3(fabs(ix), fabs(iy), fabs(iz)),
@@ -331,11 +345,18 @@ Transform RegistrationIcp::computeTransformationImpl(
 			UWARN(msg.c_str());
 		}
 	}
+	else if(dataTo.isValid())
+	{
+		if(guess.isNull())
+		{
+			msg = "RegistrationIcp cannot do registration with a null guess.";
+		}
 		else
 		{
 			msg = uFormat("Laser scans empty?!? (new[%d]=%d old[%d]=%d)",
 					dataTo.id(), dataTo.laserScanRaw().total(),
 					dataFrom.id(), dataFrom.laserScanRaw().total());
+		}
 		UERROR(msg.c_str());
 	}
 

corelib/src/Transform.cpp

@@ -242,11 +242,18 @@ Transform Transform::interpolate(float t, const Transform & other) const
 }
 
 std::string Transform::prettyPrint() const
+{
+	if(this->isNull())
+	{
+		return uFormat("xyz=[null] rpy=[null]");
+	}
+	else
 	{
 		float x,y,z,roll,pitch,yaw;
 		getTranslationAndEulerAngles(x, y, z, roll, pitch, yaw);
 		return uFormat("xyz=%f,%f,%f rpy=%f,%f,%f", x,y,z, roll,pitch,yaw);
 	}
+}
 
 Transform Transform::operator*(const Transform & t) const
 {

corelib/src/util3d.cpp

@@ -1025,7 +1025,6 @@ pcl::PointCloud<pcl::PointNormal>::Ptr laserScanToPointCloudNormal(const cv::Mat
 	pcl::PointCloud<pcl::PointNormal>::Ptr output(new pcl::PointCloud<pcl::PointNormal>);
 	output->resize(laserScan.cols);
 	bool nullTransform = transform.isNull();
-	Eigen::Affine3f transform3f = transform.toEigen3f();
 	for(int i=0; i<laserScan.cols; ++i)
 	{
 		if(laserScan.type() == CV_32FC2)
@@ -1051,7 +1050,7 @@ pcl::PointCloud<pcl::PointNormal>::Ptr laserScanToPointCloudNormal(const cv::Mat
 
 		if(!nullTransform)
 		{
-			output->at(i) = pcl::transformPoint(output->at(i), transform3f);
+			output->at(i) = util3d::transformPoint(output->at(i), transform);
 		}
 	}
 	return output;

corelib/src/util3d_filtering.cpp

@@ -673,6 +673,149 @@ pcl::IndicesPtr subtractFiltering(
 	}
 }
 
+pcl::PointCloud<pcl::PointNormal>::Ptr subtractFiltering(
+		const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
+		const pcl::PointCloud<pcl::PointNormal>::Ptr & substractCloud,
+		float radiusSearch,
+		float maxAngle,
+		int minNeighborsInRadius)
+{
+	pcl::IndicesPtr indices(new std::vector<int>);
+	pcl::IndicesPtr indicesOut = subtractFiltering(cloud, indices, substractCloud, indices, radiusSearch, maxAngle, minNeighborsInRadius);
+	pcl::PointCloud<pcl::PointNormal>::Ptr out(new pcl::PointCloud<pcl::PointNormal>);
+	pcl::copyPointCloud(*cloud, *indicesOut, *out);
+	return out;
+}
+
+
+pcl::IndicesPtr subtractFiltering(
+		const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		const pcl::PointCloud<pcl::PointNormal>::Ptr & substractCloud,
+		const pcl::IndicesPtr & substractIndices,
+		float radiusSearch,
+		float maxAngle,
+		int minNeighborsInRadius)
+{
+	UASSERT(minNeighborsInRadius > 0);
+	pcl::search::KdTree<pcl::PointNormal>::Ptr tree (new pcl::search::KdTree<pcl::PointNormal>(false));
+
+	if(indices->size())
+	{
+		pcl::IndicesPtr output(new std::vector<int>(indices->size()));
+		int oi = 0; // output iterator
+		if(substractIndices->size())
+		{
+			tree->setInputCloud(substractCloud, substractIndices);
+		}
+		else
+		{
+			tree->setInputCloud(substractCloud);
+		}
+		for(unsigned int i=0; i<indices->size(); ++i)
+		{
+			std::vector<int> kIndices;
+			std::vector<float> kDistances;
+			int k = tree->radiusSearch(cloud->at(indices->at(i)), radiusSearch, kIndices, kDistances);
+			if(k>=minNeighborsInRadius && maxAngle > 0.0f)
+			{
+				Eigen::Vector4f normal(cloud->at(indices->at(i)).normal_x, cloud->at(indices->at(i)).normal_y, cloud->at(indices->at(i)).normal_z, 0.0f);
+				if (uIsFinite(normal[0]) &&
+					uIsFinite(normal[1]) &&
+					uIsFinite(normal[2]))
+				{
+					int count = k;
+					for(int j=0; j<count && k >= minNeighborsInRadius; ++j)
+					{
+						Eigen::Vector4f v(substractCloud->at(kIndices.at(j)).normal_x, substractCloud->at(kIndices.at(j)).normal_y, substractCloud->at(kIndices.at(j)).normal_z, 0.0f);
+						if(uIsFinite(v[0]) &&
+							uIsFinite(v[1]) &&
+							uIsFinite(v[2]))
+						{
+							float angle = pcl::getAngle3D(normal, v);
+							if(angle > maxAngle)
+							{
+								k-=1;
+							}
+						}
+						else
+						{
+							k-=1;
+						}
+					}
+				}
+				else
+				{
+					k=0;
+				}
+			}
+			if(k < minNeighborsInRadius)
+			{
+				output->at(oi++) = indices->at(i);
+			}
+		}
+		output->resize(oi);
+		return output;
+	}
+	else
+	{
+		pcl::IndicesPtr output(new std::vector<int>(cloud->size()));
+		int oi = 0; // output iterator
+		if(substractIndices->size())
+		{
+			tree->setInputCloud(substractCloud, substractIndices);
+		}
+		else
+		{
+			tree->setInputCloud(substractCloud);
+		}
+		for(unsigned int i=0; i<cloud->size(); ++i)
+		{
+			std::vector<int> kIndices;
+			std::vector<float> kDistances;
+			int k = tree->radiusSearch(cloud->at(i), radiusSearch, kIndices, kDistances);
+			if(k>=minNeighborsInRadius && maxAngle > 0.0f)
+			{
+				Eigen::Vector4f normal(cloud->at(i).normal_x, cloud->at(i).normal_y, cloud->at(i).normal_z, 0.0f);
+				if (uIsFinite(normal[0]) &&
+					uIsFinite(normal[1]) &&
+					uIsFinite(normal[2]))
+				{
+					int count = k;
+					for(int j=0; j<count && k >= minNeighborsInRadius; ++j)
+					{
+						Eigen::Vector4f v(substractCloud->at(kIndices.at(j)).normal_x, substractCloud->at(kIndices.at(j)).normal_y, substractCloud->at(kIndices.at(j)).normal_z, 0.0f);
+						if(uIsFinite(v[0]) &&
+							uIsFinite(v[1]) &&
+							uIsFinite(v[2]))
+						{
+							float angle = pcl::getAngle3D(normal, v);
+							if(angle > maxAngle)
+							{
+								k-=1;
+							}
+						}
+						else
+						{
+							k-=1;
+						}
+					}
+				}
+				else
+				{
+					k=0;
+				}
+			}
+			if(k < minNeighborsInRadius)
+			{
+				output->at(oi++) = i;
+			}
+		}
+		output->resize(oi);
+		return output;
+	}
+}
+
 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr subtractFiltering(
 		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
 		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & substractCloud,

corelib/src/util3d_mapping.cpp

@@ -401,7 +401,7 @@ cv::Mat create2DMap(const std::map<int, Transform> & poses,
 		float minMapSize,
 		float scanMaxRange)
 {
-	UDEBUG("poses=%d, scans = %d", poses.size(), scans.size());
+	UDEBUG("poses=%d, scans = %d scanMaxRange=%f", poses.size(), scans.size(), scanMaxRange);
 	std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr > localScans;
 
 	// For computation issue, the maximum scan range allowed is 6 meters

corelib/src/util3d_registration.cpp

@@ -306,7 +306,7 @@ Transform icp(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_source,
 			  int maximumIterations,
 			  bool & hasConverged,
 			  pcl::PointCloud<pcl::PointXYZ> & cloud_source_registered,
-			  double epsilon,
+			  float epsilon,
 			  bool icp2D)
 {
 	pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
@@ -344,13 +344,22 @@ Transform icpPointToPlane(
 		double maxCorrespondenceDistance,
 		int maximumIterations,
 		bool & hasConverged,
-		pcl::PointCloud<pcl::PointNormal> & cloud_source_registered)
+		pcl::PointCloud<pcl::PointNormal> & cloud_source_registered,
+		float epsilon,
+		bool icp2D)
 {
 	pcl::IterativeClosestPoint<pcl::PointNormal, pcl::PointNormal> icp;
 	// Set the input source and target
 	icp.setInputTarget (cloud_target);
 	icp.setInputSource (cloud_source);
 
+	if(icp2D)
+	{
+		pcl::registration::TransformationEstimation2D<pcl::PointNormal, pcl::PointNormal>::Ptr est;
+		est.reset(new pcl::registration::TransformationEstimation2D<pcl::PointNormal, pcl::PointNormal>);
+		icp.setTransformationEstimation(est);
+	}
+
 	pcl::registration::TransformationEstimationPointToPlaneLLS<pcl::PointNormal, pcl::PointNormal>::Ptr est;
 	est.reset(new pcl::registration::TransformationEstimationPointToPlaneLLS<pcl::PointNormal, pcl::PointNormal>);
 	icp.setTransformationEstimation(est);
@@ -360,7 +369,7 @@ Transform icpPointToPlane(
 	// Set the maximum number of iterations (criterion 1)
 	icp.setMaximumIterations (maximumIterations);
 	// Set the transformation epsilon (criterion 2)
-	//icp.setTransformationEpsilon (1e-8);
+	icp.setTransformationEpsilon (epsilon);
 	// Set the euclidean distance difference epsilon (criterion 3)
 	//icp.setEuclideanFitnessEpsilon (1);
 	//icp.setRANSACOutlierRejectionThreshold(maxCorrespondenceDistance);

corelib/src/util3d_transforms.cpp

@@ -90,6 +90,23 @@ pcl::PointXYZRGB transformPoint(
 {
 	return pcl::transformPoint(pt, transform.toEigen3f());
 }
+pcl::PointNormal transformPoint(
+		const pcl::PointNormal & point,
+		const Transform & transform)
+{
+	pcl::PointNormal ret;
+	Eigen::Matrix<float, 3, 1> pt (point.x, point.y, point.z);
+	ret.x = static_cast<float> (transform (0, 0) * pt.coeffRef (0) + transform (0, 1) * pt.coeffRef (1) + transform (0, 2) * pt.coeffRef (2) + transform (0, 3));
+	ret.y = static_cast<float> (transform (1, 0) * pt.coeffRef (0) + transform (1, 1) * pt.coeffRef (1) + transform (1, 2) * pt.coeffRef (2) + transform (1, 3));
+	ret.z = static_cast<float> (transform (2, 0) * pt.coeffRef (0) + transform (2, 1) * pt.coeffRef (1) + transform (2, 2) * pt.coeffRef (2) + transform (2, 3));
+
+	// Rotate normals
+	Eigen::Matrix<float, 3, 1> nt (point.normal_x, point.normal_y, point.normal_z);
+	ret.normal_x = static_cast<float> (transform (0, 0) * nt.coeffRef (0) + transform (0, 1) * nt.coeffRef (1) + transform (0, 2) * nt.coeffRef (2));
+	ret.normal_y = static_cast<float> (transform (1, 0) * nt.coeffRef (0) + transform (1, 1) * nt.coeffRef (1) + transform (1, 2) * nt.coeffRef (2));
+	ret.normal_z = static_cast<float> (transform (2, 0) * nt.coeffRef (0) + transform (2, 1) * nt.coeffRef (1) + transform (2, 2) * nt.coeffRef (2));
+	return ret;
+}
 
 }
 

guilib/src/MainWindow.cpp

@@ -500,6 +500,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
 	_ui->statsToolBox->updateStat("Odometry/TimeEstimation/ms", 0.0f);
 	_ui->statsToolBox->updateStat("Odometry/TimeFiltering/ms", 0.0f);
 	_ui->statsToolBox->updateStat("Odometry/LocalMapSize/", 0.0f);
+	_ui->statsToolBox->updateStat("Odometry/LocalScanMapSize/", 0.0f);
 	_ui->statsToolBox->updateStat("Odometry/Interval/ms", 0.0f);
 	_ui->statsToolBox->updateStat("Odometry/Speed/kph", 0.0f);
 	_ui->statsToolBox->updateStat("Odometry/Distance/m", 0.0f);
@@ -895,30 +896,55 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom)
 					}
 				}
 
-				// 2d cloud
+				if(_preferencesDialog->isScansShown(1))
-				if(!odom.data().laserScanRaw().empty() &&
-					_preferencesDialog->isScansShown(1))
 				{
-					pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
+					// scan local map
-					cloud = util3d::laserScanToPointCloud(odom.data().laserScanRaw(), pose);
+					if(!odom.info().localScanMap.empty())
+					{
+						pcl::PointCloud<pcl::PointNormal>::Ptr cloud;
+						cloud = util3d::laserScanToPointCloudNormal(odom.info().localScanMap);
+						if(!_ui->widget_cloudViewer->addCloud("scanMapOdom", cloud, _odometryCorrection, Qt::blue))
+						{
+							UERROR("Adding scanMapOdom to viewer failed!");
+						}
+						else
+						{
+							_ui->widget_cloudViewer->setCloudVisibility("scanMapOdom", true);
+							_ui->widget_cloudViewer->setCloudOpacity("scanMapOdom", _preferencesDialog->getScanOpacity(1));
+							_ui->widget_cloudViewer->setCloudPointSize("scanMapOdom", _preferencesDialog->getScanPointSize(1));
+							scanUpdated = true;
+						}
+					}
+					// scan cloud
+					if(!odom.data().laserScanRaw().empty())
+					{
+						cv::Mat scan = odom.data().laserScanRaw();
+
 						if(_preferencesDialog->getDownsamplingStepScan(1) > 0)
 						{
-						cloud = util3d::downsample(cloud, _preferencesDialog->getDownsamplingStepScan(1));
+							scan = util3d::downsample(scan, _preferencesDialog->getDownsamplingStepScan(1));
 						}
+
+						pcl::PointCloud<pcl::PointNormal>::Ptr cloud;
+						cloud = util3d::laserScanToPointCloudNormal(scan, pose);
 						if(_preferencesDialog->getCloudVoxelSizeScan(1) > 0.0)
 						{
 							cloud = util3d::voxelize(cloud, _preferencesDialog->getCloudVoxelSizeScan(1));
 						}
-					if(!_ui->widget_cloudViewer->addCloud("scanOdom", cloud, _odometryCorrection))
+
+						if(!_ui->widget_cloudViewer->addCloud("scanOdom", cloud, _odometryCorrection, Qt::magenta))
 						{
 							UERROR("Adding scanOdom to viewer failed!");
 						}
+						else
+						{
 							_ui->widget_cloudViewer->setCloudVisibility("scanOdom", true);
 							_ui->widget_cloudViewer->setCloudOpacity("scanOdom", _preferencesDialog->getScanOpacity(1));
 							_ui->widget_cloudViewer->setCloudPointSize("scanOdom", _preferencesDialog->getScanPointSize(1));
-
 							scanUpdated = true;
 						}
+					}
+				}
 
 				// 3d features
 				if(_preferencesDialog->isFeaturesShown(1))
@@ -957,6 +983,10 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom)
 			{
 				_ui->widget_cloudViewer->setCloudVisibility("scanOdom", false);
 			}
+			if(!scanUpdated && _ui->widget_cloudViewer->getAddedClouds().contains("scanMapOdom"))
+			{
+				_ui->widget_cloudViewer->setCloudVisibility("scanMapOdom", false);
+			}
 			if(!featuresUpdated && _ui->widget_cloudViewer->getAddedClouds().contains("featuresOdom"))
 			{
 				_ui->widget_cloudViewer->setCloudVisibility("featuresOdom", false);
@@ -1092,42 +1122,16 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom)
 	}
 
 	//Process info
-	if(odom.info().inliers >= 0)
-	{
 	_ui->statsToolBox->updateStat("Odometry/Inliers/", (float)odom.data().id(), (float)odom.info().inliers);
-	}
-	if(odom.info().icpInliersRatio >= 0)
-	{
 	_ui->statsToolBox->updateStat("Odometry/ICPInliersRatio/", (float)odom.data().id(), (float)odom.info().icpInliersRatio);
-	}
-	if(odom.info().matches >= 0)
-	{
 	_ui->statsToolBox->updateStat("Odometry/Matches/", (float)odom.data().id(), (float)odom.info().matches);
-	}
-	if(odom.info().variance >= 0)
-	{
 	_ui->statsToolBox->updateStat("Odometry/StdDev/", (float)odom.data().id(), sqrt((float)odom.info().variance));
-	}
-	if(odom.info().variance >= 0)
-	{
 	_ui->statsToolBox->updateStat("Odometry/Variance/", (float)odom.data().id(), (float)odom.info().variance);
-	}
-	if(odom.info().timeEstimation > 0)
-	{
 	_ui->statsToolBox->updateStat("Odometry/TimeEstimation/ms", (float)odom.data().id(), (float)odom.info().timeEstimation*1000.0f);
-	}
-	if(odom.info().timeParticleFiltering > 0)
-	{
 	_ui->statsToolBox->updateStat("Odometry/TimeFiltering/ms", (float)odom.data().id(), (float)odom.info().timeParticleFiltering*1000.0f);
-	}
-	if(odom.info().features >=0)
-	{
 	_ui->statsToolBox->updateStat("Odometry/Features/", (float)odom.data().id(), (float)odom.info().features);
-	}
-	if(odom.info().localMapSize >=0)
-	{
 	_ui->statsToolBox->updateStat("Odometry/LocalMapSize/", (float)odom.data().id(), (float)odom.info().localMapSize);
-	}
+	_ui->statsToolBox->updateStat("Odometry/LocalScanMapSize/", (float)odom.data().id(), (float)odom.info().localScanMapSize);
 	_ui->statsToolBox->updateStat("Odometry/ID/", (float)odom.data().id(), (float)odom.data().id());
 
 	float x=0.0f,y,z, roll,pitch,yaw;
@@ -2075,6 +2079,21 @@ void MainWindow::updateMapCloud(
 			_ui->widget_cloudViewer->setCloudPointSize("scanOdom", _preferencesDialog->getScanPointSize(1));
 		}
 	}
+	if(viewerClouds.contains("scanMapOdom"))
+	{
+		if(!_preferencesDialog->isScansShown(1))
+		{
+			UDEBUG("");
+			_ui->widget_cloudViewer->setCloudVisibility("scanMapOdom", false);
+		}
+		else
+		{
+			UDEBUG("");
+			_ui->widget_cloudViewer->updateCloudPose("scanMapOdom", _odometryCorrection);
+			_ui->widget_cloudViewer->setCloudOpacity("scanMapOdom", _preferencesDialog->getScanOpacity(1));
+			_ui->widget_cloudViewer->setCloudPointSize("scanMapOdom", _preferencesDialog->getScanPointSize(1));
+		}
+	}
 	if(viewerClouds.contains("featuresOdom"))
 	{
 		if(!_preferencesDialog->isFeaturesShown(1))

guilib/src/PreferencesDialog.cpp

@@ -732,11 +732,14 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->odom_fillInfoData->setObjectName(Parameters::kOdomFillInfoData().c_str());
 	_ui->odom_dataBufferSize->setObjectName(Parameters::kOdomImageBufferSize().c_str());
 	_ui->odom_flow_keyframeThr->setObjectName(Parameters::kOdomKeyFrameThr().c_str());
+	_ui->odom_flow_scanKeyframeThr->setObjectName(Parameters::kOdomScanKeyFrameThr().c_str());
 	_ui->odom_flow_guessMotion->setObjectName(Parameters::kOdomGuessMotion().c_str());
 
 	//Odometry Frame to Map
 	_ui->odom_localHistory->setObjectName(Parameters::kOdomF2MMaxSize().c_str());
 	_ui->spinBox_odom_f2m_maxNewFeatures->setObjectName(Parameters::kOdomF2MMaxNewFeatures().c_str());
+	_ui->spinBox_odom_f2m_scanMaxSize->setObjectName(Parameters::kOdomF2MScanMaxSize().c_str());
+	_ui->doubleSpinBox_odom_f2m_scanRadius->setObjectName(Parameters::kOdomF2MScanSubstractRadius().c_str());
 	_ui->odom_fixedLocalMapPath->setObjectName(Parameters::kOdomF2MFixedMapPath().c_str());
 	connect(_ui->toolButton_odomBowFixedLocalMap, SIGNAL(clicked()), this, SLOT(changeOdomBowFixedLocalMapPath()));
 

guilib/src/ui/preferencesDialog.ui

@@ -63,7 +63,7 @@
           <property name="geometry">
            <rect>
             <x>0</x>
-            <y>0</y>
+            <y>-605</y>
             <width>681</width>
             <height>2010</height>
            </rect>
@@ -86,7 +86,7 @@
                 <enum>QFrame::Raised</enum>
                </property>
                <property name="currentIndex">
-                <number>11</number>
+                <number>14</number>
                </property>
                <widget class="QWidget" name="page_22">
                 <layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,1">
@@ -7271,6 +7271,26 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                     </item>
                     <item>
                      <layout class="QGridLayout" name="gridLayout_27" columnstretch="0,1">
+                      <item row="6" column="0">
+                       <widget class="QCheckBox" name="odom_flow_guessMotion">
+                        <property name="text">
+                         <string/>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="7" column="0">
+                       <widget class="QDoubleSpinBox" name="odom_flow_keyframeThr">
+                        <property name="maximum">
+                         <double>1.000000000000000</double>
+                        </property>
+                        <property name="singleStep">
+                         <double>0.100000000000000</double>
+                        </property>
+                        <property name="value">
+                         <double>0.500000000000000</double>
+                        </property>
+                       </widget>
+                      </item>
                       <item row="4" column="1">
                        <widget class="QLabel" name="label_233">
                         <property name="text">
@@ -7311,7 +7331,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
-                      <item row="8" column="0">
+                      <item row="9" column="0">
                        <widget class="QSpinBox" name="odom_dataBufferSize">
                         <property name="maximum">
                          <number>999999</number>
@@ -7348,7 +7368,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </item>
                        </widget>
                       </item>
-                      <item row="9" column="0">
+                      <item row="10" column="0">
                        <widget class="QPushButton" name="pushButton_testOdometry">
                         <property name="text">
                          <string>Test selected odometry</string>
@@ -7368,7 +7388,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
-                      <item row="8" column="1">
+                      <item row="9" column="1">
                        <widget class="QLabel" name="label_232">
                         <property name="text">
                          <string>Data buffer size (0 means inf).</string>
@@ -7436,17 +7456,10 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
-                      <item row="6" column="0">
-                       <widget class="QCheckBox" name="odom_flow_guessMotion">
-                        <property name="text">
-                         <string/>
-                        </property>
-                       </widget>
-                      </item>
                       <item row="7" column="1">
                        <widget class="QLabel" name="label_196">
                         <property name="text">
-                         <string>Create a new keyframe when the number of inliers drops under this threshold. Setting value to 0 means that a keyframe is created for each processed frame.</string>
+                         <string>[Visual] Create a new keyframe when the number of inliers drops under this threshold. Setting value to 0 means that a keyframe is created for each processed frame.</string>
                         </property>
                         <property name="wordWrap">
                          <bool>true</bool>
@@ -7456,8 +7469,21 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
-                      <item row="7" column="0">
+                      <item row="8" column="1">
-                       <widget class="QDoubleSpinBox" name="odom_flow_keyframeThr">
+                       <widget class="QLabel" name="label_246">
+                        <property name="text">
+                         <string>[Geometry] Create a new keyframe when the number of inliers drops under this threshold. Setting value to 0 means that a keyframe is created for each processed frame.</string>
+                        </property>
+                        <property name="wordWrap">
+                         <bool>true</bool>
+                        </property>
+                        <property name="textInteractionFlags">
+                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="8" column="0">
+                       <widget class="QDoubleSpinBox" name="odom_flow_scanKeyframeThr">
                         <property name="maximum">
                          <double>1.000000000000000</double>
                         </property>
@@ -7499,10 +7525,10 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                            </item>
                            <item>
                             <layout class="QGridLayout" name="gridLayout_29" columnstretch="0,0,1">
-                             <item row="0" column="2">
+                             <item row="2" column="2">
-                              <widget class="QLabel" name="label_190">
+                              <widget class="QLabel" name="label_195">
                                <property name="text">
-                                <string>Maximum map size: If &gt; 0 (example 5000), the odometry will maintain a local map of X maximum words. This will decrease odometry drifting when the camera is not moving.</string>
+                                <string>[Geometry] Maximum scan map size is defined by this factor times the maximum size of a single scan. For example, if the laser scans have 1000 values, then the maximum local map size will be 2000 if the factor is 2.</string>
                                </property>
                                <property name="wordWrap">
                                 <bool>true</bool>
@@ -7512,33 +7538,10 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                                </property>
                               </widget>
                              </item>
-                             <item row="2" column="0">
+                             <item row="4" column="0">
                               <widget class="QLineEdit" name="odom_fixedLocalMapPath"/>
                              </item>
-                             <item row="0" column="0">
+                             <item row="4" column="2">
-                              <widget class="QSpinBox" name="odom_localHistory">
-                               <property name="minimum">
-                                <number>0</number>
-                               </property>
-                               <property name="maximum">
-                                <number>999999</number>
-                               </property>
-                               <property name="singleStep">
-                                <number>1</number>
-                               </property>
-                               <property name="value">
-                                <number>0</number>
-                               </property>
-                              </widget>
-                             </item>
-                             <item row="2" column="1">
-                              <widget class="QToolButton" name="toolButton_odomBowFixedLocalMap">
-                               <property name="text">
-                                <string>...</string>
-                               </property>
-                              </widget>
-                             </item>
-                             <item row="2" column="2">
                               <widget class="QLabel" name="label_239">
                                <property name="text">
                                 <string>Path to a fixed map (RTAB-Map's database) to be used for odometry. Odometry will be constraint to this map. RGB-only images can be used if odometry PnP pose estimation is activated.</string>
@@ -7551,10 +7554,46 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                                </property>
                               </widget>
                              </item>
+                             <item row="0" column="2">
+                              <widget class="QLabel" name="label_190">
+                               <property name="text">
+                                <string>[Visual] Maximum map size: If &gt; 0 (example 5000), the odometry will maintain a local map of X maximum features. This will decrease odometry drifting when the camera is not moving.</string>
+                               </property>
+                               <property name="wordWrap">
+                                <bool>true</bool>
+                               </property>
+                               <property name="textInteractionFlags">
+                                <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                               </property>
+                              </widget>
+                             </item>
+                             <item row="0" column="0">
+                              <widget class="QSpinBox" name="odom_localHistory">
+                               <property name="minimum">
+                                <number>0</number>
+                               </property>
+                               <property name="maximum">
+                                <number>999999999</number>
+                               </property>
+                               <property name="singleStep">
+                                <number>1</number>
+                               </property>
+                               <property name="value">
+                                <number>0</number>
+                               </property>
+                              </widget>
+                             </item>
+                             <item row="4" column="1">
+                              <widget class="QToolButton" name="toolButton_odomBowFixedLocalMap">
+                               <property name="text">
+                                <string>...</string>
+                               </property>
+                              </widget>
+                             </item>
                              <item row="1" column="2">
                               <widget class="QLabel" name="label_194">
                                <property name="text">
-                                <string>Maximum features (sorted by keypoint response) added to local map from a new key-frame. 0 means no limit.</string>
+                                <string>[Visual] Maximum features (sorted by keypoint response) added to local map from a new key-frame. 0 means no limit.</string>
                                </property>
                                <property name="wordWrap">
                                 <bool>true</bool>
@@ -7580,6 +7619,39 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                                </property>
                               </widget>
                              </item>
+                             <item row="3" column="2">
+                              <widget class="QLabel" name="label_215">
+                               <property name="text">
+                                <string>[Geometry] Radius used to filter points of a new added scan to local map. This could match the voxel size of the laser scans.</string>
+                               </property>
+                               <property name="wordWrap">
+                                <bool>true</bool>
+                               </property>
+                               <property name="textInteractionFlags">
+                                <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                               </property>
+                              </widget>
+                             </item>
+                             <item row="3" column="0">
+                              <widget class="QDoubleSpinBox" name="doubleSpinBox_odom_f2m_scanRadius">
+                               <property name="suffix">
+                                <string> m</string>
+                               </property>
+                               <property name="decimals">
+                                <number>3</number>
+                               </property>
+                               <property name="value">
+                                <double>0.025000000000000</double>
+                               </property>
+                              </widget>
+                             </item>
+                             <item row="2" column="0">
+                              <widget class="QSpinBox" name="spinBox_odom_f2m_scanMaxSize">
+                               <property name="maximum">
+                                <number>999999999</number>
+                               </property>
+                              </widget>
+                             </item>
                             </layout>
                            </item>
                            <item>