adding tests to vision_utils

src/algorithms/activesearch/alg_activesearch.cpp

@@ -169,7 +169,7 @@ void AlgorithmACTIVESEARCH::detectNewFeatures(FramePtr& _frame, const DetectorBa
 	       		retainBest(kps,1);
 
 	       		// Check if point exist in list
-	       		int index = existsIn(kps[0], list_keypoints, 1.0);
+	       		int index = existsInKPVec(kps[0], list_keypoints, 1.0);
 
 	       		if(kps[0].response > params_ptr_->min_response_new_feature)
 	            {

src/algorithms/opticalflowpyrlk/alg_opticalflowpyrlk.cpp

@@ -69,7 +69,7 @@ void AlgorithmOPTFLOWPYRLK::detectNewFeatures(FramePtr& _frame, const DetectorBa
 		// Keep only new features
 		KeyPointVector tracked_kps = _frame->getKeyPoints();
 		std::vector<int> kps_idxs;
-		KeyPointVector kps_new = vecDifference(tracked_kps, kps, kps_idxs);
+		KeyPointVector kps_new = differenceKPVec(tracked_kps, kps, kps_idxs);
 		desc_new = cv::Mat(kps_new.size(), 1, CV_32F); // Not used in this context.
 	}
 	else

src/algorithms/trackfeatures/alg_trackfeatures.cpp

@@ -61,7 +61,7 @@ void AlgorithmTRACKFEATURES::detectNewFeatures(FramePtr& _frame, const DetectorB
 		// Keep only new features
 		std::vector<int> idxs_new;
 		KeyPointVector tracked_kps = _frame->getKeyPoints();
-		kps_new = vecDifference(tracked_kps, kps, idxs_new);
+		kps_new = differenceKPVec(tracked_kps, kps, idxs_new);
 		for (int ii=0;ii<idxs_new.size();++ii)
 			desc_new.push_back( desc.row(idxs_new[ii]) );
 	}

src/test/CMakeLists.txt

@@ -20,6 +20,10 @@ target_link_libraries(gtest_example ${PROJECT_NAME})      #
 
 # ------- First Core classes ----------
 
+# Vision Utils test
+  vu_add_gtest(gtest_vision_utils gtest_vision_utils.cpp)
+  target_link_libraries(gtest_vision_utils ${PROJECT_NAME})
+  
 # ------- Now Derived classes ----------
 
 # ROI test

src/test/gtest_roi_ORB.cpp

@@ -106,7 +106,7 @@ TEST(RoiORB, RoiBounds)
                 keypoint_filter.retainBest(target_keypoints,1);
                 cv::Point2f pnt = target_keypoints[0].pt;
 
-                int j = vision_utils::existsIn(pnt, points_to_check, 2.0);
+                int j = vision_utils::existsInPVec(pnt, points_to_check, 2.0);
 
                 ASSERT_GE(j, 0);
 

src/test/gtest_vision_utils.cpp

 #include "utils_gtest.h"
 #include "../vision_utils.h"
 
-TEST(VisionUtils, DummyTestExample)
-{
-  EXPECT_FALSE(false);
+cv::Point2f p1(1.0, 1.0);
+cv::Point2f p2(2.0, 2.0);
+cv::Point2f p3(3.0, 3.0);
+cv::Point2f p4(4.0, 4.0);
 
-  ASSERT_TRUE(true);
+cv::KeyPoint kp1(1.0, 1.0, CV_32F);
+cv::KeyPoint kp2(2.0, 2.0, CV_32F);
+cv::KeyPoint kp3(3.0, 3.0, CV_32F);
+cv::KeyPoint kp4(4.0, 4.0, CV_32F);
 
-  int my_int = 5;
+TEST(VisionUtils, LessPoints) {
+	ASSERT_FALSE(vision_utils::LessPoints(p1, p1)); // p1 !< p1
+	ASSERT_FALSE(vision_utils::LessPoints(p2, p1)); // p2 !< p1
+	ASSERT_TRUE(vision_utils::LessPoints(p1, p2));  // p1 < p2
+}
 
-  ASSERT_EQ(my_int, 5);
+TEST(VisionUtils, LessKPoints) {
+	ASSERT_FALSE(vision_utils::LessKPoints(kp1, kp1)); // p1 !< p2
+	ASSERT_FALSE(vision_utils::LessKPoints(kp2, kp1)); // p3 !< p1
+	ASSERT_TRUE(vision_utils::LessKPoints(kp1, kp2));  // p1 < p3
+}
 
-  PRINTF("All good at TestTest::DummyTestExample !\n");
+TEST(VisionUtils, existsInPVec) {
+	PointVector v1;
+	v1.push_back(p1);
+	v1.push_back(p2);
+	ASSERT_EQ(vision_utils::existsInPVec(p1, v1, 1.0), 0);  // p1 exists in v1[0]
+	ASSERT_EQ(vision_utils::existsInPVec(p2, v1, 1.0), 1);  // p2 exists in v2[1]
+	ASSERT_EQ(vision_utils::existsInPVec(p3, v1, 1.0), -1); // p3 does not exist in v1 (-1)
 }
 
-TEST(VisionUtils, DummyTestExample)
-{
-  EXPECT_FALSE(false);
+TEST(VisionUtils, existsInKPVec) {
+	KeyPointVector v1;
+	v1.push_back(kp1);
+	v1.push_back(kp2);
+	ASSERT_EQ(vision_utils::existsInKPVec(kp1, v1, 1.0), 0);  // p1 exists in v1[0]
+	ASSERT_EQ(vision_utils::existsInKPVec(kp2, v1, 1.0), 1);  // p2 exists in v2[1]
+	ASSERT_EQ(vision_utils::existsInKPVec(kp3, v1, 1.0), -1); // p3 does not exist in v1 (-1)
+}
 
-  ASSERT_TRUE(true);
+TEST(VisionUtils, differencePVec) {
+	PointVector v1;
+	v1.push_back(p1);
+	v1.push_back(p3);
+	PointVector v2;
+	v2.push_back(p1);
+	v2.push_back(p2);
+	v2.push_back(p3);
+	v2.push_back(p4);
+	std::vector<int> idx_diff_in_v2;
+	PointVector res = vision_utils::differencePVec(v1, v2, idx_diff_in_v2);
+	ASSERT_EQ(res.size(), 2);
+	ASSERT_EQ(idx_diff_in_v2.size(), 2);
+	ASSERT_EQ(idx_diff_in_v2[0], 1);
+	ASSERT_EQ(idx_diff_in_v2[1], 3);
+	ASSERT_EQ(v2[idx_diff_in_v2[0]], p2);
+	ASSERT_EQ(v2[idx_diff_in_v2[1]], p4);
+}
 
-  int my_int = 5;
+TEST(VisionUtils, differenceKPVec) {
+	KeyPointVector v1;
+	v1.push_back(kp1);
+	v1.push_back(kp3);
+	KeyPointVector v2;
+	v2.push_back(kp1);
+	v2.push_back(kp2);
+	v2.push_back(kp3);
+	v2.push_back(kp4);
+	std::vector<int> idx_diff_in_v2;
+	KeyPointVector res = vision_utils::differenceKPVec(v1, v2, idx_diff_in_v2);
+	ASSERT_EQ(res.size(), 2);
+	ASSERT_EQ(idx_diff_in_v2.size(), 2);
+	ASSERT_EQ(idx_diff_in_v2[0], 1);
+	ASSERT_EQ(idx_diff_in_v2[1], 3);
+	ASSERT_EQ(v2[idx_diff_in_v2[0]].pt, kp2.pt);
+	ASSERT_EQ(v2[idx_diff_in_v2[1]].pt, kp4.pt);
+}
 
-  ASSERT_EQ(my_int, 5);
+TEST(VisionUtils, intersecPVec){
+	PointVector v1;
+	v1.push_back(p1);
+	v1.push_back(p3);
+	PointVector v2;
+	v2.push_back(p1);
+	v2.push_back(p2);
+	v2.push_back(p3);
+	v2.push_back(p4);
+	PointVector res = vision_utils::intersecPVec(v1, v2);
+	ASSERT_EQ(res.size(), 2);
+	ASSERT_EQ(res[0], p1);
+	ASSERT_EQ(res[1], p3);
+}
 
-  PRINTF("All good at TestTest::DummyTestExample !\n");
+TEST(VisionUtils, intersecKPVec){
+	KeyPointVector v1;
+	v1.push_back(kp1);
+	v1.push_back(kp3);
+	KeyPointVector v2;
+	v2.push_back(kp1);
+	v2.push_back(kp2);
+	v2.push_back(kp3);
+	v2.push_back(kp4);
+	KeyPointVector res = vision_utils::intersecKPVec(v1, v2);
+	ASSERT_EQ(res.size(), 2);
+	ASSERT_EQ(res[0].pt, kp1.pt);
+	ASSERT_EQ(res[1].pt, kp3.pt);
 }
 
-int main(int argc, char **argv)
-{
-  testing::InitGoogleTest(&argc, argv);
-  return RUN_ALL_TESTS();
+int main(int argc, char **argv) {
+	testing::InitGoogleTest(&argc, argv);
+	return RUN_ALL_TESTS();
 }

src/vision_utils.cpp

@@ -18,41 +18,67 @@ bool LessKPoints(const cv::KeyPoint& lhs, const cv::KeyPoint& rhs)
     return (lhs.pt.x < rhs.pt.x) || ((lhs.pt.x == rhs.pt.x) && (lhs.pt.y < rhs.pt.y));
 };
 
-PointVector vecDifference(PointVector& v1, PointVector& v2, std::vector<int>& idx_diff_in_v2)
+int existsInPVec(const cv::Point2f& p, const PointVector p_vec, const double& pixel_tol)
 {
+	double pixel_tol_squared = pixel_tol*pixel_tol;
+    for (int ii = 0; ii < p_vec.size(); ++ii)
+    {
+    	double dx = std::abs(p.x - p_vec[ii].x);
+    	double dy = std::abs(p.y - p_vec[ii].y);
+        if ( dx*dx + dy*dy < pixel_tol_squared) // match based on Euclidean distance
+            return ii;
+    }
+    return -1; // -1 marks 'not found'
+}
+
+int existsInKPVec(const cv::KeyPoint& p, const KeyPointVector p_vec, const double& pixel_tol)
+{
+	double pixel_tol_squared = pixel_tol*pixel_tol;
+    for (int ii = 0; ii < p_vec.size(); ++ii)
+    {
+    	double dx = std::abs(p.pt.x - p_vec[ii].pt.x);
+    	double dy = std::abs(p.pt.y - p_vec[ii].pt.y);
+        if ( dx*dx + dy*dy < pixel_tol_squared) // match based on Euclidean distance
+            return ii;
+    }
+    return -1; // -1 marks 'not found'
+}
+
+PointVector differencePVec(PointVector& v1, PointVector& v2, std::vector<int>& idx_diff_in_v2)
+{
+	assert(!v1.empty() && "The first vector is empty.");
+	assert(!v2.empty() && "The second vector is empty.");
 	PointVector v3;
-  for (int ii=0; ii<v2.size();++ii)
-  {
-	  bool found = false;
-	  for (auto kp1 : v1)
-		  if (LessPoints(v2[ii],kp1) == 1) found = true;
-	  if (!found)
+	for (int ii=0; ii<v2.size();++ii)
+	{
+	  int idx = existsInPVec(v2[ii], v1, 1);
+	  if (idx<0)
 	  {
 		  idx_diff_in_v2.push_back(ii);
 		  v3.push_back(v2[ii]);
 	  }
-  }
-  return v3;
+	}
+	return v3;
 };
 
-KeyPointVector vecDifference(KeyPointVector& v1, KeyPointVector& v2, std::vector<int>& idx_diff_in_v2)
+KeyPointVector differenceKPVec(KeyPointVector& v1, KeyPointVector& v2, std::vector<int>& idx_diff_in_v2)
 {
-	KeyPointVector v3;
-  for (int ii=0; ii<v2.size();++ii)
-  {
-	  bool found = false;
-	  for (auto kp1 : v1)
-		  if (LessKPoints(v2[ii],kp1) == 1) found = true;
-	  if (!found)
+	assert(!v1.empty() && "The first vector is empty.");
+  	  assert(!v2.empty() && "The second vector is empty.");
+  	  KeyPointVector v3;
+	for (int ii=0; ii<v2.size();++ii)
+	{
+	  int idx = existsInKPVec(v2[ii], v1, 1);
+	  if (idx<0)
 	  {
 		  idx_diff_in_v2.push_back(ii);
 		  v3.push_back(v2[ii]);
 	  }
-  }
-  return v3;
+	}
+	return v3;
 };
 
-PointVector vecIntersec(PointVector& v1, PointVector& v2)
+PointVector intersecPVec(PointVector& v1, PointVector& v2)
 {
   PointVector v3;
   std::sort(v1.begin(), v1.end(), LessPoints);
@@ -61,7 +87,7 @@ PointVector vecIntersec(PointVector& v1, PointVector& v2)
   return v3;
 };
 
-KeyPointVector vecIntersec(KeyPointVector& v1, KeyPointVector& v2)
+KeyPointVector intersecKPVec(KeyPointVector& v1, KeyPointVector& v2)
 {
   KeyPointVector v3;
   std::sort(v1.begin(), v1.end(), LessKPoints);
@@ -70,7 +96,7 @@ KeyPointVector vecIntersec(KeyPointVector& v1, KeyPointVector& v2)
   return v3;
 };
 
-PointVector vecUnion(PointVector& v1, PointVector& v2)
+PointVector unionPVec(PointVector& v1, PointVector& v2)
 {
   PointVector v3;
   std::sort(v1.begin(), v1.end(), LessPoints);
@@ -79,7 +105,7 @@ PointVector vecUnion(PointVector& v1, PointVector& v2)
   return v3;
 };
 
-KeyPointVector vecUnion(KeyPointVector& v1, KeyPointVector& v2)
+KeyPointVector unionKPVec(KeyPointVector& v1, KeyPointVector& v2)
 {
   KeyPointVector v3;
   std::sort(v1.begin(), v1.end(), LessKPoints);
@@ -151,32 +177,6 @@ void whoHasMoved(const PointVector& _pt_vec_1, const PointVector& _pt_vec_2, Poi
     }
 }
 
-int existsIn(const cv::Point2f& p, const PointVector p_vec, const double& pixel_tol)
-{
-	double pixel_tol_squared = pixel_tol*pixel_tol;
-    for (int ii = 0; ii < p_vec.size(); ii++)
-    {
-    	double dx = p.x - p_vec[ii].x;
-    	double dy = p.y - p_vec[ii].y;
-        if ( dx*dx + dy*dy < pixel_tol_squared) // match based on Euclidean distance
-            return ii;
-    }
-    return -1; // -1 marks 'not found'
-}
-
-int existsIn(const cv::KeyPoint& p, const KeyPointVector p_vec, const double& pixel_tol)
-{
-	double pixel_tol_squared = pixel_tol*pixel_tol;
-    for (int ii = 0; ii < p_vec.size(); ii++)
-    {
-    	double dx = p.pt.x - p_vec[ii].pt.x;
-    	double dy = p.pt.y - p_vec[ii].pt.y;
-        if ( dx*dx + dy*dy < pixel_tol_squared) // match based on Euclidean distance
-            return ii;
-    }
-    return -1; // -1 marks 'not found'
-}
-
 void drawKeyPoints(cv::Mat& _image, const KeyPointVector& _kp_vec, const int& _radius, const cv::Scalar& _color, const int& _thickness)
 {
     for (auto kp : _kp_vec)

src/vision_utils.h

@@ -29,16 +29,6 @@
 #include <opencv2/line_descriptor.hpp>
 #include <opencv2/xfeatures2d.hpp>
 
-// REMOVE
-//#include <opencv2/core.hpp>
-//#include <opencv2/core/types.hpp>
-//#include <opencv2/imgproc/imgproc.hpp>
-//#include <opencv2/highgui/highgui.hpp>
-//#include <opencv2/features2d/features2d.hpp>
-//#include <opencv2/features2d.hpp>
-//#include <opencv2/xfeatures2d/nonfree.hpp>
-//#include <opencv2/calib3d.hpp>
-
 typedef std::vector<cv::Point2f> PointVector;
 typedef std::vector<cv::KeyPoint> KeyPointVector;
 typedef std::vector<cv::line_descriptor::KeyLine> KeyLineVector;
@@ -140,14 +130,17 @@ T readFromUser(const T& _default)
 bool LessPoints(const cv::Point2f& lhs, const cv::Point2f& rhs);
 bool LessKPoints(const cv::KeyPoint& lhs, const cv::KeyPoint& rhs);
 
-PointVector vecDifference(PointVector& v1, PointVector& v2, std::vector<int>& idx_diff_in_v2);
-KeyPointVector vecDifference(KeyPointVector& v1, KeyPointVector& v2, std::vector<int>& idx_diff_in_v2);
+int existsInPVec(const cv::Point2f& p, const PointVector p_vec, const double& pixel_tol);
+int existsInKPVec(const cv::KeyPoint& p, const KeyPointVector p_vec, const double& pixel_tol);
+
+PointVector differencePVec(PointVector& v1, PointVector& v2, std::vector<int>& idx_diff_in_v2);
+KeyPointVector differenceKPVec(KeyPointVector& v1, KeyPointVector& v2, std::vector<int>& idx_diff_in_v2);
 
-PointVector vecIntersec(PointVector& v1, PointVector& v2);
-KeyPointVector vecIntersec(KeyPointVector& v1, KeyPointVector& v2);
+PointVector intersecPVec(PointVector& v1, PointVector& v2);
+KeyPointVector intersecKPVec(KeyPointVector& v1, KeyPointVector& v2);
 
-PointVector vecUnion(PointVector& v1, PointVector& v2);
-KeyPointVector vecUnion(KeyPointVector& v1, KeyPointVector& v2);
+PointVector unionPVec(PointVector& v1, PointVector& v2);
+KeyPointVector unionKPVec(KeyPointVector& v1, KeyPointVector& v2);
 
 void copyKPVector(const KeyPointVector& _kpv_from, KeyPointVector& _kpv_to);
 
@@ -156,9 +149,6 @@ PointVector KPToP(const KeyPointVector& _kp_vec);
 void whoHasMoved(const KeyPointVector& _kpts1, const KeyPointVector& _kpts2, KeyPointVector& _common_kpts, KeyPointVector& _new_in_kpts2);
 void whoHasMoved(const PointVector& _pts1, const PointVector& _pts2, PointVector& _common_pts, PointVector& _new_in_pts2);
 
-int existsIn(const cv::Point2f& p, const PointVector p_vec, const double& pixel_tol);
-int existsIn(const cv::KeyPoint& p, const KeyPointVector p_vec, const double& pixel_tol);
-
 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);
 
 void drawKeyLines(cv::Mat& _image, const KeyLineVector& _kl_vec, const cv::Scalar& _color = cv::Scalar(128, 128, 255) );