working with matcher.

src/examples/test_matcher.cpp

@@ -143,84 +143,89 @@ int main(void)
     sen_ptr->open(0);
 
     cv::startWindowThread();
-    cv::namedWindow(det_ptr->getName(), cv::WINDOW_NORMAL);
-    cv::namedWindow(mat_ptr->getName(), cv::WINDOW_NORMAL);
+    cv::namedWindow("OLD", cv::WINDOW_NORMAL);
+    cv::namedWindow("CURRENT", cv::WINDOW_NORMAL);
+    cv::namedWindow("MATCHES", cv::WINDOW_NORMAL);
 
     // The following line is used to remove the OpenCV "init done" from the terminal
     std::cout << "\e[A" << "         " << std::endl;
 
-    KeyPointVector keypoints_old;
-    KeyPointVector good_keypoints;
-    KeyPointVector good_keypoints_prevf;
+    cv::Mat frame_cur;
+    cv::Mat frame_old;
+    cv::Mat frame_matches;
+
+    KeyPointVector kpts_old;
     cv::Mat descriptors_old;
-    std::vector<cv::DMatch> good_matches;
 
     for (int nframe = 0; nframe < 1000; ++nframe)
     {
         // Get frame
-        cv::Mat frame;
-		sen_ptr->getFrame(frame);
-		cv::Mat frame_matches = frame.clone();
+    	sen_ptr->getFrame(frame_cur);
 
 		// Detector
-        KeyPointVector keypoints = det_ptr->detect(frame);
+        KeyPointVector kpts = det_ptr->detect(frame_cur);
 
         // Descriptor
-        cv::Mat descriptors = des_ptr->getDescriptor(frame,keypoints);
+        cv::Mat descriptors = des_ptr->getDescriptor(frame_cur,kpts);
 
         // Matcher
         if  (nframe > 1)
         {
-        	good_matches.clear();
-        	good_keypoints.clear();
+        	std::vector<cv::DMatch> best_matches;
+            KeyPointVector kpts_matched_curf;
+        	KeyPointVector kpts_matched_prevf;
 
         	// get params
         	MatcherParamsBasePtr mat_params_ptr = mat_ptr->getParams();
 
         	if (mat_params_ptr->match_type == MATCH)
         	{
-            	mat_ptr->match(descriptors,descriptors_old,good_matches);
-
-            	// Get keypoints corresponding to good_matches
-        		for (size_t ii = 0; ii < good_matches.size(); ++ii)
-        		{
-        			cv::Point2f point = keypoints[good_matches[ii].trainIdx].pt;
-        			cv::KeyPoint kpt = cv::KeyPoint(point,1);
-        			good_keypoints.push_back(kpt);
-            	}
+        		// match
+        		std::vector<cv::DMatch> matches;
+            	mat_ptr->match(descriptors_old,descriptors,matches);
+
+            	// filter
+            	mat_ptr->filterByDistance(10, 0.25, kpts_old, kpts, matches, frame_cur.rows, frame_cur.cols, best_matches, kpts_matched_curf, kpts_matched_prevf);
         	}
         	else
         	{
+        		// match
             	std::vector< std::vector<cv::DMatch> > matches;
-            	mat_ptr->match(descriptors,descriptors_old,matches);
+            	mat_ptr->match(descriptors_old,descriptors,matches);
 
-            	mat_ptr->filterByDistance(0.25,keypoints_old, keypoints, matches, frame.rows, frame.cols, good_matches, good_keypoints);
+            	//filter
+            	mat_ptr->filterByDistance(10, 0.25, kpts_old, kpts, matches, frame_cur.rows, frame_cur.cols, best_matches, kpts_matched_curf, kpts_matched_prevf);
         	}
 
-            // Draw matches
-            if  (!good_keypoints_prevf.empty() && !good_keypoints.empty())
+            // Draw
+            frame_old = frame_cur.clone();
+            frame_matches = frame_cur.clone();
+        	if  (!kpts_matched_prevf.empty() && !kpts_matched_curf.empty())
             {
+
             	// draw detections
-                drawKeyPoints(frame, keypoints);
+            	drawKeyPoints(frame_old, kpts_old);
+            	drawKeyPoints(frame_cur, kpts);
 
                 // Draw matches
-            	drawKeyPoints(frame_matches, good_keypoints);
-            	for (unsigned int ii = 0; ii < good_keypoints.size(); ++ii)
-            		cv::line(frame_matches,good_keypoints[ii].pt,good_keypoints[ii].pt,cv::Scalar(0,255,0),3);
+            	drawKeyPoints(frame_cur, kpts_matched_curf, 5, cv::Scalar(0, 255, 0), 2);
+//            	cv::drawMatches(frame_old,kpts_matched_prevf,frame_cur,kpts_matched_curf,best_matches,frame_matches);
+            	for(std::vector<cv::DMatch>::size_type ii=0; ii<best_matches.size(); ii++)
+            			cv::line(frame_matches,kpts_matched_prevf[ii].pt,kpts_matched_curf[ii].pt,cv::Scalar(255,255,0),4);
 
             	std::cout << std::fixed << std::setprecision(4) << "\e[A" << "\e[A" << "Detection time: " << det_ptr->getTime() << "  Description time: " << des_ptr->getTime() << "  Matching time: " << mat_ptr->getTime() << "  TOTAL time: " << det_ptr->getTime() + des_ptr->getTime() + mat_ptr->getTime() << std::endl;
-            	std::cout << "Good matches: " << good_keypoints.size() << std::endl;
+            	std::cout << "Good matches: " << kpts_matched_curf.size() << std::endl;
 
                 // Show frames
-                cv::imshow(det_ptr->getName(), frame);
-            	cv::imshow(mat_ptr->getName(), frame_matches);
+            	cv::imshow("OLD", frame_old);
+            	cv::imshow("CURRENT", frame_cur);
+            	cv::imshow("MATCHES", frame_matches);
                 cv::waitKey(1);
             }
         }
 
         // Update objects
-        copyKPVector(keypoints, keypoints_old);
-        copyKPVector(good_keypoints, good_keypoints_prevf);
+        copyKPVector(kpts, kpts_old);
         descriptors_old = cv::Mat(descriptors.size(),descriptors.type());
         descriptors_old = descriptors.clone();
     }

src/matchers/matcher_base.cpp

@@ -67,11 +67,11 @@ void MatcherBase::match(const cv::Mat& _desc1, const cv::Mat& _desc2, std::vecto
     	std::cerr << "[" << name_ << "]:Wrong match type or output object." << std::endl;
 }
 
-void MatcherBase::filterByDistance(const float& _percentage, const KeyPointVector& _kpts1,const KeyPointVector& _kpts2, const std::vector< std::vector<cv::DMatch> >& _dirty, const int& _img_width, const int& _img_height, std::vector<cv::DMatch>& _filtered_matches, KeyPointVector& _filtered_kpts)
+void MatcherBase::filterByDistance(const int& _max_pixel_dist, const float& _img_size_percentage, const KeyPointVector& _kpts1,const KeyPointVector& _kpts2, const std::vector< std::vector<cv::DMatch> >& _dirty, const int& _img_width, const int& _img_height, std::vector<cv::DMatch>& _filtered_matches, KeyPointVector& _filtered_kpts, KeyPointVector& _filtered_kpts_prev)
 {
 	if (params_base_ptr_->match_type == KNNMATCH || params_base_ptr_->match_type == RADIUSMATCH)
 	{
-		double tresholdDist = _percentage * sqrt(double(_img_height*_img_height + _img_width*_img_width));
+		double tresholdDist = _img_size_percentage * sqrt(double(_img_height*_img_height + _img_width*_img_width));
 
 		std::vector< cv::DMatch > good_matches2;
 		_filtered_matches.reserve(_dirty.size());
@@ -86,18 +86,51 @@ void MatcherBase::filterByDistance(const float& _percentage, const KeyPointVecto
 				double dist = std::sqrt((from.x - to.x) * (from.x - to.x) + (from.y - to.y) * (from.y - to.y));
 
 				//save as best match if local distance is in specified area and on same height
-				if (dist < tresholdDist && std::abs(from.y-to.y)<5)
+				if (dist < tresholdDist && std::abs(from.y-to.y)<_max_pixel_dist  && std::abs(from.x-to.x)<_max_pixel_dist)
 				{
 					_filtered_matches.push_back(_dirty[ii][jj]);
 					cv::KeyPoint kpt = cv::KeyPoint(to,1);
+					cv::KeyPoint kpt_prev = cv::KeyPoint(from,1);
 					_filtered_kpts.push_back(kpt);
+					_filtered_kpts_prev.push_back(kpt_prev);
 					jj = _dirty[ii].size();
 				}
 			}
 		}
 	}
 	else
-    	std::cerr << "[" << name_ << "]: The filterByDistance method can be applied only on KNNMATCH and RADIUSMATCH types." << std::endl;
+    	std::cerr << "[" << name_ << "]: Wrong input type in filterByDistance method." << std::endl;
+}
+
+void MatcherBase::filterByDistance(const int& _max_pixel_dist, const float& _img_size_percentage, const KeyPointVector& _kpts1,const KeyPointVector& _kpts2, const std::vector<cv::DMatch>& _dirty, const int& _img_width, const int& _img_height, std::vector<cv::DMatch>& _filtered_matches, KeyPointVector& _filtered_kpts, KeyPointVector& _filtered_kpts_prev)
+{
+	if (params_base_ptr_->match_type == MATCH)
+	{
+		double tresholdDist = _img_size_percentage * sqrt(double(_img_height*_img_height + _img_width*_img_width));
+
+		std::vector< cv::DMatch > good_matches2;
+		_filtered_matches.reserve(_dirty.size());
+		for (size_t ii = 0; ii < _dirty.size(); ++ii)
+		{
+			cv::Point2f from = _kpts1[_dirty[ii].queryIdx].pt;
+			cv::Point2f to = _kpts2[_dirty[ii].trainIdx].pt;
+
+			//calculate local distance for each possible match
+			double dist = std::sqrt((from.x - to.x) * (from.x - to.x) + (from.y - to.y) * (from.y - to.y));
+
+			//save as best match if local distance is in specified area
+			if (dist < tresholdDist && std::abs(from.y-to.y)<_max_pixel_dist && std::abs(from.x-to.x)<_max_pixel_dist)
+			{
+				_filtered_matches.push_back(_dirty[ii]);
+				cv::KeyPoint kpt = cv::KeyPoint(to,1);
+				cv::KeyPoint kpt_prev = cv::KeyPoint(from,1);
+				_filtered_kpts.push_back(kpt);
+				_filtered_kpts_prev.push_back(kpt_prev);
+			}
+		}
+	}
+	else
+    	std::cerr << "[" << name_ << "]: Wrong input type in filterByDistance method." << std::endl;
 }
 
 } /* namespace vision_utils */

src/matchers/matcher_base.h

@@ -63,7 +63,8 @@ class MatcherBase : public VUBase, public std::enable_shared_from_this<MatcherBa
 
         MatcherParamsBasePtr getParams(void);
 
-        void filterByDistance(const float& _percentage, const KeyPointVector& _kpts1,const KeyPointVector& _kpts2, const std::vector< std::vector<cv::DMatch> >& _dirty, const int& _img_width, const int& _img_height, std::vector<cv::DMatch>& _filtered_matches, KeyPointVector& _filtered_kpts);
+        void filterByDistance(const int& _max_pixel_dist, const float& _percentage, const KeyPointVector& _kpts1,const KeyPointVector& _kpts2, const std::vector<cv::DMatch>& _dirty, const int& _img_width, const int& _img_height, std::vector<cv::DMatch>& _filtered_matches, KeyPointVector& _filtered_kpts, KeyPointVector& _filtered_kpts_prev);
+        void filterByDistance(const int& _max_pixel_dist, const float& _percentage, const KeyPointVector& _kpts1,const KeyPointVector& _kpts2, const std::vector< std::vector<cv::DMatch> >& _dirty, const int& _img_width, const int& _img_height, std::vector<cv::DMatch>& _filtered_matches, KeyPointVector& _filtered_kpts, KeyPointVector& _filtered_kpts_prev);
 
         // Factory method
         static MatcherBasePtr create(const std::string& _unique_name, const ParamsBasePtr _params);

src/vision_utils.h

@@ -150,10 +150,10 @@ inline std::vector<cv::Point2f> vecUnion(std::vector<cv::Point2f> v1, std::vecto
   return v3;
 }
 
-inline void drawKeyPoints(cv::Mat& _image, const KeyPointVector& _kp_vec)
+inline void drawKeyPoints(cv::Mat& _image, const KeyPointVector& _kp_vec, const int& _radius=5, const cv::Scalar& _color=cv::Scalar(0, 0, 255), const int& _thickness=-1)
 {
     for (unsigned int ii = 0; ii < _kp_vec.size(); ++ii)
-        cv::circle(_image, _kp_vec[ii].pt, 5, cv::Scalar(128, 128, 255), -1);
+        cv::circle(_image, _kp_vec[ii].pt, _radius, _color, _thickness);
 }
 
 inline void drawKeyLines(cv::Mat& _image, const KeyLineVector& _kl_vec)
@@ -165,8 +165,8 @@ inline void drawKeyLines(cv::Mat& _image, const KeyLineVector& _kl_vec)
 inline void copyKPVector(const KeyPointVector& kpv_from, KeyPointVector& kpv_to)
 {
 	kpv_to.clear();
-	kpv_to.resize(kpv_from.size());
-	std::copy(kpv_from.begin(), kpv_from.end(), std::back_inserter(kpv_to));
+	for (int ii=0; ii<kpv_from.size(); ii++)
+		kpv_to.push_back(kpv_from[ii]);
 }
 
 } /* namespace vision_utils */