28 #ifndef UTIL3D_CORRESPONDENCES_H_
29 #define UTIL3D_CORRESPONDENCES_H_
31 #include <rtabmap/core/rtabmap_core_export.h>
33 #include <pcl/point_cloud.h>
34 #include <pcl/point_types.h>
35 #include <opencv2/features2d/features2d.hpp>
48 const std::multimap<int, cv::KeyPoint> & wordsA,
49 const std::multimap<int, cv::KeyPoint> & wordsB,
50 std::list<std::pair<cv::Point2f, cv::Point2f> > & pairs);
53 const std::multimap<int, cv::Point3f> & words1,
54 const std::multimap<int, cv::Point3f> & words2,
55 std::vector<cv::Point3f> & inliers1,
56 std::vector<cv::Point3f> & inliers2,
58 std::vector<int> * uniqueCorrespondences = 0);
61 const std::map<int, cv::Point3f> & words1,
62 const std::map<int, cv::Point3f> & words2,
63 std::vector<cv::Point3f> & inliers1,
64 std::vector<cv::Point3f> & inliers2,
66 std::vector<int> * correspondences = 0);
70 const std::multimap<int, pcl::PointXYZ> & words2,
71 pcl::PointCloud<pcl::PointXYZ> & cloud1,
72 pcl::PointCloud<pcl::PointXYZ> & cloud2);
75 const std::multimap<int, pcl::PointXYZ> & words2,
76 pcl::PointCloud<pcl::PointXYZ> & cloud1,
77 pcl::PointCloud<pcl::PointXYZ> & cloud2);
79 void RTABMAP_CORE_EXPORT
extractXYZCorrespondences(
const std::list<std::pair<cv::Point2f, cv::Point2f> > & correspondences,
80 const cv::Mat & depthImage1,
81 const cv::Mat & depthImage2,
85 pcl::PointCloud<pcl::PointXYZ> & cloud1,
86 pcl::PointCloud<pcl::PointXYZ> & cloud2);
88 void RTABMAP_CORE_EXPORT
extractXYZCorrespondences(
const std::list<std::pair<cv::Point2f, cv::Point2f> > & correspondences,
89 const pcl::PointCloud<pcl::PointXYZ> & cloud1,
90 const pcl::PointCloud<pcl::PointXYZ> & cloud2,
91 pcl::PointCloud<pcl::PointXYZ> & inliers1,
92 pcl::PointCloud<pcl::PointXYZ> & inliers2,
94 void RTABMAP_CORE_EXPORT
extractXYZCorrespondences(
const std::list<std::pair<cv::Point2f, cv::Point2f> > & correspondences,
95 const pcl::PointCloud<pcl::PointXYZRGB> & cloud1,
96 const pcl::PointCloud<pcl::PointXYZRGB> & cloud2,
97 pcl::PointCloud<pcl::PointXYZ> & inliers1,
98 pcl::PointCloud<pcl::PointXYZ> & inliers2,
101 int RTABMAP_CORE_EXPORT
countUniquePairs(
const std::multimap<int, pcl::PointXYZ> & wordsA,
102 const std::multimap<int, pcl::PointXYZ> & wordsB);
104 void RTABMAP_CORE_EXPORT
filterMaxDepth(pcl::PointCloud<pcl::PointXYZ> & inliers1,
105 pcl::PointCloud<pcl::PointXYZ> & inliers2,
108 bool removeDuplicates);