fixed issue 18 : Mutiple detection of the same object

git-svn-id: http://find-object.googlecode.com/svn/trunk/find_object@205 620bd6b2-0a58-f614-fd9a-1bd335dccda9

src/MainWindow.cpp

@@ -992,7 +992,7 @@ protected:
 		{
 			h_ = findHomography(mpts_1,
 					mpts_2,
-					cv::RANSAC,
+					Settings::getHomographyMethod(),
 					Settings::getHomography_ransacReprojThr(),
 					outlierMask_);
 
@@ -1007,6 +1007,14 @@ protected:
 					++outliers_;
 				}
 			}
+			if(Settings::getHomography_ignoreWhenAllInliers())
+			{
+				// ignore homography when all features are inliers
+				if(inliers_ == (int)outlierMask_.size())
+				{
+					h_ = cv::Mat();
+				}
+			}
 		}
 
 		//printf("Homography Object %d time=%d ms\n", objectIndex_, time.elapsed());
@@ -1091,6 +1099,7 @@ void MainWindow::update(const cv::Mat & image)
 		   objectsDescriptors_[0].type() == descriptors.type()) // binary descriptor issue, if the dataTree is not yet updated with modified settings
 		{
 			QVector<QMultiMap<int, int> > matches(objects_.size()); // Map< ObjectDescriptorIndex, SceneDescriptorIndex >
+			QVector<int> matchesId(objects_.size(), -1);
 			float minMatchedDistance = -1.0f;
 			float maxMatchedDistance = -1.0f;
 
@@ -1223,7 +1232,7 @@ void MainWindow::update(const cv::Mat & image)
 			QMap<int, float> scores;
 			int maxScoreId = -1;
 			int maxScore = 0;
-			QMap<int, QPair<QRect, QTransform> > objectsDetected;
+			QMultiMap<int, QPair<QRect, QTransform> > objectsDetected;
 
 			if(Settings::getHomography_homographyComputed())
 			{
@@ -1239,7 +1248,8 @@ void MainWindow::update(const cv::Mat & image)
 
 					for(int k=i; k<i+threadCounts && k<matches.size(); ++k)
 					{
-						threads.push_back(new HomographyThread(&matches[k], k, &objects_.at(k)->keypoints(), &keypoints));
+						int objectId = matchesId[k] >=0 ? matchesId[k]:k; // the first matches ids correspond object index
+						threads.push_back(new HomographyThread(&matches[k], objectId, &objects_.at(objectId)->keypoints(), &keypoints));
 						threads.back()->start();
 					}
 
@@ -1256,6 +1266,43 @@ void MainWindow::update(const cv::Mat & image)
 						if(!threads[j]->getHomography().empty())
 						{
 							if(threads[j]->getInliers() >= Settings::getHomography_minimumInliers())
+							{
+								const cv::Mat & H = threads[j]->getHomography();
+								QTransform hTransform(
+									H.at<double>(0,0), H.at<double>(1,0), H.at<double>(2,0),
+									H.at<double>(0,1), H.at<double>(1,1), H.at<double>(2,1),
+									H.at<double>(0,2), H.at<double>(1,2), H.at<double>(2,2));
+
+								int distance = Settings::getGeneral_multiDetectionRadius(); // in pixels
+								if(Settings::getGeneral_multiDetection())
+								{
+									// Remove inliers and recompute homography
+									QMultiMap<int, int> newMatches;
+									for(unsigned int k=0; k<threads[j]->getOutlierMask().size();++k)
+									{
+										if(!threads[j]->getOutlierMask().at(k))
+										{
+											newMatches.insert(threads[j]->getIndexesA().at(k), threads[j]->getIndexesB().at(k));
+										}
+									}
+									matches.push_back(newMatches);
+									matchesId.push_back(index);
+
+									// compute distance from previous added same objects...
+									QMultiMap<int, QPair<QRect, QTransform> >::iterator objIter = objectsDetected.find(objects_.at(index)->id());
+									for(;objIter!=objectsDetected.end() && objIter.key() == objects_.at(index)->id(); ++objIter)
+									{
+										qreal dx = objIter.value().second.m31() - hTransform.m31();
+										qreal dy = objIter.value().second.m32() - hTransform.m32();
+										int d = (int)sqrt(dx*dx + dy*dy);
+										if(d < distance)
+										{
+											distance = d;
+										}
+									}
+								}
+
+								if(distance >= Settings::getGeneral_multiDetectionRadius())
 								{
 									if(this->isVisible())
 									{
@@ -1266,20 +1313,13 @@ void MainWindow::update(const cv::Mat & image)
 												objects_.at(index)->setKptColor(threads[j]->getIndexesA().at(k), color);
 												ui_->imageView_source->setKptColor(threads[j]->getIndexesB().at(k), color);
 											}
-										else
+											else if(!objectsDetected.contains(objects_.at(index)->id()))
 											{
 												objects_.at(index)->setKptColor(threads[j]->getIndexesA().at(k), QColor(0,0,0));
 											}
 										}
 									}
 
-								const cv::Mat & H = threads[j]->getHomography();
-								QTransform hTransform(
-									H.at<double>(0,0), H.at<double>(1,0), H.at<double>(2,0),
-									H.at<double>(0,1), H.at<double>(1,1), H.at<double>(2,1),
-									H.at<double>(0,2), H.at<double>(1,2), H.at<double>(2,2));
-
-								// find center of object
 									QRect rect = objects_.at(index)->pixmap().rect();
 									objectsDetected.insert(objects_.at(index)->id(), QPair<QRect, QTransform>(rect, hTransform));
 									// Example getting the center of the object in the scene using the homography
@@ -1288,8 +1328,16 @@ void MainWindow::update(const cv::Mat & image)
 
 									// add rectangle
 									if(this->isVisible())
+									{
+										if(objectsDetected.count(objects_.at(index)->id()) > 1)
+										{
+											// if a homography is already found, set the objects count
+											label->setText(QString("%1 objects found").arg(objectsDetected.count(objects_.at(index)->id())));
+										}
+										else
 										{
 											label->setText(QString("%1 in %2 out").arg(threads[j]->getInliers()).arg(threads[j]->getOutliers()));
+										}
 										QPen rectPen(color);
 										rectPen.setWidth(4);
 										QGraphicsRectItem * rectItem = new QGraphicsRectItem(rect);
@@ -1298,11 +1346,18 @@ void MainWindow::update(const cv::Mat & image)
 										ui_->imageView_source->addRect(rectItem);
 									}
 								}
-							else
+							}
+							else if(this->isVisible() && objectsDetected.count(objects_.at(index)->id()) == 0)
 							{
 								label->setText(QString("Too low inliers (%1 in %2 out)").arg(threads[j]->getInliers()).arg(threads[j]->getOutliers()));
 							}
 						}
+						else if(this->isVisible() && objectsDetected.count(objects_.at(index)->id()) == 0)
+						{
+							if(threads[j]->getInliers() >= Settings::getHomography_minimumInliers())
+							{
+								label->setText(QString("Ignored, all inliers (%1 in %2 out)").arg(matches[index].size()).arg(threads[j]->getOutliers()));
+							}
 							else
 							{
 								label->setText(QString("Too low matches (%1)").arg(matches[index].size()));
@@ -1310,6 +1365,7 @@ void MainWindow::update(const cv::Mat & image)
 						}
 					}
 				}
+			}
 			else
 			{
 				for(int i=0; i<matches.size(); ++i)
@@ -1330,10 +1386,14 @@ void MainWindow::update(const cv::Mat & image)
 			//scores
 			for(int i=0; i<matches.size(); ++i)
 			{
-				scores.insert(objects_.at(i)->id(), matches[i].size());
+				int objectIndex = matchesId.at(i) >= 0? matchesId.at(i): i;
+				if(!scores.contains(objects_.at(objectIndex)->id()))
+				{
+					scores.insert(objects_.at(objectIndex)->id(), matches[i].size());
+				}
 				if(maxScoreId == -1 || maxScore < matches[i].size())
 				{
-					maxScoreId = objects_.at(i)->id();
+					maxScoreId = objects_.at(objectIndex)->id();
 					maxScore = matches[i].size();
 				}
 			}

src/MainWindow.h

@@ -74,7 +74,7 @@ private slots:
 	void moveCameraFrame(int frame);
 
 signals:
-	void objectsFound(const QMap<int, QPair<QRect, QTransform> > &);
+	void objectsFound(const QMultiMap<int, QPair<QRect, QTransform> > &);
 
 private:
 	void addObjectFromFile(const QString & filePath);

src/Settings.cpp

@@ -9,6 +9,7 @@
 #include <QtCore/QDir>
 #include <stdio.h>
 #include <opencv2/nonfree/features2d.hpp>
+#include <opencv2/calib3d/calib3d.hpp>
 
 #define VERBOSE 0
 
@@ -519,3 +520,32 @@ cv::flann::SearchParams Settings::getFlannSearchParams()
 			getNearestNeighbor_9search_sorted());
 }
 
+int Settings::getHomographyMethod()
+{
+	int method = cv::RANSAC;
+	QString str = getHomography_method();
+	QStringList split = str.split(':');
+	if(split.size()==2)
+	{
+		bool ok = false;
+		int index = split.first().toInt(&ok);
+		if(ok)
+		{
+			QStringList strategies = split.last().split(';');
+			if(strategies.size() == 2 && index>=0 && index<2)
+			{
+				switch(method)
+				{
+				case 0:
+					method = cv::LMEDS;
+					break;
+				default:
+					method = cv::RANSAC;
+					break;
+				}
+			}
+		}
+	}
+	if(VERBOSE)printf("Settings::getHomographyMethod() method=%d\n", method);
+	return method;
+}

src/Settings.h

@@ -173,10 +173,14 @@ class Settings
 	PARAMETER(General, invertedSearch, bool, false, "In contrast to classic Find-Object usage (where we match descriptors from the objects to those in a vocabulary created with descriptors extracted from the scene), we create a vocabulary from all the objects' descriptors and we match scene's descriptors to this vocabulary. It is the inverted search mode.");
 	PARAMETER(General, controlsShown, bool, false, "Show play/image seek controls (useful with video file and directory of images modes).");
 	PARAMETER(General, threads, int, 1, "Number of threads used for objects matching and homography computation. 0 means as many threads as objects. On InvertedSearch mode, multi-threading has only effect on homography computation.");
+	PARAMETER(General, multiDetection, bool, false, "Multiple detection of the same object.");
+	PARAMETER(General, multiDetectionRadius, int, 30, "Ignore detection of the same object in X pixels radius of the previous detections.");
 
 	PARAMETER(Homography, homographyComputed, bool, true, "Compute homography? On ROS, this is required to publish objects detected.");
+	PARAMETER(Homography, method, QString, "1:LMEDS;RANSAC", "Type of the robust estimation algorithm: least-median algorithm or RANSAC algorithm.");
 	PARAMETER(Homography, ransacReprojThr, double, 1.0, "Maximum allowed reprojection error to treat a point pair as an inlier (used in the RANSAC method only). It usually makes sense to set this parameter somewhere in the range of 1 to 10.");
 	PARAMETER(Homography, minimumInliers, int, 10, "Minimum inliers to accept the homography.");
+	PARAMETER(Homography, ignoreWhenAllInliers, bool, false, "Ignore homography when all features are inliers (sometimes when the homography doesn't converge, it returns the best homography with all features as inliers).");
 
 public:
 	virtual ~Settings(){}
@@ -207,6 +211,8 @@ public:
 	static cvflann::flann_distance_t getFlannDistanceType();
 	static cv::flann::SearchParams getFlannSearchParams();
 
+	static int getHomographyMethod();
+
 private:
 	Settings(){}