Mask both frame 1 and 2 & replace inverse() by analytical formula

Author: yzhou-bcom

interfaces/SolARDescriptorMatcherGeometricOpencv.h

@@ -73,8 +73,9 @@ class SOLAROPENCV_EXPORT_API SolARDescriptorMatcherGeometricOpencv : public base
     /// @param[in] camParams1 The intrinsic parameters of the camera 1.
     /// @param[in] camParams2 The intrinsic parameters of the camera 2.
     /// @param[out] matches A vector of matches representing pairs of indices relatively to the first and second set of descriptors.
-    /// @param[in] mask The indices of descriptors in the first frame are used for matching to the second frame. If it is empty then all will be used.
-    /// @return FrameworkReturnCode::_SUCCESS if matching succeed, else FrameworkReturnCode::_ERROR_
+    /// @param[in] mask1 The indices of descriptors in the first frame are used for matching to the second frame. If it is empty then all will be used.
+    /// @param[in] mask2 The indices of descriptors in the second frame are used for matching to the first frame. If it is empty then all will be used.
+	/// @return FrameworkReturnCode::_SUCCESS if matching succeed, else FrameworkReturnCode::_ERROR_
     FrameworkReturnCode match(const SRef<SolAR::datastructure::DescriptorBuffer> descriptors1,
                               const SRef<SolAR::datastructure::DescriptorBuffer> descriptors2,
                               const std::vector<SolAR::datastructure::Keypoint> &undistortedKeypoints1,
@@ -84,7 +85,8 @@ class SOLAROPENCV_EXPORT_API SolARDescriptorMatcherGeometricOpencv : public base
                               const SolAR::datastructure::CameraParameters & camParams1,
                               const SolAR::datastructure::CameraParameters & camParams2,
                               std::vector<SolAR::datastructure::DescriptorMatch> & matches,
-                              const std::vector<uint32_t>& mask = {}) override;
+                              const std::vector<uint32_t>& mask1 = {},
+							  const std::vector<uint32_t>& mask2 = {}) override;
 
 	org::bcom::xpcf::XPCFErrorCode onConfigured() override final;
 	void unloadComponent() override;

src/SolARDescriptorMatcherGeometricOpencv.cpp

@@ -68,7 +68,8 @@ FrameworkReturnCode SolARDescriptorMatcherGeometricOpencv::match(const SRef<SolA
                                                                  const SolAR::datastructure::CameraParameters & camParams1,
                                                                  const SolAR::datastructure::CameraParameters & camParams2,
                                                                  std::vector<SolAR::datastructure::DescriptorMatch> & matches,
-                                                                 const std::vector<uint32_t>& mask)
+                                                                 const std::vector<uint32_t>& mask1,
+																 const std::vector<uint32_t>& mask2)
 {
 	matches.clear();
 	// check conditions
@@ -120,17 +121,24 @@ FrameworkReturnCode SolARDescriptorMatcherGeometricOpencv::match(const SRef<SolA
 
 	// get input points in the first frame
 	std::vector<cv::Point2f> pts1;
-	std::vector<uint32_t> indices1;
-	if (mask.size() == 0) {
+	std::vector<uint32_t> indices1, indices2;
+	if (mask1.size() == 0) {
 		for (int i = 0; i < undistortedKeypoints1.size(); ++i) {
 			pts1.push_back(cv::Point2f(undistortedKeypoints1[i].getX(), undistortedKeypoints1[i].getY()));
 			indices1.push_back(i);
 		}
 	}
 	else {
-		indices1 = mask;
-		for (int i = 0; i < mask.size(); ++i)
-			pts1.push_back(cv::Point2f(undistortedKeypoints1[mask[i]].getX(), undistortedKeypoints1[mask[i]].getY()));
+		indices1 = mask1;
+		for (int i = 0; i < mask1.size(); ++i)
+			pts1.push_back(cv::Point2f(undistortedKeypoints1[mask1[i]].getX(), undistortedKeypoints1[mask1[i]].getY()));
+	}
+	if (mask2.size() == 0) {
+		for (int i = 0; i < undistortedKeypoints2.size(); ++i)
+			indices2.push_back(i);
+	}
+	else {
+		indices2 = mask2;
 	}
 
 	// compute epipolar lines
@@ -139,10 +147,30 @@ FrameworkReturnCode SolARDescriptorMatcherGeometricOpencv::match(const SRef<SolA
 
 	// convert frame descriptor to opencv's descriptor
 	uint32_t type_conversion = SolAR::MODULES::OPENCV::SolAROpenCVHelper::deduceOpenDescriptorCVType(descriptors1->getDescriptorDataType());
+#ifdef OPTIM_ON
+	cv::Mat cvDescriptor1(indices1.size(), descriptors1->getNbElements(), type_conversion);
+	uchar* buf_in = (uchar*)descriptors1->data();
+	uchar* buf_out = cvDescriptor1.data;
+	uint32_t num_elements = static_cast<uint32_t>(descriptors1->getNbElements());
+	for (auto i = 0; i < indices1.size(); i++) {
+		std::memcpy(buf_out, buf_in + num_elements * indices1[i], num_elements);
+		buf_out += num_elements;
+	}
+	cv::Mat cvDescriptor2(indices2.size(), descriptors2->getNbElements(), type_conversion);
+	buf_in = (uchar*)descriptors2->data();
+	buf_out = cvDescriptor2.data;
+	num_elements = static_cast<uint32_t>(descriptors2->getNbElements());
+	for (auto i = 0; i < indices2.size(); i++) {
+		std::memcpy(buf_out, buf_in + num_elements * indices2[i], num_elements);
+		buf_out += num_elements;
+	}
+#else 
 	cv::Mat cvDescriptor1(descriptors1->getNbDescriptors(), descriptors1->getNbElements(), type_conversion);
 	cvDescriptor1.data = (uchar*)descriptors1->data();
 	cv::Mat cvDescriptor2(descriptors2->getNbDescriptors(), descriptors2->getNbElements(), type_conversion);
 	cvDescriptor2.data = (uchar*)descriptors2->data();
+#endif
+	
 
 #ifdef OPTIM_ON
 	// perform descriptor matching first on GPU using Cuda if SolARModuleOpenCVCuda
@@ -161,10 +189,10 @@ FrameworkReturnCode SolARDescriptorMatcherGeometricOpencv::match(const SRef<SolA
 #endif
 
 	// filter the matches by three conditions (best distance, 2nd best distance and epipolar constraint)
-	std::vector<bool> checkMatches(undistortedKeypoints2.size(), true);
+	std::vector<bool> checkMatches(indices2.size(), true);
 	float acceptedDist = m_paddingRatio * camParams2.resolution.width;
 	for (auto i = 0; i < indices1.size(); i++) {
-		const auto& best_matches = nn_matches[indices1[i]];
+		const auto& best_matches = nn_matches[i];
 		if (!checkMatches[best_matches[0].trainIdx])
 			continue;
 		if (best_matches[0].distance >= m_matchingDistanceMax)
@@ -173,16 +201,16 @@ FrameworkReturnCode SolARDescriptorMatcherGeometricOpencv::match(const SRef<SolA
 			continue;
 		// the match is accepted 
 		// now we should check the epipolar constraint 
-		int idx1 = best_matches[0].queryIdx;
-		int idx2 = best_matches[0].trainIdx;
+		int idx1 = indices1[best_matches[0].queryIdx];
+		int idx2 = indices2[best_matches[0].trainIdx];
 		float distance = best_matches[0].distance;
 		cv::Point3f l = lines2[i];  // line equation a*x + b*y +c = 0
 		float x = undistortedKeypoints2[idx2].getX();
 		float y = undistortedKeypoints2[idx2].getY();
 		float disPointLine = std::abs(x * l.x + y * l.y + l.z) / std::sqrt(l.x * l.x + l.y * l.y);
 		if (disPointLine < acceptedDist) {
 			matches.push_back(DescriptorMatch(idx1, idx2, distance));
-			checkMatches[idx2] = false;
+			checkMatches[best_matches[0].trainIdx] = false;
 		}
 	}
 #else 

src/SolARSVDTriangulationOpencv.cpp

@@ -20,7 +20,7 @@
 #include "SolAROpenCVHelper.h"
 #include "core/Log.h"
 #include "opencv2/calib3d/calib3d.hpp"
-
+#define OPTIM_ON
 namespace xpcf  = org::bcom::xpcf;
 
 #define EPSILON 0.0001
@@ -274,8 +274,30 @@ double SolARSVDTriangulationOpencv::triangulate(SRef<SolAR::datastructure::Frame
 	pcloud.clear();
 	Transform3Df poseView1 = frame1->getPose();
 	Transform3Df poseView2 = frame2->getPose();
+#ifdef OPTIM_ON
+	// compute inverse of pose using pose's property (rigid transformation)
+	Transform3Df poseView1Inverse, poseView2Inverse;
+	for (int i = 0; i < 3; i++) {
+		for (int j = 0; j < 3; j++) {
+			poseView1Inverse(i, j) = poseView1(j, i);
+			poseView2Inverse(i, j) = poseView2(j, i);
+		}
+	}
+	for (int i = 0; i < 3; i++) {
+		poseView1Inverse(3, i) = 0.f;
+		poseView2Inverse(3, i) = 0.f;
+	}
+	poseView1Inverse(3, 3) = 1.f;
+	poseView2Inverse(3, 3) = 1.f;
+	for (int i = 0; i < 3; i++) {
+		poseView1Inverse(i, 3) = -(poseView1(0, 3)*poseView1(0, i) + poseView1(1, 3)*poseView1(1, i) + poseView1(2, 3)*poseView1(2, i));
+		poseView2Inverse(i, 3) = -(poseView2(0, 3)*poseView2(0, i) + poseView2(1, 3)*poseView2(1, i) + poseView2(2, 3)*poseView2(2, i));
+	}
+#else 
 	Transform3Df poseView1Inverse = poseView1.inverse();
 	Transform3Df poseView2Inverse = poseView2.inverse();
+#endif
+	
 	cv::Mat Pose1 = (cv::Mat_<float>(3, 4) << poseView1Inverse(0, 0), poseView1Inverse(0, 1), poseView1Inverse(0, 2), poseView1Inverse(0, 3),
 		poseView1Inverse(1, 0), poseView1Inverse(1, 1), poseView1Inverse(1, 2), poseView1Inverse(1, 3),
 		poseView1Inverse(2, 0), poseView1Inverse(2, 1), poseView1Inverse(2, 2), poseView1Inverse(2, 3));