Merge branch 'develop' into feat/deploy-dir

Author: jim-bcom

interfaces/SolARDescriptorMatcherGeometricOpencv.h

@@ -25,6 +25,9 @@
 #include "opencv2/imgcodecs.hpp"
 #include "opencv2/highgui.hpp"
 #include <opencv2/calib3d.hpp>
+#ifdef WITHCUDA
+#include <opencv2/cudafeatures2d.hpp>
+#endif
 
 namespace SolAR {
 namespace MODULES {
@@ -70,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,
@@ -81,14 +85,24 @@ 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;
 
 private:
     float m_distanceRatio = 0.75f;
     float m_paddingRatio = 0.003f;
 	float m_matchingDistanceMax = 500.f;
+	/// matcher type
+	std::string m_type = "BruteForce";
+	/// Matcher
+#ifdef WITHCUDA
+	cv::Ptr<cv::cuda::DescriptorMatcher> m_matcher;
+#else
+	cv::Ptr<cv::DescriptorMatcher> m_matcher;
+#endif  
 };
 
 }

interfaces/SolARSVDTriangulationOpencv.h

@@ -133,6 +133,8 @@ class SOLAROPENCV_EXPORT_API SolARSVDTriangulationOpencv : public org::bcom::xpc
                        SRef<SolAR::datastructure::Frame> frame2,
                        const std::vector<SolAR::datastructure::DescriptorMatch> &matches,
                        const std::pair<uint32_t, uint32_t> & working_views,
+                       const SolAR::datastructure::CameraParameters & camParams1,
+                       const SolAR::datastructure::CameraParameters & camParams2,
                        std::vector<SRef<SolAR::datastructure::CloudPoint>> & pcloud,
                        const bool& onlyDepth = false) override;
 

src/SolARDescriptorMatcherGeometricOpencv.cpp

@@ -20,6 +20,9 @@
 
 namespace xpcf  = org::bcom::xpcf;
 
+// test optimization code implemented by yzhou 
+#define OPTIM_ON
+
 XPCF_DEFINE_FACTORY_CREATE_INSTANCE(SolAR::MODULES::OPENCV::SolARDescriptorMatcherGeometricOpencv)
 
 namespace SolAR {
@@ -40,6 +43,22 @@ SolARDescriptorMatcherGeometricOpencv::~SolARDescriptorMatcherGeometricOpencv()
     LOG_DEBUG(" SolARDescriptorMatcherGeometricOpencv destructor")
 }
 
+xpcf::XPCFErrorCode SolARDescriptorMatcherGeometricOpencv::onConfigured()
+{
+#ifdef 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	
+	return xpcf::XPCFErrorCode::_SUCCESS;
+}
+
 FrameworkReturnCode SolARDescriptorMatcherGeometricOpencv::match(const SRef<SolAR::datastructure::DescriptorBuffer> descriptors1,
                                                                  const SRef<SolAR::datastructure::DescriptorBuffer> descriptors2,
                                                                  const std::vector<SolAR::datastructure::Keypoint> &undistortedKeypoints1,
@@ -49,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
@@ -58,7 +78,20 @@ FrameworkReturnCode SolARDescriptorMatcherGeometricOpencv::match(const SRef<SolA
 		return FrameworkReturnCode::_ERROR_;
 	}
 	// calculate fundametal matrix
+#ifdef OPTIM_ON
+	Transform3Df pose1Inv;
+	for (int i = 0; i < 3; i++)
+		for (int j = 0; j < 3; j++)
+			pose1Inv(i, j) = pose1(j, i);
+	for (int i = 0; i < 3; i++)
+		pose1Inv(3, i) = 0.f;
+	pose1Inv(3, 3) = 1.f;
+	for (int i = 0; i < 3; i++)
+		pose1Inv(i, 3) = -(pose1(0, 3)*pose1(0, i) + pose1(1, 3)*pose1(1, i) + pose1(2, 3)*pose1(2, i));
+	Transform3Df pose12 = pose1Inv * pose2;
+#else 
 	Transform3Df pose12 = pose1.inverse() * pose2;
+#endif
 	cv::Mat R12 = (cv::Mat_<float>(3, 3) << pose12(0, 0), pose12(0, 1), pose12(0, 2),
 		pose12(1, 0), pose12(1, 1), pose12(1, 2),
 		pose12(2, 0), pose12(2, 1), pose12(2, 2));
@@ -68,21 +101,44 @@ FrameworkReturnCode SolARDescriptorMatcherGeometricOpencv::match(const SRef<SolA
 		-T12.at<float>(1), T12.at<float>(0), 0);
 	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
+	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);
+	K1tInv.at<float>(2, 2) = 1.f;
+	K1tInv.at<float>(2, 0) = -K1.at<float>(0, 2) / K1.at<float>(0, 0);
+	K1tInv.at<float>(2, 1) = -K1.at<float>(1, 2) / K1.at<float>(1, 1);
+	cv::Mat K2Inv = cv::Mat::zeros(3, 3, CV_32FC1);
+	K2Inv.at<float>(0, 0) = 1.f / K2.at<float>(0, 0);
+	K2Inv.at<float>(1, 1) = 1.f / K2.at<float>(1, 1);
+	K2Inv.at<float>(2, 2) = 1.f;
+	K2Inv.at<float>(0, 2) = -K2.at<float>(0, 2) / K2.at<float>(0, 0);
+	K2Inv.at<float>(1, 2) = -K2.at<float>(1, 2) / K2.at<float>(1, 1);
+	cv::Mat F = K1tInv * T12x * R12 * K2Inv;
+#else 
 	cv::Mat F = K1.t().inv() * T12x * R12 * K2.inv();
+#endif
 
 	// 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
@@ -91,24 +147,88 @@ 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
+	
 
-	// match
+#ifdef OPTIM_ON
+	// perform descriptor matching first on GPU using Cuda if SolARModuleOpenCVCuda
+	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;
+	for (auto i = 0; i < indices1.size(); i++) {
+		const auto& best_matches = nn_matches[i];
+		if (!checkMatches[best_matches[0].trainIdx])
+			continue;
+		if (best_matches[0].distance >= m_matchingDistanceMax)
+			continue;
+		if (best_matches[0].distance >= m_distanceRatio * best_matches[1].distance) // no big difference between best and 2nd best, Lowe's ratio test
+			continue;
+		// the match is accepted 
+		// now we should check the epipolar constraint 
+		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[best_matches[0].trainIdx] = false;
+		}
+	}
+#else 
+	// first apply epipolar constraint then apply descriptor matching  
 	std::vector<bool> checkMatches(undistortedKeypoints2.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]);
 		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();
-			cv::Point3f l = lines2[i];
-			float disPointLine = std::abs(x * l.x + y * l.y + l.z) / std::sqrt(l.x * l.x + l.y * l.y);
+			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);
 				if (dist < bestDist)
@@ -128,7 +248,7 @@ FrameworkReturnCode SolARDescriptorMatcherGeometricOpencv::match(const SRef<SolA
 			checkMatches[bestIdx] = false;
 		}
 	}
-
+#endif 
 	return FrameworkReturnCode::_SUCCESS;
 }
 

src/SolARImageViewerOpencv.cpp

@@ -128,7 +128,10 @@ FrameworkReturnCode SolARImageViewerOpencv::displayKey(const SRef<Image> img, ch
         key = cv::waitKey(1);  // wait for a keystroke to display window
 
     if(key == (char)(m_exitKey))
+    {
+        cv::destroyWindow(m_title);
         return FrameworkReturnCode::_STOP;
+    }
 
     return FrameworkReturnCode::_SUCCESS;
 }

src/SolARKeypointDetectorOpencv.cpp

@@ -225,8 +225,13 @@ void SolARKeypointDetectorOpencv::detect(const SRef<Image> image, std::vector<Ke
 	   float px = keypoint.pt.x;
 	   float py = keypoint.pt.y;
 	   cv::Vec3b bgr{ 0, 0, 0 };
-	   if (opencvImage.channels() == 3)
-		   bgr = opencvImage.at<cv::Vec3b>((int)py, (int)px);
+       if (opencvImage.channels() == 3) {
+           bgr = image->getPixel<cv::Vec3b>((int)py, (int)px);
+       }
+       else {
+           uint8_t value = image->getPixel<uint8_t>((int)py, (int)px);
+           bgr = cv::Vec3b(value, value, value);
+       }
        kpa.init(kpID++, px, py, bgr[2], bgr[1], bgr[0], keypoint.size, keypoint.angle, keypoint.response, keypoint.octave, keypoint.class_id) ;
        keypoints.push_back(kpa);	   
     }

src/SolARMapFusionOpencv.cpp

@@ -164,7 +164,12 @@ void SolARMapFusionOpencv::fuseMap(const std::vector<std::pair<uint32_t, uint32_
 	globalMap->getKeyframeCollection(globalKeyframeCollection);
 	globalKeyframeCollection->getAllKeyframes(globalKeyframes);
 
-	// get covisibility graph
+    // get Camera Parameters
+    const SRef<CameraParametersCollection>& cameraParametersCollection = map->getConstCameraParametersCollection();
+    SRef<CameraParametersCollection> globalCameraParametersCollection;
+    globalMap->getCameraParametersCollection(globalCameraParametersCollection);
+
+    // get covisibility graph
 	const SRef<CovisibilityGraph>& covisibilityGraph = map->getConstCovisibilityGraph();
 	SRef<CovisibilityGraph> globalCovisibilityGraph;
 	globalMap->getCovisibilityGraph(globalCovisibilityGraph);
@@ -191,10 +196,54 @@ void SolARMapFusionOpencv::fuseMap(const std::vector<std::pair<uint32_t, uint32_
 		idxCPOldNew[idxOld] = cp->getId();
 	}
 
+    // update global map camera parameters with local map ones
+    std::vector<SRef<CameraParameters>> localMapCameraParameters;
+    cameraParametersCollection->getAllCameraParameters(localMapCameraParameters);
+    std::vector<SRef<CameraParameters>> globalMapCameraParameters;
+    globalCameraParametersCollection->getAllCameraParameters(globalMapCameraParameters);
+    std::map<uint32_t, uint32_t> local2GlobalMapCameraID; // A table matching camera ID between the local and global map
+    for (const auto &itLocal: localMapCameraParameters) {
+        bool foundCameraParam = false;
+        for (const auto &itGlobal: globalMapCameraParameters) {
+            if (itLocal->intrinsic == itGlobal->intrinsic &&
+                itLocal->distortion == itGlobal->distortion &&
+                itLocal->type == itGlobal->type &&
+                itLocal->resolution.width == itGlobal->resolution.width &&
+                itLocal->resolution.height == itGlobal->resolution.height &&
+                itLocal->name == itGlobal->name)
+            {
+                LOG_DEBUG("Camera parameters found in global map with ID = {}", itGlobal->id);
+                local2GlobalMapCameraID.insert(std::make_pair(itLocal->id, itGlobal->id));
+                foundCameraParam = true;
+                continue;
+            }
+        }
+        if (!foundCameraParam) {
+            SRef<CameraParameters> newCameraParameters = xpcf::utils::make_shared<CameraParameters>(*itLocal);
+            globalCameraParametersCollection->addCameraParameters(newCameraParameters);
+            local2GlobalMapCameraID.insert(std::make_pair(itLocal->id, newCameraParameters->id));
+            LOG_DEBUG("Add new camera parameters in global map with ID = {}", newCameraParameters->id);
+        }
+    }
+
 	// add keyframes of local map to global map
 	std::map<uint32_t, uint32_t> idxKfOldNew;
-	for (const auto &kf : keyframes) {
-		// unscale keyframe pose
+    for (const auto &kf : keyframes) {
+        // Check if CameraID exists in matching table
+        std::map<uint32_t, uint32_t>::iterator camIt = local2GlobalMapCameraID.find(kf->getCameraID());
+
+        if (camIt != local2GlobalMapCameraID.end()) {
+            LOG_DEBUG("Camera parameters for current key frame found => set to global map ID ({} -> {})",
+                     kf->getCameraID(), camIt->second);
+
+            kf->setCameraID(camIt->second);
+        }
+        else {
+            LOG_ERROR("camera parameters with ID {} does not exists in local map", kf->getCameraID());
+            continue;
+        }
+
+        // unscale keyframe pose
 		Transform3Df kfPose = kf->getPose();
 		Eigen::Matrix3f scale;
 		Eigen::Matrix3f rot;