Added parameters: Mem/ImagePreDecimation Mem/ImagePostDecimation Odom/ImageDecimation

corelib/include/rtabmap/core/Memory.h

@@ -244,7 +244,8 @@ private:
 	bool _generateIds;
 	bool _badSignaturesIgnored;
 	bool _mapLabelsAdded;
-	int _imageDecimation;
+	int _imagePreDecimation;
+	int _imagePostDecimation;
 	float _laserScanDownsampleStepSize;
 	bool _reextractLoopClosureFeatures;
 	float _rehearsalMaxDistance;

corelib/include/rtabmap/core/Odometry.h

@@ -83,6 +83,7 @@ private:
 	bool _fillInfoData;
 	float _kalmanProcessNoise;
 	float _kalmanMeasurementNoise;
+	int _imageDecimation;
 	Transform _pose;
 	int _resetCurrentCount;
 	double previousStamp_;

corelib/include/rtabmap/core/Parameters.h

@@ -205,10 +205,11 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(Mem, RehearsalWeightIgnoredWhileMoving, bool, false, "When the robot is moving, weights are not updated on rehearsal.");
 	RTABMAP_PARAM(Mem, GenerateIds,             bool, true,     "True=Generate location IDs, False=use input image IDs.");
 	RTABMAP_PARAM(Mem, BadSignaturesIgnored,    bool, false,     "Bad signatures are ignored.");
-	RTABMAP_PARAM(Mem, InitWMWithAllNodes,      bool, false,    "Initialize the Working Memory with all nodes in Long-Term Memory. When false, it is initialized with nodes of the previous session.");
+	RTABMAP_PARAM(Mem, InitWMWithAllNodes,      bool, false,     "Initialize the Working Memory with all nodes in Long-Term Memory. When false, it is initialized with nodes of the previous session.");
-	RTABMAP_PARAM(Mem, ImageDecimation,         int, 1,          "Image decimation (>=1) when creating a signature.");
+	RTABMAP_PARAM(Mem, ImagePreDecimation,      int, 1,          "Image decimation (>=1) before features extraction.");
-	RTABMAP_PARAM(Mem, LaserScanDownsampleStepSize, int, 1,  "If > 1, downsample the laser scans when creating a signature.");
+	RTABMAP_PARAM(Mem, ImagePostDecimation,     int, 1,          "Image decimation (>=1) of saved data in created signatures (after features extraction). Decimation is done from the original image.");
-	RTABMAP_PARAM(Mem, UseOdomFeatures,         bool, false,   "Use odometry features.");
+	RTABMAP_PARAM(Mem, LaserScanDownsampleStepSize, int, 1,      "If > 1, downsample the laser scans when creating a signature.");
+	RTABMAP_PARAM(Mem, UseOdomFeatures,             bool, false, "Use odometry features.");
 
 	// KeypointMemory (Keypoint-based)
 	RTABMAP_PARAM(Kp, NNStrategy,            int, 1,            "kNNFlannNaive=0, kNNFlannKdTree=1, kNNFlannLSH=2, kNNBruteForce=3, kNNBruteForceGPU=4");
@@ -365,6 +366,7 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(Odom, GuessMotion,            bool, false,      "Guess next transformation from the last motion computed.");
 	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.");
+	RTABMAP_PARAM(Odom, ImageDecimation,        int, 1,           "Decimation of the images before registration.");
 
 	// Odometry Bag-of-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.");

corelib/src/Memory.cpp

@@ -83,7 +83,8 @@ Memory::Memory(const ParametersMap & parameters) :
 	_generateIds(Parameters::defaultMemGenerateIds()),
 	_badSignaturesIgnored(Parameters::defaultMemBadSignaturesIgnored()),
 	_mapLabelsAdded(Parameters::defaultMemMapLabelsAdded()),
-	_imageDecimation(Parameters::defaultMemImageDecimation()),
+	_imagePreDecimation(Parameters::defaultMemImagePreDecimation()),
+	_imagePostDecimation(Parameters::defaultMemImagePostDecimation()),
 	_laserScanDownsampleStepSize(Parameters::defaultMemLaserScanDownsampleStepSize()),
 	_reextractLoopClosureFeatures(Parameters::defaultRGBDLoopClosureReextractFeatures()),
 	_rehearsalMaxDistance(Parameters::defaultRGBDLinearUpdate()),
@@ -397,7 +398,8 @@ void Memory::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kMemRecentWmRatio(), _recentWmRatio);
 	Parameters::parse(parameters, Parameters::kMemTransferSortingByWeightId(), _transferSortingByWeightId);
 	Parameters::parse(parameters, Parameters::kMemSTMSize(), _maxStMemSize);
-	Parameters::parse(parameters, Parameters::kMemImageDecimation(), _imageDecimation);
+	Parameters::parse(parameters, Parameters::kMemImagePreDecimation(), _imagePreDecimation);
+	Parameters::parse(parameters, Parameters::kMemImagePostDecimation(), _imagePostDecimation);
 	Parameters::parse(parameters, Parameters::kMemLaserScanDownsampleStepSize(), _laserScanDownsampleStepSize);
 	Parameters::parse(parameters, Parameters::kRGBDLoopClosureReextractFeatures(), _reextractLoopClosureFeatures);
 	Parameters::parse(parameters, Parameters::kRGBDLinearUpdate(), _rehearsalMaxDistance);
@@ -408,7 +410,8 @@ void Memory::parseParameters(const ParametersMap & parameters)
 	UASSERT_MSG(_maxStMemSize >= 0, uFormat("value=%d", _maxStMemSize).c_str());
 	UASSERT_MSG(_similarityThreshold >= 0.0f && _similarityThreshold <= 1.0f, uFormat("value=%f", _similarityThreshold).c_str());
 	UASSERT_MSG(_recentWmRatio >= 0.0f && _recentWmRatio <= 1.0f, uFormat("value=%f", _recentWmRatio).c_str());
-	UASSERT(_imageDecimation >= 1);
+	UASSERT(_imagePreDecimation >= 1);
+	UASSERT(_imagePostDecimation >= 1);
 	UASSERT(_rehearsalMaxDistance >= 0.0f);
 	UASSERT(_rehearsalMaxAngle >= 0.0f);
 
@@ -3191,31 +3194,52 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
 		preUpdateThread.start();
 	}
 
+	int preDecimation = 1;
 	std::vector<cv::Point3f> keypoints3D;
 	if(!_useOdometryFeatures || data.keypoints().empty() || (int)data.keypoints().size() != data.descriptors().rows)
 	{
 		if(_feature2D->getMaxFeatures() >= 0 && !data.imageRaw().empty() && !isIntermediateNode)
 		{
+			SensorData decimatedData = data;
+			if(_imagePreDecimation > 1)
+			{
+				preDecimation = _imagePreDecimation;
+				decimatedData.setImageRaw(util2d::decimate(decimatedData.imageRaw(), _imagePreDecimation));
+				decimatedData.setDepthOrRightRaw(util2d::decimate(decimatedData.depthOrRightRaw(), _imagePreDecimation));
+				std::vector<CameraModel> cameraModels = decimatedData.cameraModels();
+				for(unsigned int i=0; i<cameraModels.size(); ++i)
+				{
+					cameraModels[i] = cameraModels[i].scaled(1.0/double(_imagePreDecimation));
+				}
+				decimatedData.setCameraModels(cameraModels);
+				StereoCameraModel stereoModel = decimatedData.stereoCameraModel();
+				if(stereoModel.isValidForProjection())
+				{
+					stereoModel.scale(1.0/double(_imagePreDecimation));
+				}
+				decimatedData.setStereoCameraModel(stereoModel);
+			}
+
 			UINFO("Extract features");
 			cv::Mat imageMono;
-			if(data.imageRaw().channels() == 3)
+			if(decimatedData.imageRaw().channels() == 3)
 			{
-				cv::cvtColor(data.imageRaw(), imageMono, CV_BGR2GRAY);
+				cv::cvtColor(decimatedData.imageRaw(), imageMono, CV_BGR2GRAY);
 			}
 			else
 			{
-				imageMono = data.imageRaw();
+				imageMono = decimatedData.imageRaw();
 			}
 
 			cv::Mat depthMask;
-			if(!data.depthRaw().empty() &&
+			if(!decimatedData.depthRaw().empty() &&
 			   _feature2D->getType() != Feature2D::kFeatureOrb) // ORB's mask pyramids don't seem to work well
 			{
-				if(imageMono.rows % data.depthRaw().rows == 0 &&
+				if(imageMono.rows % decimatedData.depthRaw().rows == 0 &&
-					imageMono.cols % data.depthRaw().cols == 0 &&
+					imageMono.cols % decimatedData.depthRaw().cols == 0 &&
-					imageMono.rows/data.depthRaw().rows == imageMono.cols/data.depthRaw().cols)
+					imageMono.rows/decimatedData.depthRaw().rows == imageMono.cols/decimatedData.depthRaw().cols)
 				{
-					depthMask = util2d::interpolate(data.depthRaw(), imageMono.rows/data.depthRaw().rows, 0.1f);
+					depthMask = util2d::interpolate(decimatedData.depthRaw(), imageMono.rows/decimatedData.depthRaw().rows, 0.1f);
 				}
 			}
 
@@ -3236,10 +3260,10 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
 			{
 				descriptors = cv::Mat();
 			}
-			else if((!data.depthRaw().empty() && data.cameraModels().size() && data.cameraModels()[0].isValidForProjection()) ||
+			else if((!decimatedData.depthRaw().empty() && decimatedData.cameraModels().size() && decimatedData.cameraModels()[0].isValidForProjection()) ||
-					(!data.rightRaw().empty() && data.stereoCameraModel().isValidForProjection()))
+					(!decimatedData.rightRaw().empty() && decimatedData.stereoCameraModel().isValidForProjection()))
 			{
-				keypoints3D = _feature2D->generateKeypoints3D(data, keypoints);
+				keypoints3D = _feature2D->generateKeypoints3D(decimatedData, keypoints);
 				if(_feature2D->getMinDepth() > 0.0f || _feature2D->getMaxDepth() > 0.0f)
 				{
 					UDEBUG("");
@@ -3400,20 +3424,20 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
 		UASSERT(wordIds.size() == keypoints.size());
 		UASSERT(keypoints3D.size() == 0 || keypoints3D.size() == wordIds.size());
 		unsigned int i=0;
+		float decimationRatio = preDecimation / _imagePostDecimation;
 		for(std::list<int>::iterator iter=wordIds.begin(); iter!=wordIds.end() && i < keypoints.size(); ++iter, ++i)
 		{
-			if(_imageDecimation > 1)
+			cv::KeyPoint kpt = keypoints[i];
+			if(preDecimation != _imagePostDecimation)
 			{
-				cv::KeyPoint kpt = keypoints[i];
+				// remap keypoints to final image size
-				kpt.pt.x /= float(_imageDecimation);
+				kpt.pt.x *= decimationRatio;
-				kpt.pt.y /= float(_imageDecimation);
+				kpt.pt.y *= decimationRatio;
-				kpt.size /= float(_imageDecimation);
+				kpt.size *= decimationRatio;
-				words.insert(std::pair<int, cv::KeyPoint>(*iter, kpt));
+				kpt.octave += log2(preDecimation);
-			}
-			else
-			{
-				words.insert(std::pair<int, cv::KeyPoint>(*iter, keypoints[i]));
 			}
+			words.insert(std::pair<int, cv::KeyPoint>(*iter, kpt));
+
 			if(keypoints3D.size())
 			{
 				words3D.insert(std::pair<int, cv::Point3f>(*iter, keypoints3D.at(i)));
@@ -3483,17 +3507,17 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
 	StereoCameraModel stereoCameraModel = data.stereoCameraModel();
 
 	// apply decimation?
-	if(_imageDecimation > 1)
+	if(_imagePostDecimation > 1)
 	{
-		image = util2d::decimate(image, _imageDecimation);
+		image = util2d::decimate(image, _imagePostDecimation);
-		depthOrRightImage = util2d::decimate(depthOrRightImage, _imageDecimation);
+		depthOrRightImage = util2d::decimate(depthOrRightImage, _imagePostDecimation);
 		for(unsigned int i=0; i<cameraModels.size(); ++i)
 		{
-			cameraModels[i] = cameraModels[i].scaled(1.0/double(_imageDecimation));
+			cameraModels[i] = cameraModels[i].scaled(1.0/double(_imagePostDecimation));
 		}
 		if(stereoCameraModel.isValidForProjection())
 		{
-			stereoCameraModel.scale(1.0/double(_imageDecimation));
+			stereoCameraModel.scale(1.0/double(_imagePostDecimation));
 		}
 	}
 

corelib/src/Odometry.cpp

@@ -33,6 +33,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/utilite/UTimer.h"
 #include "rtabmap/utilite/UConversion.h"
 #include "rtabmap/core/ParticleFilter.h"
+#include "rtabmap/core/util2d.h"
 
 namespace rtabmap {
 
@@ -75,6 +76,7 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
 		_fillInfoData(Parameters::defaultOdomFillInfoData()),
 		_kalmanProcessNoise(Parameters::defaultOdomKalmanProcessNoise()),
 		_kalmanMeasurementNoise(Parameters::defaultOdomKalmanMeasurementNoise()),
+		_imageDecimation(Parameters::defaultOdomImageDecimation()),
 		_resetCurrentCount(0),
 		previousStamp_(0),
 		distanceTravelled_(0)
@@ -97,6 +99,9 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
 	UASSERT(_particleLambdaR>0);
 	Parameters::parse(parameters, Parameters::kOdomKalmanProcessNoise(), _kalmanProcessNoise);
 	Parameters::parse(parameters, Parameters::kOdomKalmanMeasurementNoise(), _kalmanMeasurementNoise);
+	Parameters::parse(parameters, Parameters::kOdomImageDecimation(), _imageDecimation);
+	UASSERT(_imageDecimation>=1);
+
 	if(_filteringStrategy == 2)
 	{
 		// Initialize the Particle filters
@@ -223,7 +228,63 @@ Transform Odometry::process(SensorData & data, OdometryInfo * info)
 	}
 
 	UTimer time;
-	Transform t = this->computeTransform(data, guess, info);
+	Transform t;
+	if(_imageDecimation > 1)
+	{
+		// Decimation of images with calibrations
+		SensorData decimatedData = data;
+		decimatedData.setImageRaw(util2d::decimate(decimatedData.imageRaw(), _imageDecimation));
+		decimatedData.setDepthOrRightRaw(util2d::decimate(decimatedData.depthOrRightRaw(), _imageDecimation));
+		std::vector<CameraModel> cameraModels = decimatedData.cameraModels();
+		for(unsigned int i=0; i<cameraModels.size(); ++i)
+		{
+			cameraModels[i] = cameraModels[i].scaled(1.0/double(_imageDecimation));
+		}
+		decimatedData.setCameraModels(cameraModels);
+		StereoCameraModel stereoModel = decimatedData.stereoCameraModel();
+		if(stereoModel.isValidForProjection())
+		{
+			stereoModel.scale(1.0/double(_imageDecimation));
+		}
+		decimatedData.setStereoCameraModel(stereoModel);
+
+		// compute transform
+		t = this->computeTransform(decimatedData, guess, info);
+
+		// transform back the keypoints in the original image
+		std::vector<cv::KeyPoint> kpts = decimatedData.keypoints();
+		for(unsigned int i=0; i<kpts.size(); ++i)
+		{
+			kpts[i].pt.x *= _imageDecimation;
+			kpts[i].pt.y *= _imageDecimation;
+			kpts[i].size *= _imageDecimation;
+			kpts[i].octave += log2(_imageDecimation);
+		}
+		data.setFeatures(kpts, decimatedData.descriptors());
+
+		if(info)
+		{
+			UASSERT(info->newCorners.size() == info->refCorners.size());
+			for(unsigned int i=0; i<info->newCorners.size(); ++i)
+			{
+				info->refCorners[i].x *= _imageDecimation;
+				info->refCorners[i].y *= _imageDecimation;
+				info->newCorners[i].x *= _imageDecimation;
+				info->newCorners[i].y *= _imageDecimation;
+			}
+			for(std::multimap<int, cv::KeyPoint>::iterator iter=info->words.begin(); iter!=info->words.end(); ++iter)
+			{
+				iter->second.pt.x *= _imageDecimation;
+				iter->second.pt.y *= _imageDecimation;
+				iter->second.size *= _imageDecimation;
+				iter->second.octave += log2(_imageDecimation);
+			}
+		}
+	}
+	else
+	{
+		t = this->computeTransform(data, guess, info);
+	}
 
 	if(info)
 	{

corelib/src/Parameters.cpp

@@ -155,6 +155,9 @@ const std::map<std::string, std::pair<bool, std::string> > & Parameters::getRemo
 	{
 		// removed parameters
 
+		// 0.11.3
+		removedParameters_.insert(std::make_pair("Mem/ImageDecimation",      std::make_pair(true, Parameters::kMemImagePostDecimation())));
+
 		// 0.11.2
 		removedParameters_.insert(std::make_pair("OdomLocalMap/HistorySize",      std::make_pair(true, Parameters::kOdomF2MMaxSize())));
 		removedParameters_.insert(std::make_pair("OdomLocalMap/FixedMapPath",     std::make_pair(true, Parameters::kOdomF2MFixedMapPath())));

corelib/src/RegistrationVis.cpp

@@ -243,33 +243,36 @@ Transform RegistrationVis::computeTransformationImpl(
 
 		Feature2D * detector = createFeatureDetector();
 		std::vector<cv::KeyPoint> kptsFrom;
+		cv::Mat imageFrom = fromSignature.sensorData().imageRaw();
+		cv::Mat imageTo = toSignature.sensorData().imageRaw();
+
 		if(fromSignature.getWords().empty())
 		{
 			if(fromSignature.sensorData().keypoints().empty())
 			{
-				if(!fromSignature.sensorData().imageRaw().empty())
+				if(!imageFrom.empty())
 				{
-					if(fromSignature.sensorData().imageRaw().channels() > 1)
+					if(imageFrom.channels() > 1)
 					{
 						cv::Mat tmp;
-						cv::cvtColor(fromSignature.sensorData().imageRaw(), tmp, cv::COLOR_BGR2GRAY);
+						cv::cvtColor(imageFrom, tmp, cv::COLOR_BGR2GRAY);
-						fromSignature.sensorData().setImageRaw(tmp);
+						imageFrom = tmp;
 					}
 
 					cv::Mat depthMask;
 					if(!fromSignature.sensorData().depthRaw().empty() &&
 					   detector->getType() != Feature2D::kFeatureOrb) // ORB's mask pyramids don't seem to work well
 					{
-						if(fromSignature.sensorData().imageRaw().rows % fromSignature.sensorData().depthRaw().rows == 0 &&
+						if(imageFrom.rows % fromSignature.sensorData().depthRaw().rows == 0 &&
-							fromSignature.sensorData().imageRaw().cols % fromSignature.sensorData().depthRaw().cols == 0 &&
+						   imageFrom.cols % fromSignature.sensorData().depthRaw().cols == 0 &&
-							fromSignature.sensorData().imageRaw().rows/fromSignature.sensorData().depthRaw().rows == fromSignature.sensorData().imageRaw().cols/fromSignature.sensorData().depthRaw().cols)
+						   imageFrom.rows/fromSignature.sensorData().depthRaw().rows == fromSignature.sensorData().imageRaw().cols/fromSignature.sensorData().depthRaw().cols)
 						{
 							depthMask = util2d::interpolate(fromSignature.sensorData().depthRaw(), fromSignature.sensorData().imageRaw().rows/fromSignature.sensorData().depthRaw().rows, 0.1f);
 						}
 					}
 
 					kptsFrom = detector->generateKeypoints(
-							fromSignature.sensorData().imageRaw(),
+							imageFrom,
 							depthMask);
 				}
 			}
@@ -290,22 +293,22 @@ Transform RegistrationVis::computeTransformationImpl(
 		std::multimap<int, cv::Mat> wordsDescFrom;
 		std::multimap<int, cv::Mat> wordsDescTo;
 		if(_correspondencesApproach == 1 && //Optical Flow
-		   !fromSignature.sensorData().imageRaw().empty() &&
+		   !imageFrom.empty() &&
-		   !toSignature.sensorData().imageRaw().empty())
+		   !imageTo.empty())
 		{
 			UDEBUG("");
 			// convert to grayscale
-			if(fromSignature.sensorData().imageRaw().channels() > 1)
+			if(imageFrom.channels() > 1)
 			{
 				cv::Mat tmp;
-				cv::cvtColor(fromSignature.sensorData().imageRaw(), tmp, cv::COLOR_BGR2GRAY);
+				cv::cvtColor(imageFrom, tmp, cv::COLOR_BGR2GRAY);
-				fromSignature.sensorData().setImageRaw(tmp);
+				imageFrom = tmp;
 			}
-			if(toSignature.sensorData().imageRaw().channels() > 1)
+			if(imageTo.channels() > 1)
 			{
 				cv::Mat tmp;
-				cv::cvtColor(toSignature.sensorData().imageRaw(), tmp, cv::COLOR_BGR2GRAY);
+				cv::cvtColor(imageTo, tmp, cv::COLOR_BGR2GRAY);
-				toSignature.sensorData().setImageRaw(tmp);
+				imageTo = tmp;
 			}
 
 			std::vector<cv::Point3f> kptsFrom3D;
@@ -318,7 +321,7 @@ Transform RegistrationVis::computeTransformationImpl(
 				kptsFrom3D = uValues(fromSignature.getWords3());
 			}
 
-			if(!toSignature.sensorData().imageRaw().empty())
+			if(!imageTo.empty())
 			{
 				std::vector<cv::Point2f> cornersFrom;
 				cv::KeyPoint::convert(kptsFrom, cornersFrom);
@@ -345,8 +348,8 @@ Transform RegistrationVis::computeTransformationImpl(
 				std::vector<float> err;
 				UDEBUG("cv::calcOpticalFlowPyrLK() begin");
 				cv::calcOpticalFlowPyrLK(
-						fromSignature.sensorData().imageRaw(),
+						imageFrom,
-						toSignature.sensorData().imageRaw(),
+						imageTo,
 						cornersFrom,
 						cornersTo,
 						status,
@@ -364,8 +367,8 @@ Transform RegistrationVis::computeTransformationImpl(
 				for(unsigned int i=0; i<status.size(); ++i)
 				{
 					if(status[i] &&
-					   uIsInBounds(cornersTo[i].x, 0.0f, float(toSignature.sensorData().imageRaw().cols)) &&
+					   uIsInBounds(cornersTo[i].x, 0.0f, float(imageTo.cols)) &&
-					   uIsInBounds(cornersTo[i].y, 0.0f, float(toSignature.sensorData().imageRaw().rows)))
+					   uIsInBounds(cornersTo[i].y, 0.0f, float(imageTo.rows)))
 					{
 						kptsFrom[ki] = cv::KeyPoint(cornersFrom[i], 1);
 						kptsFrom3DKept[ki] = kptsFrom3D[i];
@@ -420,29 +423,29 @@ Transform RegistrationVis::computeTransformationImpl(
 			if(toSignature.getWords().empty())
 			{
 				if(toSignature.sensorData().keypoints().empty() &&
-				   !toSignature.sensorData().imageRaw().empty())
+				   !imageTo.empty())
 				{
-					if(toSignature.sensorData().imageRaw().channels() > 1)
+					if(imageTo.channels() > 1)
 					{
 						cv::Mat tmp;
-						cv::cvtColor(toSignature.sensorData().imageRaw(), tmp, cv::COLOR_BGR2GRAY);
+						cv::cvtColor(imageTo, tmp, cv::COLOR_BGR2GRAY);
-						toSignature.sensorData().setImageRaw(tmp);
+						imageTo = tmp;
 					}
 
 					cv::Mat depthMask;
 					if(!toSignature.sensorData().depthRaw().empty() &&
 						detector->getType() != Feature2D::kFeatureOrb) // ORB's mask pyramids don't seem to work well
 					{
-						if(toSignature.sensorData().imageRaw().rows % toSignature.sensorData().depthRaw().rows == 0 &&
+						if(imageTo.rows % toSignature.sensorData().depthRaw().rows == 0 &&
-							toSignature.sensorData().imageRaw().cols % toSignature.sensorData().depthRaw().cols == 0 &&
+						   imageTo.cols % toSignature.sensorData().depthRaw().cols == 0 &&
-							toSignature.sensorData().imageRaw().rows/toSignature.sensorData().depthRaw().rows == toSignature.sensorData().imageRaw().cols/toSignature.sensorData().depthRaw().cols)
+						   imageTo.rows/toSignature.sensorData().depthRaw().rows == imageTo.cols/toSignature.sensorData().depthRaw().cols)
 						{
-							depthMask = util2d::interpolate(toSignature.sensorData().depthRaw(), toSignature.sensorData().imageRaw().rows/toSignature.sensorData().depthRaw().rows, 0.1f);
+							depthMask = util2d::interpolate(toSignature.sensorData().depthRaw(), imageTo.rows/toSignature.sensorData().depthRaw().rows, 0.1f);
 						}
 					}
 
 					kptsTo = detector->generateKeypoints(
-							toSignature.sensorData().imageRaw(),
+							imageTo,
 							depthMask);
 				}
 				else
@@ -477,15 +480,15 @@ Transform RegistrationVis::computeTransformationImpl(
 			{
 				descriptorsFrom = fromSignature.sensorData().descriptors();
 			}
-			else if(!fromSignature.sensorData().imageRaw().empty())
+			else if(!imageFrom.empty())
 			{
-				if(fromSignature.sensorData().imageRaw().channels() > 1)
+				if(imageFrom.channels() > 1)
 				{
 					cv::Mat tmp;
-					cv::cvtColor(fromSignature.sensorData().imageRaw(), tmp, cv::COLOR_BGR2GRAY);
+					cv::cvtColor(imageFrom, tmp, cv::COLOR_BGR2GRAY);
-					fromSignature.sensorData().setImageRaw(tmp);
+					imageFrom = tmp;
 				}
-				descriptorsFrom = detector->generateDescriptors(fromSignature.sensorData().imageRaw(), kptsFrom);
+				descriptorsFrom = detector->generateDescriptors(imageFrom, kptsFrom);
 			}
 
 			cv::Mat descriptorsTo;
@@ -508,16 +511,16 @@ Transform RegistrationVis::computeTransformationImpl(
 				{
 					descriptorsTo = toSignature.sensorData().descriptors();
 				}
-				else if(!toSignature.sensorData().imageRaw().empty())
+				else if(!imageTo.empty())
 				{
-					if(toSignature.sensorData().imageRaw().channels() > 1)
+					if(imageTo.channels() > 1)
 					{
 						cv::Mat tmp;
-						cv::cvtColor(toSignature.sensorData().imageRaw(), tmp, cv::COLOR_BGR2GRAY);
+						cv::cvtColor(imageTo, tmp, cv::COLOR_BGR2GRAY);
-						toSignature.sensorData().setImageRaw(tmp);
+						imageTo = tmp;
 					}
 
-					descriptorsTo = detector->generateDescriptors(toSignature.sensorData().imageRaw(), kptsTo);
+					descriptorsTo = detector->generateDescriptors(imageTo, kptsTo);
 				}
 			}
 
@@ -618,7 +621,7 @@ Transform RegistrationVis::computeTransformationImpl(
 			// We have all data we need here, so match!
 			if(descriptorsFrom.rows > 0 && descriptorsTo.rows > 0)
 			{
-				cv::Size imageSize = toSignature.sensorData().imageRaw().size();
+				cv::Size imageSize = imageTo.size();
 				bool isCalibrated = false;
 				if(imageSize.height == 0 || imageSize.width == 0)
 				{
@@ -931,7 +934,7 @@ Transform RegistrationVis::computeTransformationImpl(
 	float variance = 1.0f;
 	int inliersCount = 0;
 	int matchesCount = 0;
-	if(toSignature.getWords().size() || !toSignature.sensorData().imageRaw().empty())
+	if(toSignature.getWords().size())
 	{
 		Transform transforms[2];
 		std::vector<int> inliers[2];

corelib/src/RtabmapThread.cpp

@@ -545,12 +545,13 @@ void RtabmapThread::addData(const OdometryEvent & odomEvent)
 			}
 		}
 		if(_dataBufferMaxSize > 0 &&
-				((!lastPose_.isIdentity() && odomEvent.pose().isIdentity()) ||
+				(!lastPose_.isIdentity() &&
+					   (odomEvent.pose().isIdentity() ||
 						odomEvent.info().variance>=9999 ||
 						odomEvent.rotVariance()>=9999 ||
-						odomEvent.transVariance()>=9999))
+						odomEvent.transVariance()>=9999)))
 		{
-			UWARN("Odometry is reset (identity pose or high variance (>=9999) detected). Increment map id!");
+			UWARN("Odometry is reset (identity pose or high variance >=9999 detected). Increment map id!");
 			pushNewState(kStateTriggeringMap);
 			_rotVariance = 0;
 			_transVariance = 0;

guilib/src/ExportCloudsDialog.cpp

@@ -905,7 +905,7 @@ bool ExportCloudsDialog::getExportedClouds(
 						for(unsigned int i=0; i<textureMesh->tex_polygons[0].size(); ++i)
 						{
 							const pcl::Vertices & vertices = textureMesh->tex_polygons[0][i];
-							UASSERT(polygonSize == vertices.vertices.size());
+							UASSERT(polygonSize == (int)vertices.vertices.size());
 							for(int k=0; k<polygonSize; ++k)
 							{
 								//uv

guilib/src/PreferencesDialog.cpp

@@ -530,7 +530,8 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->general_checkBox_createMapLabels->setObjectName(Parameters::kMemMapLabelsAdded().c_str());
 	_ui->general_checkBox_initWMWithAllNodes->setObjectName(Parameters::kMemInitWMWithAllNodes().c_str());
 	_ui->checkBox_localSpaceScanMatchingIDsSaved->setObjectName(Parameters::kRGBDScanMatchingIdsSavedInLinks().c_str());
-	_ui->spinBox_imageDecimation->setObjectName(Parameters::kMemImageDecimation().c_str());
+	_ui->spinBox_imagePreDecimation->setObjectName(Parameters::kMemImagePreDecimation().c_str());
+	_ui->spinBox_imagePostDecimation->setObjectName(Parameters::kMemImagePostDecimation().c_str());
 	_ui->general_spinBox_laserScanDownsample->setObjectName(Parameters::kMemLaserScanDownsampleStepSize().c_str());
 	_ui->checkBox_useOdomFeatures->setObjectName(Parameters::kMemUseOdomFeatures().c_str());
 
@@ -738,6 +739,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_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());
+	_ui->odom_imageDecimation->setObjectName(Parameters::kOdomImageDecimation().c_str());
 
 	//Odometry Frame to Map
 	_ui->odom_localHistory->setObjectName(Parameters::kOdomF2MMaxSize().c_str());

guilib/src/ui/DatabaseViewer.ui

@@ -52,11 +52,11 @@
           <rect>
            <x>0</x>
            <y>0</y>
-           <width>205</width>
+           <width>202</width>
-           <height>208</height>
+           <height>196</height>
           </rect>
          </property>
-         <layout class="QGridLayout" name="gridLayout" rowstretch="0,0,0,0,0,0,0,0" columnstretch="1,1">
+         <layout class="QGridLayout" name="gridLayout" columnstretch="0,1">
           <item row="1" column="1">
            <widget class="QLabel" name="label_childrenA">
             <property name="text">
@@ -210,8 +210,8 @@
           <rect>
            <x>0</x>
            <y>0</y>
-           <width>203</width>
+           <width>201</width>
-           <height>208</height>
+           <height>196</height>
           </rect>
          </property>
          <layout class="QGridLayout" name="gridLayout_2" columnstretch="0,1">
@@ -484,7 +484,7 @@
      <x>0</x>
      <y>0</y>
      <width>1285</width>
-     <height>22</height>
+     <height>25</height>
     </rect>
    </property>
    <widget class="QMenu" name="menuFile">
@@ -983,8 +983,8 @@
          <rect>
           <x>0</x>
           <y>0</y>
-          <width>324</width>
+          <width>333</width>
-          <height>188</height>
+          <height>186</height>
          </rect>
         </property>
         <attribute name="label">
@@ -1119,8 +1119,8 @@
          <rect>
           <x>0</x>
           <y>0</y>
-          <width>280</width>
+          <width>278</width>
-          <height>628</height>
+          <height>578</height>
          </rect>
         </property>
         <attribute name="label">
@@ -1501,8 +1501,8 @@
          <rect>
           <x>0</x>
           <y>0</y>
-          <width>201</width>
+          <width>289</width>
-          <height>126</height>
+          <height>182</height>
          </rect>
         </property>
         <attribute name="label">
@@ -1601,8 +1601,8 @@
          <rect>
           <x>0</x>
           <y>0</y>
-          <width>173</width>
+          <width>289</width>
-          <height>166</height>
+          <height>182</height>
          </rect>
         </property>
         <attribute name="label">

guilib/src/ui/preferencesDialog.ui

@@ -64,8 +64,8 @@
            <rect>
             <x>0</x>
             <y>0</y>
-            <width>681</width>
+            <width>686</width>
-            <height>2044</height>
+            <height>2023</height>
            </rect>
           </property>
           <layout class="QVBoxLayout" name="verticalLayout_16">
@@ -86,7 +86,7 @@
                 <enum>QFrame::Raised</enum>
                </property>
                <property name="currentIndex">
-                <number>12</number>
+                <number>14</number>
                </property>
                <widget class="QWidget" name="page_22">
                 <layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,1">
@@ -4662,6 +4662,16 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
+                      <item row="13" column="0">
+                       <widget class="QSpinBox" name="spinBox_imagePostDecimation">
+                        <property name="minimum">
+                         <number>1</number>
+                        </property>
+                        <property name="maximum">
+                         <number>16</number>
+                        </property>
+                       </widget>
+                      </item>
                       <item row="7" column="1">
                        <widget class="QLabel" name="label_retrieved_10">
                         <property name="text">
@@ -4802,10 +4812,10 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
-                      <item row="12" column="1">
+                      <item row="13" column="1">
                        <widget class="QLabel" name="label_retrieved_6">
                         <property name="text">
-                         <string>Image decimation. This feature can be used to save images in lower resolution (size/decimation).</string>
+                         <string>Image post decimation. This option can be used to save images in lower resolution (size/decimation). It is done on the original image.</string>
                         </property>
                         <property name="wordWrap">
                          <bool>true</bool>
@@ -4828,7 +4838,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
-                      <item row="13" column="1">
+                      <item row="14" column="1">
                        <widget class="QLabel" name="label_retrieved_8">
                         <property name="text">
                          <string>If &gt; 1, downsample the laser scans when creating a location. This feature can be used to save laser scans already downsampled.</string>
@@ -4874,7 +4884,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
-                      <item row="13" column="0">
+                      <item row="14" column="0">
                        <widget class="QSpinBox" name="general_spinBox_laserScanDownsample">
                         <property name="minimumSize">
                          <size>
@@ -4890,16 +4900,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
-                      <item row="12" column="0">
-                       <widget class="QSpinBox" name="spinBox_imageDecimation">
-                        <property name="minimum">
-                         <number>1</number>
-                        </property>
-                        <property name="maximum">
-                         <number>16</number>
-                        </property>
-                       </widget>
-                      </item>
                       <item row="11" column="0">
                        <widget class="QCheckBox" name="general_checkBox_saveDepth16bits">
                         <property name="text">
@@ -4933,6 +4933,29 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
+                      <item row="12" column="0">
+                       <widget class="QSpinBox" name="spinBox_imagePreDecimation">
+                        <property name="minimum">
+                         <number>1</number>
+                        </property>
+                        <property name="maximum">
+                         <number>16</number>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="12" column="1">
+                       <widget class="QLabel" name="label_retrieved_13">
+                        <property name="text">
+                         <string>Image pre decimation. This option can be used to reduce image size before features extraction.</string>
+                        </property>
+                        <property name="wordWrap">
+                         <bool>true</bool>
+                        </property>
+                        <property name="textInteractionFlags">
+                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                        </property>
+                       </widget>
+                      </item>
                      </layout>
                     </item>
                     <item>
@@ -7291,14 +7314,7 @@ 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">
+                      <item row="8" 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>
@@ -7311,6 +7327,13 @@ 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="4" column="1">
                        <widget class="QLabel" name="label_233">
                         <property name="text">
@@ -7351,7 +7374,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
-                      <item row="9" column="0">
+                      <item row="10" column="0">
                        <widget class="QSpinBox" name="odom_dataBufferSize">
                         <property name="maximum">
                          <number>999999</number>
@@ -7388,7 +7411,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </item>
                        </widget>
                       </item>
-                      <item row="10" column="0">
+                      <item row="11" column="0">
                        <widget class="QPushButton" name="pushButton_testOdometry">
                         <property name="text">
                          <string>Test selected odometry</string>
@@ -7408,7 +7431,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
-                      <item row="9" column="1">
+                      <item row="10" column="1">
                        <widget class="QLabel" name="label_232">
                         <property name="text">
                          <string>Data buffer size (0 means inf).</string>
@@ -7476,7 +7499,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
-                      <item row="7" column="1">
+                      <item row="8" column="1">
                        <widget class="QLabel" name="label_196">
                         <property name="text">
                          <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>
@@ -7489,7 +7512,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_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>
@@ -7502,7 +7525,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="QDoubleSpinBox" name="odom_flow_scanKeyframeThr">
                         <property name="maximum">
                          <double>1.000000000000000</double>
@@ -7515,6 +7538,26 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
+                      <item row="7" column="1">
+                       <widget class="QLabel" name="label_248">
+                        <property name="text">
+                         <string>Decimation of images before features extraction.</string>
+                        </property>
+                        <property name="wordWrap">
+                         <bool>true</bool>
+                        </property>
+                        <property name="textInteractionFlags">
+                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="7" column="0">
+                       <widget class="QSpinBox" name="odom_imageDecimation">
+                        <property name="minimum">
+                         <number>1</number>
+                        </property>
+                       </widget>
+                      </item>
                      </layout>
                     </item>
                     <item>