Merge branch 'master' of github.com:introlab/rtabmap into devel

corelib/src/Features2d.cpp

@@ -492,30 +492,42 @@ std::vector<cv::KeyPoint> Feature2D::generateKeypoints(const cv::Mat & image, co
 	UASSERT(image.type() == CV_8UC1);
 
 	cv::Mat mask;
-	if(maskIn.type()==CV_16UC1 || maskIn.type() == CV_32FC1)
+	if(!maskIn.empty())
 	{
-		mask = cv::Mat::zeros(maskIn.rows, maskIn.cols, CV_8UC1);
+		if(maskIn.type()==CV_16UC1 || maskIn.type() == CV_32FC1)
-		for(int i=0; i<(int)mask.total(); ++i)
 		{
-			float value = 0.0f;
+			mask = cv::Mat::zeros(maskIn.rows, maskIn.cols, CV_8UC1);
-			if(maskIn.type()==CV_16UC1)
+			for(int i=0; i<(int)mask.total(); ++i)
 			{
-				if(((unsigned short*)maskIn.data)[i] > 0 &&
+				float value = 0.0f;
-				   ((unsigned short*)maskIn.data)[i] < std::numeric_limits<unsigned short>::max())
+				if(maskIn.type()==CV_16UC1)
 				{
-					value = float(((unsigned short*)maskIn.data)[i])*0.001f;
+					if(((unsigned short*)maskIn.data)[i] > 0 &&
+					   ((unsigned short*)maskIn.data)[i] < std::numeric_limits<unsigned short>::max())
+					{
+						value = float(((unsigned short*)maskIn.data)[i])*0.001f;
+					}
+				}
+				else
+				{
+					value = ((float*)maskIn.data)[i];
+				}
+
+				if(value>_minDepth &&
+				   (_maxDepth == 0.0f || value <= _maxDepth))
+				{
+					((unsigned char*)mask.data)[i] = 255; // ORB uses 255 to handle pyramids
 				}
 			}
-			else
+		}
-			{
+		else if(maskIn.type()==CV_8UC1)
-				value = ((float*)maskIn.data)[i];
+		{
-			}
+			// assume a standard mask
-
+			mask = maskIn;
-			if(value>_minDepth &&
+		}
-			   (_maxDepth == 0.0f || value <= _maxDepth))
+		else
-			{
+		{
-				((unsigned char*)mask.data)[i] = 1;
+			UERROR("Wrong mask type (%d)! Should be 8UC1, 16UC1 or 32FC1.", maskIn.type());
-			}
 		}
 	}
 
@@ -527,7 +539,7 @@ std::vector<cv::KeyPoint> Feature2D::generateKeypoints(const cv::Mat & image, co
 	// Get keypoints
 	cv::Rect roi = Feature2D::computeRoi(image, _roiRatios);
 	keypoints = this->generateKeypointsImpl(image, roi.width && roi.height?roi:cv::Rect(0,0,image.cols, image.rows), mask);
-	UDEBUG("Keypoints extraction time = %f s, keypoints extracted = %d", timer.ticks(), keypoints.size());
+	UDEBUG("Keypoints extraction time = %f s, keypoints extracted = %d (mask empty=%d)", timer.ticks(), keypoints.size(), mask.empty()?1:0);
 
 	limitKeypoints(keypoints, maxFeatures_);
 

corelib/src/Memory.cpp

@@ -451,7 +451,7 @@ void Memory::parseParameters(const ParametersMap & parameters)
 	else if(_feature2D)
 	{
 		_feature2D->parseParameters(parameters);
-	}
+	}
 
 	Registration::Type regStrategy = Registration::kTypeUndef;
 	if((iter=parameters.find(Parameters::kRegStrategy())) != parameters.end())
@@ -478,29 +478,29 @@ void Memory::parseParameters(const ParametersMap & parameters)
 	{
 		_registrationIcp->parseParameters(parameters);
 	}
-
+
-	// do this after all parameters are parsed
+	// do this after all parameters are parsed
-	// SLAM mode vs Localization mode
+	// SLAM mode vs Localization mode
-	iter = parameters.find(Parameters::kMemIncrementalMemory());
+	iter = parameters.find(Parameters::kMemIncrementalMemory());
-	if(iter != parameters.end())
+	if(iter != parameters.end())
-	{
+	{
-		bool value = uStr2Bool(iter->second.c_str());
+		bool value = uStr2Bool(iter->second.c_str());
-		if(value == false && _incrementalMemory)
+		if(value == false && _incrementalMemory)
-		{
+		{
-			// From SLAM to localization, change map id
+			// From SLAM to localization, change map id
-			this->incrementMapId();
+			this->incrementMapId();
-
+
-			// The easiest way to make sure that the mapping session is saved
+			// The easiest way to make sure that the mapping session is saved
-			// is to save the memory in the database and reload it.
+			// is to save the memory in the database and reload it.
-			if((_memoryChanged || _linksChanged) && _dbDriver)
+			if((_memoryChanged || _linksChanged) && _dbDriver)
-			{
+			{
-				UWARN("Switching from Mapping to Localization mode, the database will be saved and reloaded.");
+				UWARN("Switching from Mapping to Localization mode, the database will be saved and reloaded.");
 				this->init(_dbDriver->getUrl());
-				UWARN("Switching from Mapping to Localization mode, the database is reloaded!");
+				UWARN("Switching from Mapping to Localization mode, the database is reloaded!");
-			}
+			}
-		}
+		}
-		_incrementalMemory = value;
+		_incrementalMemory = value;
-	}
+	}
 }
 
 void Memory::preUpdate()
@@ -2131,7 +2131,7 @@ Transform Memory::computeTransform(
 	{
 		Signature tmpFrom = fromS;
 		Signature tmpTo = toS;
-
+
 		if(_reextractLoopClosureFeatures)
 		{
 			UDEBUG("");
@@ -3222,8 +3222,7 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
 			}
 
 			cv::Mat depthMask;
-			if(!decimatedData.depthRaw().empty() &&
+			if(!decimatedData.depthRaw().empty())
-			   _feature2D->getType() != Feature2D::kFeatureOrb) // ORB's mask pyramids don't seem to work well
 			{
 				if(imageMono.rows % decimatedData.depthRaw().rows == 0 &&
 					imageMono.cols % decimatedData.depthRaw().cols == 0 &&
@@ -3254,34 +3253,6 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
 					(!decimatedData.rightRaw().empty() && decimatedData.stereoCameraModel().isValidForProjection()))
 			{
 				keypoints3D = _feature2D->generateKeypoints3D(decimatedData, keypoints);
-				if(_feature2D->getMinDepth() > 0.0f || _feature2D->getMaxDepth() > 0.0f)
-				{
-					UDEBUG("");
-					//remove all keypoints/descriptors with no valid 3D points
-					UASSERT((int)keypoints.size() == descriptors.rows &&
-							keypoints3D.size() == keypoints.size());
-					std::vector<cv::KeyPoint> validKeypoints(keypoints.size());
-					std::vector<cv::Point3f> validKeypoints3D(keypoints.size());
-					cv::Mat validDescriptors(descriptors.size(), descriptors.type());
-
-					int oi=0;
-					for(unsigned int i=0; i<keypoints3D.size(); ++i)
-					{
-						if(util3d::isFinite(keypoints3D[i]))
-						{
-							validKeypoints[oi] = keypoints[i];
-							validKeypoints3D[oi] = keypoints3D[i];
-							descriptors.row(i).copyTo(validDescriptors.row(oi));
-							++oi;
-						}
-					}
-					UDEBUG("Removed %d invalid 3D points", (int)keypoints3D.size()-oi);
-					validKeypoints.resize(oi);
-					validKeypoints3D.resize(oi);
-					keypoints = validKeypoints;
-					keypoints3D = validKeypoints3D;
-					descriptors = validDescriptors.rowRange(0, oi).clone();
-				}
 				t = timer.ticks();
 				if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_3D(), t*1000.0f);
 				UDEBUG("time keypoints 3D (%d) = %fs", (int)keypoints3D.size(), t);

corelib/src/RegistrationVis.cpp

@@ -260,8 +260,7 @@ Transform RegistrationVis::computeTransformationImpl(
 					}
 
 					cv::Mat depthMask;
-					if(!fromSignature.sensorData().depthRaw().empty() &&
+					if(!fromSignature.sensorData().depthRaw().empty())
-					   detector->getType() != Feature2D::kFeatureOrb) // ORB's mask pyramids don't seem to work well
 					{
 						if(imageFrom.rows % fromSignature.sensorData().depthRaw().rows == 0 &&
 						   imageFrom.cols % fromSignature.sensorData().depthRaw().cols == 0 &&
@@ -433,8 +432,7 @@ Transform RegistrationVis::computeTransformationImpl(
 					}
 
 					cv::Mat depthMask;
-					if(!toSignature.sensorData().depthRaw().empty() &&
+					if(!toSignature.sensorData().depthRaw().empty())
-						detector->getType() != Feature2D::kFeatureOrb) // ORB's mask pyramids don't seem to work well
 					{
 						if(imageTo.rows % toSignature.sensorData().depthRaw().rows == 0 &&
 						   imageTo.cols % toSignature.sensorData().depthRaw().cols == 0 &&