Clean and improve some code

Author: yzhou-bcom

interfaces/SolARDescriptorMatcherGeometricOpencv.h

@@ -95,13 +95,9 @@ class SOLAROPENCV_EXPORT_API SolARDescriptorMatcherGeometricOpencv : public base
     float m_distanceRatio = 0.75f;
     float m_paddingRatio = 0.003f;
 	float m_matchingDistanceMax = 500.f;
-	/// matcher type
-	std::string m_type = "BruteForce";
-	/// Matcher
+	/// Matcher used only in case of cuda 
 #ifdef WITHCUDA
 	cv::Ptr<cv::cuda::DescriptorMatcher> m_matcher;
-#else
-	cv::Ptr<cv::DescriptorMatcher> m_matcher;
 #endif  
 };
 

src/SolARDescriptorMatcherGeometricOpencv.cpp

@@ -20,7 +20,8 @@
 
 namespace xpcf  = org::bcom::xpcf;
 
-// test optimization code implemented by yzhou 
+// Optimizations made by yzhou. The optimization is based on the assumption that a high performance GPU is available
+// If you want to unactivate these optimizations you can comment the following line
 #define OPTIM_ON
 
 XPCF_DEFINE_FACTORY_CREATE_INSTANCE(SolAR::MODULES::OPENCV::SolARDescriptorMatcherGeometricOpencv)
@@ -45,17 +46,10 @@ SolARDescriptorMatcherGeometricOpencv::~SolARDescriptorMatcherGeometricOpencv()
 
 xpcf::XPCFErrorCode SolARDescriptorMatcherGeometricOpencv::onConfigured()
 {
-#ifdef OPTIM_ON
+#if defined(WITHCUDA) && defined(OPTIM_ON)
 	LOG_DEBUG(" SolARDescriptorMatcherGeometricOpencv onConfigured");
-#ifdef WITHCUDA
 	m_matcher = cv::cuda::DescriptorMatcher::createBFMatcher(cv::NORM_L2);
-#else
-	if (SolAROpenCVHelper::createMatcher(m_type, m_matcher) != FrameworkReturnCode::_SUCCESS) {
-		LOG_ERROR("Descriptor matcher type {} is not supported", m_type);
-		return xpcf::XPCFErrorCode::_FAIL;
-	}
-#endif // WITHCUDA
-#endif	
+#endif 
 	return xpcf::XPCFErrorCode::_SUCCESS;
 }
 
@@ -79,6 +73,7 @@ FrameworkReturnCode SolARDescriptorMatcherGeometricOpencv::match(const SRef<SolA
 	}
 	// calculate fundametal matrix
 #ifdef OPTIM_ON
+	// avoid inverting pose matrix 
 	Transform3Df pose1Inv;
 	for (int i = 0; i < 3; i++)
 		for (int j = 0; j < 3; j++)
@@ -102,6 +97,7 @@ FrameworkReturnCode SolARDescriptorMatcherGeometricOpencv::match(const SRef<SolA
 	cv::Mat K1(3, 3, CV_32FC1, (void *)camParams1.intrinsic.data());
 	cv::Mat K2(3, 3, CV_32FC1, (void *)camParams2.intrinsic.data());
 #ifdef OPTIM_ON
+	// avoid inverting intrinsic matrix 
 	cv::Mat K1tInv = cv::Mat::zeros(3, 3, CV_32FC1);
 	K1tInv.at<float>(0, 0) = 1.f / K1.at<float>(0, 0);
 	K1tInv.at<float>(1, 1) = 1.f / K1.at<float>(1, 1);
@@ -147,47 +143,48 @@ 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
+	// take into account of mask for both descriptors 1 & 2 
 	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;
+	if (mask1.size() == 0) {
+		cvDescriptor1.data = (uchar*)descriptors1->data();
 	}
-	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 {
+		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;
+		}
 	}
-#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
 
+	cv::Mat cvDescriptor2(indices2.size(), descriptors2->getNbElements(), type_conversion);
+	if (mask2.size() == 0) {
+		cvDescriptor2.data = (uchar*)descriptors2->data();
+	}
+	else {
+		uchar* buf_in = (uchar*)descriptors2->data();
+		uchar* buf_out = cvDescriptor2.data;
+		uint32_t 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;
+		}
+	}
 
-#ifdef OPTIM_ON
 	// perform descriptor matching first on GPU using Cuda if SolARModuleOpenCVCuda
+#if defined(WITHCUDA) && defined(OPTIM_ON)
+	cv::cuda::GpuMat cvDescriptor1Gpu, cvDescriptor2Gpu;
 	std::vector< std::vector<cv::DMatch> > nn_matches;
-#ifdef WITHCUDA
 	if (descriptors1->getDescriptorDataType() != DescriptorDataType::TYPE_32F) // convert to float because cuda descriptor match only supports float type 
 		cvDescriptor1.convertTo(cvDescriptor1, CV_32F);
 	if (descriptors2->getDescriptorDataType() != DescriptorDataType::TYPE_32F)
 		cvDescriptor2.convertTo(cvDescriptor2, CV_32F);
-	cv::cuda::GpuMat cvDescriptor1Gpu, cvDescriptor2Gpu;
+		
 	cvDescriptor1Gpu.upload(cvDescriptor1);
 	cvDescriptor2Gpu.upload(cvDescriptor2);
 	m_matcher->knnMatch(cvDescriptor1Gpu, cvDescriptor2Gpu, nn_matches, 2);
-#else
-	m_matcher->knnMatch(cvDescriptor1, cvDescriptor2, nn_matches, 2);
-#endif
-	
+
 	// filter the matches by three conditions (best distance, 2nd best distance and epipolar constraint)
 	std::vector<bool> checkMatches(indices2.size(), true);
 	float acceptedDist = m_paddingRatio * camParams2.resolution.width;
@@ -215,19 +212,19 @@ FrameworkReturnCode SolARDescriptorMatcherGeometricOpencv::match(const SRef<SolA
 	}
 #else 
 	// first apply epipolar constraint then apply descriptor matching  
-	std::vector<bool> checkMatches(undistortedKeypoints2.size(), true);
+	std::vector<bool> checkMatches(indices2.size(), true);
 	float acceptedDist = m_paddingRatio * camParams2.resolution.width;
 	for (int i = 0; i < indices1.size(); i++) {
-		const cv::Mat cvDes1 = cvDescriptor1.row(indices1[i]);
+		const cv::Mat cvDes1 = cvDescriptor1.row(i);
 		float bestDist = std::numeric_limits<float>::max();
 		float bestDist2 = std::numeric_limits<float>::max();
 		int bestIdx = -1;
 		cv::Point3f l = lines2[i];
 		float lxyz_norm = std::sqrt(l.x * l.x + l.y * l.y);
-		
-		for (int j = 0; j < undistortedKeypoints2.size(); j++) {
-			float x = undistortedKeypoints2[j].getX();
-			float y = undistortedKeypoints2[j].getY();
+
+		for (int j = 0; j < indices2.size(); j++) {
+			float x = undistortedKeypoints2[indices2[j]].getX();
+			float y = undistortedKeypoints2[indices2[j]].getY();
 			float disPointLine = std::abs(x * l.x + y * l.y + l.z) / lxyz_norm;
 			if (disPointLine < acceptedDist) {
 				float dist = cv::norm(cvDes1, cvDescriptor2.row(j), cv::NORM_L2);
@@ -244,11 +241,11 @@ FrameworkReturnCode SolARDescriptorMatcherGeometricOpencv::match(const SRef<SolA
 			}
 		}
 		if ((bestIdx != -1) && (bestDist < m_matchingDistanceMax) && (bestDist < m_distanceRatio * bestDist2) && (checkMatches[bestIdx])) {
-			matches.push_back(DescriptorMatch(indices1[i], bestIdx, bestDist));
+			matches.push_back(DescriptorMatch(indices1[i], indices2[bestIdx], bestDist));
 			checkMatches[bestIdx] = false;
 		}
 	}
-#endif 
+#endif
 	return FrameworkReturnCode::_SUCCESS;
 }