WIP gtest trifocal tensor

src/common_class/trifocaltensor.cpp

@@ -108,7 +108,7 @@ void TrifocalTensor::computeMatrixB(const Eigen::Vector3d& p1, const Eigen::Vect
 	}
 }
 
-void TrifocalTensor::transfert(const Eigen::Vector3d& p1 , Eigen::Vector3d& p2, Eigen::Vector3d& p3) {
+void TrifocalTensor::transfer(const Eigen::Vector3d& p1 , Eigen::Vector3d& p2, Eigen::Vector3d& p3) {
 
 	Eigen::MatrixXd B;
 	Eigen::VectorXd b;

src/common_class/trifocaltensor.h

@@ -34,7 +34,7 @@ public:
 	/**
 	 * \brief Compute the coordinates of the point p3 on third img corresponding to p1 and p2 of others images
 	 */
-	void transfert(const Eigen::Vector3d& p1 , Eigen::Vector3d& p2, Eigen::Vector3d& p3);
+	void transfer(const Eigen::Vector3d& p1 , Eigen::Vector3d& p2, Eigen::Vector3d& p3);
 
 	/**
 	 * \biref Display the tensor, just for debug

src/test/gtest_trifocaltensor.cpp

@@ -141,28 +141,72 @@ TEST(TrifocalTensor, DetectorDescriptorMatcher)
     ASSERT_EQ(matches12.size(),200);
 
     // filter
+    for (int ii = 0; ii < matches12.size(); ++ii )
+        if(matches12[ii].distance > 200)
+        	matches12.erase(matches12.begin()+ii-1);
+    ASSERT_EQ(matches12.size(),200);
+
+    // RANSAC
+    PointVector trainMatches;
+    PointVector queryMatches;
+    for( int ii = 0; ii < matches12.size(); ++ii )
+    {
+    	trainMatches.push_back( kpts2[matches12[ii].trainIdx].pt);
+    	queryMatches.push_back( kpts1[matches12[ii].queryIdx].pt);
+    }
+    std::vector<uchar> status;
+    cv::Mat h = cv::findHomography(trainMatches,queryMatches,CV_RANSAC,1.0, status);
     DMatchVector best_matches12;
     KeyPointVector kpts_matched12_img1;
     KeyPointVector kpts_matched12_img2;
-    mat_ptr->filterByDistance(10, 0.25, kpts1, kpts2, matches12, image1.rows, image1.cols, best_matches12, kpts_matched12_img2, kpts_matched12_img1);
-    ASSERT_EQ(best_matches12.size(),19);
-
-	// match 2-3
+    for(size_t ii = 0; ii < queryMatches.size(); ++ii)
+    {
+    	if(status.at(ii) != 0)
+    	{
+    		best_matches12.push_back(matches12[ii]);
+    		kpts_matched12_img1.push_back(cv::KeyPoint(queryMatches.at(ii),CV_32F));
+    		kpts_matched12_img2.push_back(cv::KeyPoint(trainMatches.at(ii),CV_32F));
+    	}
+    }
+
+    // match 2-3
 	DMatchVector matches23;
     mat_ptr->match(desc2,desc3,matches23);
     ASSERT_EQ(matches23.size(),200);
 
     // filter
+    for (int ii = 0; ii < matches23.size(); ++ii )
+        if(matches23[ii].distance > 200)
+        	matches23.erase(matches23.begin()+ii-1);
+    ASSERT_EQ(matches23.size(),200);
+
+    // RANSAC
+    trainMatches.clear();
+    queryMatches.clear();
+    for( int ii = 0; ii < matches23.size(); ++ii )
+    {
+    	trainMatches.push_back( kpts3[matches23[ii].trainIdx].pt);
+    	queryMatches.push_back( kpts2[matches23[ii].queryIdx].pt);
+    }
+    status.clear();
+    h = cv::findHomography(trainMatches,queryMatches,CV_RANSAC,1.0, status);
     DMatchVector best_matches23;
     KeyPointVector kpts_matched23_img2;
     KeyPointVector kpts_matched23_img3;
-    mat_ptr->filterByDistance(10, 0.25, kpts2, kpts3, matches23, image1.rows, image1.cols, best_matches23, kpts_matched23_img3, kpts_matched23_img2);
-    ASSERT_EQ(best_matches23.size(),153);
+    for(size_t ii = 0; ii < queryMatches.size(); ++ii)
+    {
+    	if(status.at(ii) != 0)
+    	{
+    		best_matches23.push_back(matches12[ii]);
+    		kpts_matched23_img2.push_back(cv::KeyPoint(queryMatches.at(ii),CV_32F));
+    		kpts_matched23_img3.push_back(cv::KeyPoint(trainMatches.at(ii),CV_32F));
+    	}
+    }
 
 #ifdef _VU_DEBUG
     cv::Mat img_graphics = mat_ptr->drawMatches(image1, image2, image3,kpts_matched12_img1, kpts_matched12_img2, kpts_matched23_img2, kpts_matched23_img3);
-    cv::imshow("test1",img_graphics);
-    cv::waitKey(2000);
+    cv::imshow("test",img_graphics);
+    cv::waitKey(5000);
 #endif
 
     // get only those matches existing in all 3 images
@@ -170,12 +214,12 @@ TEST(TrifocalTensor, DetectorDescriptorMatcher)
     KeyPointVector kpts_good_in2;
     KeyPointVector kpts_good_in3;
     vision_utils::getCommonKps(kpts_matched12_img1, kpts_matched12_img2, kpts_matched23_img2, kpts_matched23_img3, kpts_good_in1, kpts_good_in2, kpts_good_in3);
-    ASSERT_EQ(kpts_good_in1.size(), 19);
+    ASSERT_EQ(kpts_good_in1.size(), 11);
 
 #ifdef _VU_DEBUG
     cv::Mat img_graphics2 = mat_ptr->drawMatches(image1, image2, image3, kpts_good_in1, kpts_good_in2, kpts_good_in3);
-    cv::imshow("test2",img_graphics2);
-    cv::waitKey(2000);
+    cv::imshow("test1",img_graphics2);
+    cv::waitKey(5000);
 #endif
 }
 
@@ -234,30 +278,78 @@ TEST(TrifocalTensor, ComputeTensor)
     ASSERT_EQ(matches12.size(),200);
 
     // filter
+    for (int ii = 0; ii < matches12.size(); ++ii )
+        if(matches12[ii].distance > 200)
+        	matches12.erase(matches12.begin()+ii-1);
+
+    // RANSAC
+    PointVector trainMatches;
+    PointVector queryMatches;
+    for( int ii = 0; ii < matches12.size(); ++ii )
+    {
+    	trainMatches.push_back( kpts2[matches12[ii].trainIdx].pt);
+    	queryMatches.push_back( kpts1[matches12[ii].queryIdx].pt);
+    }
+    std::vector<uchar> status;
+    cv::Mat h = cv::findHomography(trainMatches,queryMatches,CV_RANSAC,1.0, status);
     DMatchVector best_matches12;
     KeyPointVector kpts_matched12_img1;
     KeyPointVector kpts_matched12_img2;
-    mat_ptr->filterByDistance(10, 0.25, kpts1, kpts2, matches12, image1.rows, image1.cols, best_matches12, kpts_matched12_img2, kpts_matched12_img1);
-    ASSERT_EQ(best_matches12.size(),20);
-
-	// match 2-3
+    for(size_t ii = 0; ii < queryMatches.size(); ++ii)
+    {
+    	if(status.at(ii) != 0)
+    	{
+    		best_matches12.push_back(matches12[ii]);
+    		kpts_matched12_img1.push_back(cv::KeyPoint(queryMatches.at(ii),CV_32F));
+    		kpts_matched12_img2.push_back(cv::KeyPoint(trainMatches.at(ii),CV_32F));
+    	}
+    }
+
+    // match 2-3
 	DMatchVector matches23;
     mat_ptr->match(desc2,desc3,matches23);
     ASSERT_EQ(matches23.size(),200);
 
     // filter
+    for (int ii = 0; ii < matches23.size(); ++ii )
+        if(matches23[ii].distance > 200)
+        	matches23.erase(matches23.begin()+ii-1);
+
+    // RANSAC
+    trainMatches.clear();
+    queryMatches.clear();
+    for( int ii = 0; ii < matches23.size(); ++ii )
+    {
+    	trainMatches.push_back( kpts3[matches23[ii].trainIdx].pt);
+    	queryMatches.push_back( kpts2[matches23[ii].queryIdx].pt);
+    }
+    status.clear();
+    h = cv::findHomography(trainMatches,queryMatches,CV_RANSAC,1.0, status);
     DMatchVector best_matches23;
     KeyPointVector kpts_matched23_img2;
     KeyPointVector kpts_matched23_img3;
-    mat_ptr->filterByDistance(10, 0.25, kpts2, kpts3, matches23, image1.rows, image1.cols, best_matches23, kpts_matched23_img3, kpts_matched23_img2);
-    ASSERT_EQ(best_matches23.size(),154);
+    for(size_t ii = 0; ii < queryMatches.size(); ++ii)
+    {
+    	if(status.at(ii) != 0)
+    	{
+    		best_matches23.push_back(matches12[ii]);
+    		kpts_matched23_img2.push_back(cv::KeyPoint(queryMatches.at(ii),CV_32F));
+    		kpts_matched23_img3.push_back(cv::KeyPoint(trainMatches.at(ii),CV_32F));
+    	}
+    }
 
     // get only those matches existing in all 3 images
     KeyPointVector kpts_good_in1;
     KeyPointVector kpts_good_in2;
     KeyPointVector kpts_good_in3;
     vision_utils::getCommonKps(kpts_matched12_img1, kpts_matched12_img2, kpts_matched23_img2, kpts_matched23_img3, kpts_good_in1, kpts_good_in2, kpts_good_in3);
-    ASSERT_EQ(kpts_good_in1.size(), 19);
+    ASSERT_EQ(kpts_good_in1.size(), 11);
+
+#ifdef _VU_DEBUG
+    cv::Mat img_graphics2 = mat_ptr->drawMatches(image1, image2, image3, kpts_good_in1, kpts_good_in2, kpts_good_in3);
+    cv::imshow("test1",img_graphics2);
+    cv::waitKey(5000);
+#endif
 
     // Compute tensor
     Eigen::MatrixXd list1 = vision_utils::KeyPointVecToEigenMat(kpts_good_in1);
@@ -267,11 +359,35 @@ TEST(TrifocalTensor, ComputeTensor)
     vision_utils::TrifocalTensor tensor;
     tensor.computeTensor(list1, list2, list3);
 
-    tensor.print();
+    KeyPointVector kpts_tensor_in3;
+    for (int ii=0;ii<kpts_good_in1.size(); ++ii)
+    {
+    	Eigen::Vector3d p1,p2,p3;
+    	p1 << kpts_good_in1[0].pt.x,kpts_good_in1[ii].pt.y,1;
+    	p2 << kpts_good_in2[0].pt.x,kpts_good_in2[ii].pt.y,1;
+    	tensor.transfer(p1, p2, p3);
+    	kpts_tensor_in3.push_back(cv::KeyPoint(p3(0),p3(1),CV_32F));
+    }
 
-//    Eigen::Vector3d p1,p2,p3;
-//    tensor.transfert(p1, p2, p3);
+#ifdef _VU_DEBUG
+    cv::Mat img_tmp;
+    cvtColor(image3, img_tmp, cv::COLOR_GRAY2RGB);
+
+    for (int ii=0;ii<kpts_tensor_in3.size(); ++ii)
+    {
+    	cv::circle(img_tmp,kpts_good_in3[ii].pt,5,cv::Scalar(0,255,0));
+    	cv::circle(img_tmp,kpts_tensor_in3[ii].pt,5,cv::Scalar(0,0,255));
+    }
+    cv::imshow("test2",img_tmp);
+    cv::waitKey(10000);
+#endif
 
+    for (int ii=0;ii<kpts_tensor_in3.size(); ++ii)
+    {
+    	std::cout << "TODO: debug tensor with gtest_trifocaltensor" << std::endl;
+    	std::cout << std::sqrt(std::pow(kpts_tensor_in3[ii].pt.x-kpts_good_in3[ii].pt.x,2) + std::pow(kpts_tensor_in3[ii].pt.y-kpts_good_in3[ii].pt.y,2)) << std::endl;
+//    	ASSERT_TRUE(std::sqrt(std::pow(kpts_tensor_in3[ii].pt.x-kpts_good_in3[ii].pt.x,2) + std::pow(kpts_tensor_in3[ii].pt.y-kpts_good_in3[ii].pt.y,2))<=1.0);
+    }
 }