Fixed crash when using binary descriptors and maxWords/maxDepth are set

Added BRISK detector

git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@1839 f169173b-cf89-36c8-b27e-44dbe73f0c83

corelib/include/rtabmap/core/Features2d.h

@@ -81,7 +81,8 @@ public:
 		kFeatureFastFreak=3,
 		kFeatureFastBrief=4,
 		kFeatureGfttFreak=5,
-		kFeatureGfttBrief=6};
+		kFeatureGfttBrief=6,
+		kFeatureBrisk=7};
 
 public:
 	virtual ~Feature2D() {}
@@ -310,6 +311,28 @@ private:
 	cv::FREAK * _freak;
 };
 
+//BRISK
+class RTABMAP_EXP BRISK : public Feature2D
+{
+public:
+	BRISK(const ParametersMap & parameters = ParametersMap());
+	virtual ~BRISK();
+
+	virtual void parseParameters(const ParametersMap & parameters);
+	virtual Feature2D::Type getType() const {return kFeatureBrisk;}
+
+private:
+	virtual std::vector<cv::KeyPoint> generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi) const;
+	virtual cv::Mat generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const;
+
+private:
+	int thresh_;
+	int octaves_;
+	float patternScale_;
+
+	cv::BRISK * brisk_;
+};
+
 
 }
 

corelib/include/rtabmap/core/Parameters.h

@@ -226,6 +226,10 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(FREAK, PatternScale,          float, 22.0,  "Scaling of the description pattern.");
 	RTABMAP_PARAM(FREAK, NOctaves,              int, 4,        "Number of octaves covered by the detected keypoints.");
 
+	RTABMAP_PARAM(BRISK, Thresh,                int, 30,      "FAST/AGAST detection threshold score.");
+	RTABMAP_PARAM(BRISK, Octaves,               int, 3,       "Detection octaves. Use 0 to do single scale.");
+	RTABMAP_PARAM(BRISK, PatternScale,          float, 1.0,  "Apply this scale to the pattern used for sampling the neighbourhood of a keypoint.");
+
 	// BayesFilter
 	RTABMAP_PARAM(Bayes, VirtualPlacePriorThr,           float, 0.9, "Virtual place prior");
 	RTABMAP_PARAM_STR(Bayes, PredictionLC, "0.1 0.36 0.30 0.16 0.062 0.0151 0.00255 0.000324 2.5e-05 1.3e-06 4.8e-08 1.2e-09 1.9e-11 2.2e-13 1.7e-15 8.5e-18 2.9e-20 6.9e-23", "Prediction of loop closures (Gaussian-like, here with sigma=1.6) - Format: {VirtualPlaceProb, LoopClosureProb, NeighborLvl1, NeighborLvl2, ...}.");

corelib/include/rtabmap/core/SensorData.h

@@ -31,6 +31,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/RtabmapExp.h>
 #include <rtabmap/core/Transform.h>
 #include <opencv2/core/core.hpp>
+#include <opencv2/features2d/features2d.hpp>
 
 namespace rtabmap
 {
@@ -88,6 +89,14 @@ public:
 	const Transform & pose() const {return _pose;}
 	const Transform & localTransform() const {return _localTransform;}
 
+	void setFeatures(const std::vector<cv::KeyPoint> & keypoints, const cv::Mat & descriptors)
+	{
+		_keypoints = keypoints;
+		_descriptors = descriptors;
+	}
+	const std::vector<cv::KeyPoint> & keypoints() const {return _keypoints;}
+	const cv::Mat & descriptors() const {return _descriptors;}
+
 private:
 	cv::Mat _image;
 	int _id;
@@ -101,6 +110,10 @@ private:
 	float _cy;
 	Transform _pose;
 	Transform _localTransform;
+
+	// features
+	std::vector<cv::KeyPoint> _keypoints;
+	cv::Mat _descriptors;
 };
 
 }

corelib/src/Features2d.cpp

@@ -100,7 +100,14 @@ void filterKeypointsByDepth(
 				{
 					if(indexes[i] == 1)
 					{
-						memcpy(newDescriptors.ptr<float>(di++), descriptors.ptr<float>(i), descriptors.cols*sizeof(float));
+						if(descriptors.type() == CV_32FC1)
+						{
+							memcpy(newDescriptors.ptr<float>(di++), descriptors.ptr<float>(i), descriptors.cols*sizeof(float));
+						}
+						else // CV_8UC1
+						{
+							memcpy(newDescriptors.ptr<char>(di++), descriptors.ptr<char>(i), descriptors.cols*sizeof(char));
+						}
 					}
 				}
 				descriptors = newDescriptors;
@@ -146,7 +153,14 @@ void limitKeypoints(std::vector<cv::KeyPoint> & keypoints, cv::Mat & descriptors
 			kptsTmp[k] = keypoints[iter->second];
 			if(descriptors.rows)
 			{
-				memcpy(descriptorsTmp.ptr<float>(k), descriptors.ptr<float>(iter->second), descriptors.cols*sizeof(float));
+				if(descriptors.type() == CV_32FC1)
+				{
+					memcpy(descriptorsTmp.ptr<float>(k), descriptors.ptr<float>(iter->second), descriptors.cols*sizeof(float));
+				}
+				else
+				{
+					memcpy(descriptorsTmp.ptr<char>(k), descriptors.ptr<char>(iter->second), descriptors.cols*sizeof(char));
+				}
 			}
 		}
 		ULOGGER_DEBUG("%d keypoints removed, (kept %d), minimum response=%f", removed, keypoints.size(), kptsTmp.size()?kptsTmp.back().response:0.0f);
@@ -842,4 +856,56 @@ cv::Mat GFTT_FREAK::generateDescriptorsImpl(const cv::Mat & image, std::vector<c
 	return descriptors;
 }
 
+//////////////////////////
+//BRISK
+//////////////////////////
+BRISK::BRISK(const ParametersMap & parameters) :
+	thresh_(Parameters::defaultBRISKThresh()),
+	octaves_(Parameters::defaultBRISKOctaves()),
+	patternScale_(Parameters::defaultBRISKPatternScale()),
+	brisk_(0)
+{
+	parseParameters(parameters);
+}
+
+BRISK::~BRISK()
+{
+	if(brisk_)
+	{
+		delete brisk_;
+	}
+}
+
+void BRISK::parseParameters(const ParametersMap & parameters)
+{
+	Parameters::parse(parameters, Parameters::kBRISKThresh(), thresh_);
+	Parameters::parse(parameters, Parameters::kBRISKOctaves(), octaves_);
+	Parameters::parse(parameters, Parameters::kBRISKPatternScale(), patternScale_);
+
+	if(brisk_)
+	{
+		delete brisk_;
+		brisk_ = 0;
+	}
+
+	brisk_ = new cv::BRISK(thresh_, octaves_, patternScale_);
+}
+
+std::vector<cv::KeyPoint> BRISK::generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi) const
+{
+	UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
+	std::vector<cv::KeyPoint> keypoints;
+	cv::Mat imgRoi(image, roi);
+	brisk_->detect(imgRoi, keypoints); // Opencv keypoints
+	return keypoints;
+}
+
+cv::Mat BRISK::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const
+{
+	UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
+	cv::Mat descriptors;
+	brisk_->compute(image, keypoints, descriptors);
+	return descriptors;
+}
+
 }

corelib/src/Memory.cpp

@@ -455,6 +455,10 @@ void Memory::parseParameters(const ParametersMap & parameters)
 			_feature2D = new GFTT_BRIEF(parameters);
 			_featureType = Feature2D::kFeatureGfttBrief;
 			break;
+		case Feature2D::kFeatureBrisk:
+			_feature2D = new BRISK(parameters);
+			_featureType = Feature2D::kFeatureBrisk;
+			break;
 		case Feature2D::kFeatureSurf:
 		default:
 			_feature2D = new SURF(parameters);
@@ -3095,7 +3099,7 @@ private:
 Signature * Memory::createSignature(const SensorData & data, bool keepRawData)
 {
 	UASSERT(data.image().empty() || data.image().type() == CV_8UC1 || data.image().type() == CV_8UC3);
-	UASSERT(data.depth().empty() || data.depth().type() == CV_16UC1);
+	UASSERT(data.depth().empty() || data.depth().type() == CV_16UC1 || data.depth().type() == CV_32FC1);
 	UASSERT(data.depth2d().empty() || data.depth2d().type() == CV_32FC2);
 
 	PreUpdateThread preUpdateThread(_vwd);
@@ -3147,24 +3151,44 @@ Signature * Memory::createSignature(const SensorData & data, bool keepRawData)
 		preUpdateThread.start();
 	}
 
-	// Extract features
+	if(data.keypoints().size() == 0)
-	cv::Mat imageMono;
-	// convert to grayscale
-	if(data.image().channels() > 1)
 	{
-		cv::cvtColor(data.image(), imageMono, cv::COLOR_BGR2GRAY);
+		// Extract features
+		cv::Mat imageMono;
+		// convert to grayscale
+		if(data.image().channels() > 1)
+		{
+			cv::cvtColor(data.image(), imageMono, cv::COLOR_BGR2GRAY);
+		}
+		else
+		{
+			imageMono = data.image();
+		}
+
+		this->extractKeypointsAndDescriptors(imageMono,
+				data.depth(),
+				data.depthFx(), data.depthFy(),
+				data.depthCx(), data.depthCy(),
+				keypoints,
+				descriptors);
+
+		UDEBUG("ratio=%f, meanWordsPerLocation=%d", _badSignRatio, meanWordsPerLocation);
+		if(descriptors.rows && descriptors.rows < _badSignRatio * float(meanWordsPerLocation))
+		{
+			descriptors = cv::Mat();
+		}
 	}
 	else
 	{
-		imageMono = data.image();
+		keypoints = data.keypoints();
-	}
+		descriptors = data.descriptors().clone();
 
-	this->extractKeypointsAndDescriptors(imageMono, data.depth(), data.depthFx(), data.depthFy(), data.depthCx(), data.depthCy(), keypoints, descriptors);
+		filterKeypointsByDepth(keypoints, descriptors,
-
+				data.depth(),
-	UDEBUG("ratio=%f, meanWordsPerLocation=%d", _badSignRatio, meanWordsPerLocation);
+				data.depthFx(), data.depthFy(),
-	if(descriptors.rows && descriptors.rows < _badSignRatio * float(meanWordsPerLocation))
+				data.depthCx(), data.depthCy(),
-	{
+				_wordsMaxDepth);
-		descriptors = cv::Mat();
+		limitKeypoints(keypoints, descriptors, _wordsPerImageTarget);
 	}
 
 	if(_parallelized)
@@ -3215,8 +3239,13 @@ Signature * Memory::createSignature(const SensorData & data, bool keepRawData)
 	{
 		std::vector<unsigned char> imageBytes;
 		std::vector<unsigned char> depthBytes;
+		if(data.depth().type() == CV_32FC1)
+		{
+			UWARN("Keeping raw data in database: depth type is 32FC1, use 16UC1 depth format to avoid a conversion.");
+		}
+		cv::Mat depthMM = data.depth().type() == CV_32FC1?util3d::cvtDepthFromFloat(data.depth()):data.depth();
 		util3d::CompressionThread ctImage(data.image(), std::string(".jpg"));
-		util3d::CompressionThread ctDepth(data.depth(), std::string(".png"));
+		util3d::CompressionThread ctDepth(depthMM, std::string(".png"));
 		ctImage.start();
 		ctDepth.start();
 		ctImage.join();

corelib/src/Odometry.cpp

@@ -154,7 +154,8 @@ OdometryBOW::OdometryBOW(const ParametersMap & parameters) :
 			group.compare("FAST") == 0 ||
 			group.compare("ORB") == 0 ||
 			group.compare("FREAK") == 0 ||
-			group.compare("GFTT") == 0)
+			group.compare("GFTT") == 0 ||
+			group.compare("BRISK") == 0)
 		{
 			customParameters.insert(*iter);
 		}
@@ -256,6 +257,8 @@ Transform OdometryBOW::computeTransform(const SensorData & data, int * quality,
 						0,
 						&uniqueCorrespondences);
 
+				UDEBUG("localMap=%d, new=%d, unique correspondences=%d", (int)localMeansMap.size(), (int)newSignature->getWords3().size(), (int)uniqueCorrespondences.size());
+
 				if((int)inliers1->size() >= this->getMinInliers())
 				{
 					correspondences = inliers1->size();
@@ -266,6 +269,22 @@ Transform OdometryBOW::computeTransform(const SensorData & data, int * quality,
 							this->getInlierDistance(),
 							this->getIterations(),
 							&inliers);
+
+
+					/*
+					//refine ICP test
+					bool hasConverged;
+					double fitness;
+					inliers2 = util3d::transformPointCloud(inliers2, transform);
+					Transform icpT = util3d::icp(inliers1,
+						inliers2,
+						0.02,
+						100,
+						hasConverged,
+						fitness);
+
+					transform = transform * icpT;
+					*/
 
 					if(quality)
 					{
@@ -371,6 +390,7 @@ Transform OdometryBOW::computeTransform(const SensorData & data, int * quality,
 			localMap_.clear();
 			output.setIdentity();
 
+			int count = 0;
 			std::list<int> uniques = uUniqueKeys(newSignature->getWords3());
 			for(std::list<int>::iterator iter = uniques.begin(); iter!=uniques.end(); ++iter)
 			{
@@ -381,8 +401,13 @@ Transform OdometryBOW::computeTransform(const SensorData & data, int * quality,
 					{
 						localMap_.insert(std::make_pair(*iter, std::make_pair(newSignature->id(), pt)));
 					}
+					else
+					{
+						++count;
+					}
 				}
 			}
+			UDEBUG("uniques=%d, pt not finite = %d", (int)uniques.size(),count);
 		}
 
 		_memory->emptyTrash();

corelib/src/VWDictionary.cpp

@@ -378,7 +378,7 @@ void VWDictionary::removeAllWordRef(int wordId, int signatureId)
 std::list<int> VWDictionary::addNewWords(const cv::Mat & descriptors,
 							   int signatureId)
 {
-	UDEBUG("");
+	UDEBUG("id=%d descriptors=%d", signatureId, descriptors.rows);
 	UTimer timer;
 	std::list<int> wordIds;
 	if(descriptors.rows == 0 || descriptors.cols == 0)

corelib/src/util3d.cpp

@@ -1136,11 +1136,6 @@ Transform transformFromXYZCorrespondences(
 	Transform transform;
 	if(cloud1->size() && cloud1->size() == cloud2->size())
 	{
-		// Not robust to outliers...
-		//Eigen::Matrix4f transformMatrix;
-		//pcl::registration::TransformationEstimationSVD<pcl::PointXYZ, pcl::PointXYZ> trans_est;
-		//trans_est.estimateRigidTransformation (*cloud2, *cloud1, transformMatrix);
-
 		// Robust to outliers RANSAC
 		pcl::CorrespondencesPtr correspondences(new pcl::Correspondences);
 		for(unsigned int i = 0; i<cloud1->size(); ++i)
@@ -1159,6 +1154,14 @@ Transform transformFromXYZCorrespondences(
 
 		UDEBUG("RANSAC inliers=%d outliers=%d", (int)correspondencesInliers.size(), (int)correspondences->size()-(int)correspondencesInliers.size());
 		transform = util3d::transformFromEigen4f(crsc.getBestTransformation());
+		
+		/*UDEBUG("RANSAC=%s", transform.prettyPrint().c_str());
+
+		pcl::registration::TransformationEstimationSVD<pcl::PointXYZ, pcl::PointXYZ> trans_est;
+		Eigen::Matrix4f transform_svd;
+		trans_est.estimateRigidTransformation (*cloud2, *cloud1, correspondencesInliers, transform_svd);
+		transform = util3d::transformFromEigen4f(transform_svd);
+		UDEBUG("SVD=%s", transform.prettyPrint().c_str());*/
 
 		if(correspondencesInliers.size() == correspondences->size() && transform.isIdentity())
 		{

guilib/src/PreferencesDialog.cpp

@@ -397,6 +397,11 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->checkBox_GFTT_useHarrisDetector->setObjectName(Parameters::kGFTTUseHarrisDetector().c_str());
 	_ui->doubleSpinBox_GFTT_k->setObjectName(Parameters::kGFTTK().c_str());
 
+	//BRISK
+	_ui->spinBox_BRISK_thresh->setObjectName(Parameters::kBRISKThresh().c_str());
+	_ui->spinBox_BRISK_octaves->setObjectName(Parameters::kBRISKOctaves().c_str());
+	_ui->doubleSpinBox_BRISK_patterScale->setObjectName(Parameters::kBRISKPatternScale().c_str());
+
 	// verifyHypotheses
 	_ui->comboBox_vh_strategy->setObjectName(Parameters::kRtabmapVhStrategy().c_str());
 	_ui->surf_spinBox_matchCountMinAccepted->setObjectName(Parameters::kVhEpMatchCountMin().c_str());
@@ -1994,6 +1999,10 @@ void PreferencesDialog::addParameter(const QObject * object, int value)
 						this->addParameters(_ui->groupBox_detector_gftt2);
 						this->addParameters(_ui->groupBox_detector_brief2);
 					}
+					else if(value == 7) //  brisk
+					{
+						this->addParameters(_ui->groupBox_detector_brisk2);
+					}
 				}
 				else if(comboBox == _ui->globalDetection_icpType)
 				{

guilib/src/ui/preferencesDialog.ui

@@ -86,7 +86,7 @@
                 <enum>QFrame::Raised</enum>
                </property>
                <property name="currentIndex">
-                <number>5</number>
+                <number>17</number>
                </property>
                <widget class="QWidget" name="page_22">
                 <layout class="QVBoxLayout" name="verticalLayout_29">
@@ -3415,6 +3415,11 @@ generate the number of words requested.</string>
                           <string>GFTT+BRIEF</string>
                          </property>
                         </item>
+                        <item>
+                         <property name="text">
+                          <string>BRISK</string>
+                         </property>
+                        </item>
                        </widget>
                       </item>
                      </layout>
@@ -4447,6 +4452,95 @@ When set to false, no new words are added to dictionary, so no more updates are
                  </item>
                 </layout>
                </widget>
+               <widget class="QWidget" name="page_25">
+                <layout class="QVBoxLayout" name="verticalLayout_47">
+                 <item>
+                  <widget class="QGroupBox" name="groupBox_detector_brisk2">
+                   <property name="title">
+                    <string>BRISK</string>
+                   </property>
+                   <layout class="QGridLayout" name="gridLayout_26" columnstretch="0,1">
+                    <item row="0" column="1">
+                     <widget class="QLabel" name="label_191">
+                      <property name="text">
+                       <string>FAST/AGAST detection threshold score.</string>
+                      </property>
+                      <property name="wordWrap">
+                       <bool>true</bool>
+                      </property>
+                     </widget>
+                    </item>
+                    <item row="2" column="0">
+                     <widget class="QDoubleSpinBox" name="doubleSpinBox_BRISK_patterScale">
+                      <property name="maximum">
+                       <double>10.000000000000000</double>
+                      </property>
+                      <property name="singleStep">
+                       <double>0.100000000000000</double>
+                      </property>
+                      <property name="value">
+                       <double>1.000000000000000</double>
+                      </property>
+                     </widget>
+                    </item>
+                    <item row="1" column="1">
+                     <widget class="QLabel" name="label_188">
+                      <property name="text">
+                       <string>Detection octaves. Use 0 to do single scale.</string>
+                      </property>
+                      <property name="wordWrap">
+                       <bool>true</bool>
+                      </property>
+                     </widget>
+                    </item>
+                    <item row="1" column="0">
+                     <widget class="QSpinBox" name="spinBox_BRISK_octaves">
+                      <property name="maximum">
+                       <number>10</number>
+                      </property>
+                      <property name="value">
+                       <number>3</number>
+                      </property>
+                     </widget>
+                    </item>
+                    <item row="2" column="1">
+                     <widget class="QLabel" name="label_189">
+                      <property name="text">
+                       <string>Apply this scale to the pattern used for sampling the neighbourhood of a keypoint.</string>
+                      </property>
+                      <property name="wordWrap">
+                       <bool>true</bool>
+                      </property>
+                     </widget>
+                    </item>
+                    <item row="0" column="0">
+                     <widget class="QSpinBox" name="spinBox_BRISK_thresh">
+                      <property name="maximum">
+                       <number>9999</number>
+                      </property>
+                      <property name="value">
+                       <number>30</number>
+                      </property>
+                     </widget>
+                    </item>
+                   </layout>
+                  </widget>
+                 </item>
+                 <item>
+                  <spacer name="verticalSpacer_26">
+                   <property name="orientation">
+                    <enum>Qt::Vertical</enum>
+                   </property>
+                   <property name="sizeHint" stdset="0">
+                    <size>
+                     <width>20</width>
+                     <height>742</height>
+                    </size>
+                   </property>
+                  </spacer>
+                 </item>
+                </layout>
+               </widget>
                <widget class="QWidget" name="page_19">
                 <layout class="QVBoxLayout" name="verticalLayout_13">
                  <item>
@@ -5297,7 +5391,7 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
                          <item row="1" column="0">
                           <widget class="QComboBox" name="reextract_type">
                            <property name="currentIndex">
-                            <number>4</number>
+                            <number>0</number>
                            </property>
                            <property name="sizeAdjustPolicy">
                             <enum>QComboBox::AdjustToContents</enum>
@@ -5337,6 +5431,11 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
                              <string>GFTT+BRIEF</string>
                             </property>
                            </item>
+                           <item>
+                            <property name="text">
+                             <string>BRISK</string>
+                            </property>
+                           </item>
                           </widget>
                          </item>
                          <item row="1" column="1">
@@ -6077,6 +6176,11 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
                           <string>GFTT+BRIEF</string>
                          </property>
                         </item>
+                        <item>
+                         <property name="text">
+                          <string>BRISK</string>
+                         </property>
+                        </item>
                        </widget>
                       </item>
                       <item row="13" column="1">