i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-10-04 00:57:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

added doc/tests for util3d_correspondences.h

Browse Source
This commit is contained in:
matlabbe
2025-06-22 15:04:03 -07:00
parent 3ca3c37ee7
commit 114f673ec2
5 changed files with 683 additions and 64 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/include/rtabmap/core/util3d_correspondences.h
+193 -27
View File
@@ -43,39 +43,81 @@ namespace rtabmap
namespace util3d
{
void RTABMAP_CORE_EXPORT findCorrespondences(
const std::multimap<int, cv::KeyPoint> & wordsA,
const std::multimap<int, cv::KeyPoint> & wordsB,
std::list<std::pair<cv::Point2f, cv::Point2f> > & pairs);
void RTABMAP_CORE_EXPORT findCorrespondences(
const std::multimap<int, cv::Point3f> & words1,
const std::multimap<int, cv::Point3f> & words2,
std::vector<cv::Point3f> & inliers1,
std::vector<cv::Point3f> & inliers2,
float maxDepth,
std::vector<int> * uniqueCorrespondences = 0);
void RTABMAP_CORE_EXPORT findCorrespondences(
const std::map<int, cv::Point3f> & words1,
const std::map<int, cv::Point3f> & words2,
std::vector<cv::Point3f> & inliers1,
std::vector<cv::Point3f> & inliers2,
float maxDepth,
std::vector<int> * correspondences = 0);
// remove depth by z axis
/**
* @brief Extracts 3D point correspondences between two sets of labeled 3D points.
*
* This function identifies common point IDs (keys) between two multimap structures containing
* `pcl::PointXYZ` points. For each shared key that appears **exactly once** in both input maps,
* and where both corresponding points are finite, the matched points are added to two output point clouds.
*
* The resulting `cloud1` and `cloud2` point clouds will contain points with a one-to-one correspondence,
* useful for geometric registration (e.g., ICP).
*
* @param words1 Input multimap of point ID to 3D point for the first dataset.
* @param words2 Input multimap of point ID to 3D point for the second dataset.
* @param cloud1 Output point cloud (corresponding to points from `words1`).
* @param cloud2 Output point cloud (corresponding to points from `words2`).
*
* @note Only keys that appear exactly once in both `words1` and `words2`, and whose associated
* `pcl::PointXYZ` entries are finite, will be included in the output clouds.
*/
void RTABMAP_CORE_EXPORT extractXYZCorrespondences(const std::multimap<int, pcl::PointXYZ> & words1,
const std::multimap<int, pcl::PointXYZ> & words2,
pcl::PointCloud<pcl::PointXYZ> & cloud1,
pcl::PointCloud<pcl::PointXYZ> & cloud2);
/**
* @brief Extracts reliable 3D point correspondences between two sets of labeled 3D points using RANSAC filtering.
*
* This function finds correspondences between `words1` and `words2` based on shared unique keys. For each common key
* that appears exactly once in both maps, and where the corresponding 3D points are finite, a candidate correspondence
* is formed. If more than 7 such pairs exist, RANSAC is used via OpenCV’s `cv::findFundamentalMat` to reject outliers
* based on the geometric consistency of the 2D projections.
*
* Only the inlier correspondences determined by RANSAC are returned in the output point clouds `cloud1` and `cloud2`.
*
* @param words1 Input multimap of point ID to `pcl::PointXYZ` for the first set of 3D features.
* @param words2 Input multimap of point ID to `pcl::PointXYZ` for the second set of 3D features.
* @param cloud1 Output point cloud containing inlier points from `words1`.
* @param cloud2 Output point cloud containing inlier points from `words2`.
*
* @note At least 8 valid point correspondences are required for RANSAC to compute a fundamental matrix.
* If fewer than 8 valid matches exist, the function does not modify the output clouds.
*
* @warning Only 2D `(x, y)` components of the 3D points are used for RANSAC filtering.
*
*/
void RTABMAP_CORE_EXPORT extractXYZCorrespondencesRANSAC(const std::multimap<int, pcl::PointXYZ> & words1,
const std::multimap<int, pcl::PointXYZ> & words2,
pcl::PointCloud<pcl::PointXYZ> & cloud1,
pcl::PointCloud<pcl::PointXYZ> & cloud2);
/**
* @brief Extracts 3D point correspondences from 2D pixel matches using depth images.
*
* This function projects matched 2D keypoints (pixel correspondences) from two RGB-D images into 3D space
* using the provided camera intrinsic parameters. Only valid and finite 3D points are retained. If a `maxDepth`
* threshold is provided, points farther than this threshold are excluded.
*
* The function returns two synchronized point clouds, `cloud1` and `cloud2`, where each point pair at the
* same index corresponds to a match between the two views.
*
* @param correspondences List of 2D point correspondences between image 1 and image 2.
* @param depthImage1 Depth image corresponding to the first set of points (CV_32FC1 or CV_16UC1).
* @param depthImage2 Depth image corresponding to the second set of points (same format as depthImage1).
* @param cx Principal point x-coordinate (camera intrinsic).
* @param cy Principal point y-coordinate (camera intrinsic).
* @param fx Focal length in x-direction (camera intrinsic).
* @param fy Focal length in y-direction (camera intrinsic).
* @param maxDepth Maximum allowed depth for a correspondence to be considered valid. If <= 0, all depths are accepted.
* @param cloud1 Output point cloud with 3D points corresponding to the first image.
* @param cloud2 Output point cloud with 3D points corresponding to the second image.
*
* @note
* - Both output point clouds are resized to contain only the valid 3D matches after filtering.
* - Invalid, non-finite, or out-of-range depth values are automatically filtered out.
*
*/
void RTABMAP_CORE_EXPORT extractXYZCorrespondences(const std::list<std::pair<cv::Point2f, cv::Point2f> > & correspondences,
const cv::Mat & depthImage1,
const cv::Mat & depthImage2,
@@ -85,28 +127,152 @@ void RTABMAP_CORE_EXPORT extractXYZCorrespondences(const std::list<std::pair<cv:
pcl::PointCloud<pcl::PointXYZ> & cloud1,
pcl::PointCloud<pcl::PointXYZ> & cloud2);
/**
* @brief Extracts 3D correspondences from 2D feature matches using `pcl::PointXYZ` organized point clouds.
*
* This function projects 2D keypoint matches into 3D using the corresponding organized
* point clouds (`cloud1` and `cloud2`). Points that are not finite are discarded.
*
* @param correspondences List of 2D point correspondences between image 1 and image 2.
* @param cloud1 Organized `pcl::PointXYZ` point cloud corresponding to the first image.
* @param cloud2 Organized `pcl::PointXYZ` point cloud corresponding to the second image.
* @param inliers1 Output 3D points from `cloud1` corresponding to valid 2D matches.
* @param inliers2 Output 3D points from `cloud2` corresponding to valid 2D matches.
*
* @note
* - Only organized point clouds are supported (i.e., width × height layout must match
* image size from which 2D keypoints were taken).
*
*/
void RTABMAP_CORE_EXPORT extractXYZCorrespondences(const std::list<std::pair<cv::Point2f, cv::Point2f> > & correspondences,
const pcl::PointCloud<pcl::PointXYZ> & cloud1,
const pcl::PointCloud<pcl::PointXYZ> & cloud2,
pcl::PointCloud<pcl::PointXYZ> & inliers1,
pcl::PointCloud<pcl::PointXYZ> & inliers2,
char depthAxis);
pcl::PointCloud<pcl::PointXYZ> & inliers2);
/**
* @brief Extracts 3D correspondences from 2D feature matches using `pcl::PointXYZRGB` organized point clouds.
*
* This overload behaves identically to the `pcl::PointXYZ` version, but supports input point clouds
* that contain RGB color data. The color is not used—only the XYZ fields are extracted.
*
* @param correspondences List of matched 2D keypoints between two images.
* @param cloud1 Organized `pcl::PointXYZRGB` point cloud for the first image.
* @param cloud2 Organized `pcl::PointXYZRGB` point cloud for the second image.
* @param inliers1 Output 3D points from `cloud1` corresponding to valid 2D matches.
* @param inliers2 Output 3D points from `cloud2` corresponding to valid 2D matches.
*/
void RTABMAP_CORE_EXPORT extractXYZCorrespondences(const std::list<std::pair<cv::Point2f, cv::Point2f> > & correspondences,
const pcl::PointCloud<pcl::PointXYZRGB> & cloud1,
const pcl::PointCloud<pcl::PointXYZRGB> & cloud2,
pcl::PointCloud<pcl::PointXYZ> & inliers1,
pcl::PointCloud<pcl::PointXYZ> & inliers2,
char depthAxis);
pcl::PointCloud<pcl::PointXYZ> & inliers2);
/**
* @brief Counts the number of unique 3D point correspondences between two sets of word-indexed features.
*
* This function iterates over the unique keys (word IDs) in `wordsA` and checks if the same key exists
* in `wordsB`. A pair is considered "unique" if both `wordsA` and `wordsB` contain exactly one 3D point
* (i.e., one `pcl::PointXYZ`) associated with the same key.
*
* @param wordsA A multimap of word IDs to 3D points (e.g., from frame A).
* @param wordsB A multimap of word IDs to 3D points (e.g., from frame B).
* @return The number of unique pairs where both `wordsA` and `wordsB` contain exactly one point for a given word ID.
*/
int RTABMAP_CORE_EXPORT countUniquePairs(const std::multimap<int, pcl::PointXYZ> & wordsA,
const std::multimap<int, pcl::PointXYZ> & wordsB);
/**
* @brief Filters pairs of 3D points by maximum depth along a specified axis and optionally removes duplicates.
*
* This function takes two point clouds (`inliers1` and `inliers2`) containing corresponding 3D points,
* and filters out pairs where either point exceeds a specified maximum depth value along the given axis.
* It can also optionally remove duplicate points in the first point cloud.
*
* @param[in,out] inliers1 The first point cloud of 3D points to be filtered. Points failing the filter will be removed.
* @param[in,out] inliers2 The second point cloud of 3D points corresponding to `inliers1`. Points failing the filter will be removed.
* Must be the same size as `inliers1`.
* @param[in] maxDepth The maximum allowed depth value along the specified axis. Points with coordinate values greater or equal to
* this value on that axis will be removed. If `maxDepth` is less or equal to zero, no filtering is performed.
* @param[in] depthAxis The axis ('x', 'y', or 'z') along which to measure depth for filtering.
* @param[in] removeDuplicates If `true`, duplicate points in `inliers1` (exact coordinate matches) will be removed.
* Duplicates are detected only in `inliers1`.
*
* @warning The function modifies `inliers1` and `inliers2` in place, replacing them with filtered versions.
*/
void RTABMAP_CORE_EXPORT filterMaxDepth(pcl::PointCloud<pcl::PointXYZ> & inliers1,
pcl::PointCloud<pcl::PointXYZ> & inliers2,
float maxDepth,
char depthAxis,
bool removeDuplicates);
/**
* @brief Finds 2D point correspondences between two sets of keypoints based on matching word IDs.
*
* This function compares two multimap structures containing word IDs associated with `cv::KeyPoint`s.
* It extracts correspondences where the same word ID appears **exactly once** in each set.
*
* @param[in] wordsA A multimap from word ID to keypoints in set A.
* @param[in] wordsB A multimap from word ID to keypoints in set B.
* @param[out] pairs A list of matching 2D point correspondences (Point2f) between wordsA and wordsB.
*
* @note Only unique word ID matches (count == 1 in both sets) are considered valid correspondences.
*
* @example
* If `wordsA = [1 2 3 4 6 6]` and `wordsB = [1 1 2 4 5 6 6]`, the output `pairs` will contain correspondences
* for IDs `2` and `4`, because only those have exactly one match in both sets.
*/
void RTABMAP_CORE_EXPORT findCorrespondences(
const std::multimap<int, cv::KeyPoint> & wordsA,
const std::multimap<int, cv::KeyPoint> & wordsB,
std::list<std::pair<cv::Point2f, cv::Point2f> > & pairs);
/**
* @brief Finds 3D point correspondences between two sets of points based on matching word IDs.
*
* This function compares two multimaps of 3D points (typically from different views or frames).
* It returns point pairs where the same word ID appears **once** in both maps, the points are finite and valid,
* and optionally filtered by a maximum X-depth.
*
* @param[in] words1 A multimap of word IDs to 3D points in the first set.
* @param[in] words2 A multimap of word IDs to 3D points in the second set.
* @param[out] inliers1 Output vector of 3D points from `words1` with valid correspondences.
* @param[out] inliers2 Output vector of corresponding 3D points from `words2`.
* @param[in] maxDepth Optional filter: only points with X-values in (0, maxDepth] are kept. Use <= 0 to disable.
* @param[out] uniqueCorrespondences (Optional) Vector of word IDs corresponding to each pair.
*
* @note Only pairs with exactly one occurrence in each map and non-zero, finite coordinates are kept.
*/
void RTABMAP_CORE_EXPORT findCorrespondences(
const std::multimap<int, cv::Point3f> & words1,
const std::multimap<int, cv::Point3f> & words2,
std::vector<cv::Point3f> & inliers1,
std::vector<cv::Point3f> & inliers2,
float maxDepth,
std::vector<int> * uniqueCorrespondences = 0);
/**
* @brief Finds 3D point correspondences between two sets of uniquely indexed 3D points.
*
* This overload works with `std::map`, where each word ID appears at most once. It finds matching IDs
* and returns valid point pairs based on similar criteria to the multimap version.
*
* @param[in] words1 A map of word IDs to 3D points in the first set.
* @param[in] words2 A map of word IDs to 3D points in the second set.
* @param[out] inliers1 Output vector of 3D points from `words1` with valid correspondences.
* @param[out] inliers2 Output vector of corresponding 3D points from `words2`.
* @param[in] maxDepth Optional filter: only points with X-values in (0, maxDepth] are kept. Use <= 0 to disable.
* @param[out] correspondences (Optional) Vector of word IDs corresponding to valid matched pairs.
*
* @note Finite, non-zero points are required. The function ignores word IDs not found in both sets.
*/
void RTABMAP_CORE_EXPORT findCorrespondences(
const std::map<int, cv::Point3f> & words1,
const std::map<int, cv::Point3f> & words2,
std::vector<cv::Point3f> & inliers1,
std::vector<cv::Point3f> & inliers2,
float maxDepth,
std::vector<int> * correspondences = 0);
} // namespace util3d
} // namespace rtabmap
corelib/src/util3d_correspondences.cpp
+15 -33
View File
@@ -169,15 +169,22 @@ inline void extractXYZCorrespondencesImpl(const std::list<std::pair<cv::Point2f,
const pcl::PointCloud<PointT> & cloud1,
const pcl::PointCloud<PointT> & cloud2,
pcl::PointCloud<pcl::PointXYZ> & inliers1,
pcl::PointCloud<pcl::PointXYZ> & inliers2,
char depthAxis)
pcl::PointCloud<pcl::PointXYZ> & inliers2)
{
UASSERT(cloud1.empty() || cloud1.isOrganized());
UASSERT(cloud2.empty() || cloud2.isOrganized());
for(std::list<std::pair<cv::Point2f, cv::Point2f> >::const_iterator iter = correspondences.begin();
iter!=correspondences.end();
++iter)
{
PointT pt1 = cloud1.at(int(iter->first.y+0.5f) * cloud1.width + int(iter->first.x+0.5f));
PointT pt2 = cloud2.at(int(iter->second.y+0.5f) * cloud2.width + int(iter->second.x+0.5f));
int u1 = int(iter->first.x+0.5f);
int v1 = int(iter->first.y+0.5f);
int u2 = int(iter->second.x+0.5f);
int v2 = int(iter->second.y+0.5f);
UASSERT(v1>=0 && v1 < (int)cloud1.height && u1>=0 && u1<(int)cloud1.width);
UASSERT(v2>=0 && v2 < (int)cloud2.height && u2>=0 && u2<(int)cloud2.width);
PointT pt1 = cloud1.at(v1 * cloud1.width + u1);
PointT pt2 = cloud2.at(v2 * cloud2.width + u2);
if(pcl::isFinite(pt1) &&
pcl::isFinite(pt2))
{
@@ -191,19 +198,17 @@ void extractXYZCorrespondences(const std::list<std::pair<cv::Point2f, cv::Point2
const pcl::PointCloud<pcl::PointXYZ> & cloud1,
const pcl::PointCloud<pcl::PointXYZ> & cloud2,
pcl::PointCloud<pcl::PointXYZ> & inliers1,
pcl::PointCloud<pcl::PointXYZ> & inliers2,
char depthAxis)
pcl::PointCloud<pcl::PointXYZ> & inliers2)
{
extractXYZCorrespondencesImpl(correspondences, cloud1, cloud2, inliers1, inliers2, depthAxis);
extractXYZCorrespondencesImpl(correspondences, cloud1, cloud2, inliers1, inliers2);
}
void extractXYZCorrespondences(const std::list<std::pair<cv::Point2f, cv::Point2f> > & correspondences,
const pcl::PointCloud<pcl::PointXYZRGB> & cloud1,
const pcl::PointCloud<pcl::PointXYZRGB> & cloud2,
pcl::PointCloud<pcl::PointXYZ> & inliers1,
pcl::PointCloud<pcl::PointXYZ> & inliers2,
char depthAxis)
pcl::PointCloud<pcl::PointXYZ> & inliers2)
{
extractXYZCorrespondencesImpl(correspondences, cloud1, cloud2, inliers1, inliers2, depthAxis);
extractXYZCorrespondencesImpl(correspondences, cloud1, cloud2, inliers1, inliers2);
}
int countUniquePairs(const std::multimap<int, pcl::PointXYZ> & wordsA,
@@ -270,29 +275,6 @@ void filterMaxDepth(pcl::PointCloud<pcl::PointXYZ> & inliers1,
}
}
// a kdtree is constructed with cloud_target, then nearest neighbor
// is computed for each cloud_source points.
int getCorrespondencesCount(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_source,
const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_target,
float maxDistance)
{
pcl::search::KdTree<pcl::PointXYZ>::Ptr kdTree(new pcl::search::KdTree<pcl::PointXYZ>);
kdTree->setInputCloud(cloud_target);
int count = 0;
float sqrdMaxDistance = maxDistance * maxDistance;
for(unsigned int i=0; i<cloud_source->size(); ++i)
{
std::vector<int> ind(1);
std::vector<float> dist(1);
if(kdTree->nearestKSearch(cloud_source->at(i), 1, ind, dist) && dist[0] < sqrdMaxDistance)
{
++count;
}
}
return count;
}
/**
* if a=[1 2 3 4 6 6], b=[1 1 2 4 5 6 6], results= [(2,2) (4,4)]
* realPairsCount = 5
corelib/test/CMakeLists.txt
+6 -1
View File
@@ -29,4 +29,9 @@ gtest_discover_tests(test_util3d_registration)
#util3d_features.h
add_executable(test_util3d_features test_util3d_features.cpp)
target_link_libraries(test_util3d_features gtest_main rtabmap_core)
gtest_discover_tests(test_util3d_features)
gtest_discover_tests(test_util3d_features)
#util3d_correspondences.h
add_executable(test_util3d_correspondences test_util3d_correspondences.cpp)
target_link_libraries(test_util3d_correspondences gtest_main rtabmap_core)
gtest_discover_tests(test_util3d_correspondences)
corelib/test/test_util3d_correspondences.cpp
+469
View File
@@ -0,0 +1,469 @@
#include "gtest/gtest.h"
#include "rtabmap/core/util3d.h"
#include "rtabmap/core/util3d_correspondences.h"
#include "rtabmap/core/CameraModel.h"
#include "rtabmap/utilite/UException.h"
#include "rtabmap/utilite/UConversion.h"
#include <pcl/io/pcd_io.h>
using namespace rtabmap;
TEST(Util3dCorrespondences, extractXYZCorrespondencesValidOneToOneMatch) {
std::multimap<int, pcl::PointXYZ> words1 = {
{1, pcl::PointXYZ(1, 2, 3)},
{2, pcl::PointXYZ(4, 5, 6)}
};
std::multimap<int, pcl::PointXYZ> words2 = {
{1, pcl::PointXYZ(1.1f, 2.1f, 3.1f)},
{2, pcl::PointXYZ(4.1f, 5.1f, 6.1f)}
};
pcl::PointCloud<pcl::PointXYZ> cloud1, cloud2;
util3d::extractXYZCorrespondences(words1, words2, cloud1, cloud2);
ASSERT_EQ(cloud1.size(), 2);
ASSERT_EQ(cloud2.size(), 2);
EXPECT_EQ(cloud1.points[0].x, 1);
EXPECT_EQ(cloud2.points[1].z, 6.1f);
}
TEST(Util3dCorrespondences, extractXYZCorrespondencesDuplicateKeysIgnored) {
std::multimap<int, pcl::PointXYZ> words1 = {
{1, pcl::PointXYZ(0, 0, 0)},
{1, pcl::PointXYZ(1, 1, 1)}, // duplicate
{2, pcl::PointXYZ(2, 2, 2)}
};
std::multimap<int, pcl::PointXYZ> words2 = {
{1, pcl::PointXYZ(1, 1, 1)},
{2, pcl::PointXYZ(2.1f, 2.1f, 2.1f)}
};
pcl::PointCloud<pcl::PointXYZ> cloud1, cloud2;
util3d::extractXYZCorrespondences(words1, words2, cloud1, cloud2);
ASSERT_EQ(cloud1.size(), 1);
ASSERT_EQ(cloud2.size(), 1);
EXPECT_EQ(cloud1[0].x, 2);
EXPECT_EQ(cloud2[0].z, 2.1f);
}
TEST(Util3dCorrespondences, extractXYZCorrespondencesInvalidPointsIgnored) {
pcl::PointXYZ nanPt(std::numeric_limits<float>::quiet_NaN(), 0, 0);
std::multimap<int, pcl::PointXYZ> words1 = {
{1, pcl::PointXYZ(1, 2, 3)},
{2, nanPt}
};
std::multimap<int, pcl::PointXYZ> words2 = {
{1, pcl::PointXYZ(1.5f, 2.5f, 3.5f)},
{2, pcl::PointXYZ(4, 5, 6)}
};
pcl::PointCloud<pcl::PointXYZ> cloud1, cloud2;
util3d::extractXYZCorrespondences(words1, words2, cloud1, cloud2);
ASSERT_EQ(cloud1.size(), 1);
ASSERT_EQ(cloud2.size(), 1);
EXPECT_EQ(cloud1[0].x, 1);
EXPECT_EQ(cloud2[0].y, 2.5f);
}
TEST(Util3dCorrespondences, extractXYZCorrespondencesNoCommonIDs) {
std::multimap<int, pcl::PointXYZ> words1 = {
{10, pcl::PointXYZ(1, 2, 3)}
};
std::multimap<int, pcl::PointXYZ> words2 = {
{20, pcl::PointXYZ(4, 5, 6)}
};
pcl::PointCloud<pcl::PointXYZ> cloud1, cloud2;
util3d::extractXYZCorrespondences(words1, words2, cloud1, cloud2);
EXPECT_TRUE(cloud1.empty());
EXPECT_TRUE(cloud2.empty());
}
TEST(Util3dCorrespondences, extractXYZCorrespondencesRANSACAcceptsCleanMatches) {
std::multimap<int, pcl::PointXYZ> words1;
std::multimap<int, pcl::PointXYZ> words2;
// 10 consistent matches
for (int i = 0; i < 10; ++i) {
words1.insert({i, pcl::PointXYZ(i * 1.0f, exp2(i)/10.0f, 0.0f)});
words2.insert({i, pcl::PointXYZ(i * 1.0f + 1.1f, exp2(i)/10.0f + 1.1f, 0.0f)}); // Slight noise
}
pcl::PointCloud<pcl::PointXYZ> cloud1, cloud2;
util3d::extractXYZCorrespondencesRANSAC(words1, words2, cloud1, cloud2);
EXPECT_EQ(cloud1.size(), cloud2.size());
EXPECT_GE(cloud1.size(), 8); // At least 8 inliers from 10 consistent matches
}
TEST(Util3dCorrespondences, extractXYZCorrespondencesRANSACRejectsOutliers) {
std::multimap<int, pcl::PointXYZ> words1;
std::multimap<int, pcl::PointXYZ> words2;
// 8 inliers
for (int i = 0; i < 8; ++i) {
words1.insert({i, pcl::PointXYZ(i * 1.0f, exp2(i)/10.0f, 0.0f)});
words2.insert({i, pcl::PointXYZ(i * 1.0f + 1.1f, exp2(i)/10.0f + 1.1f, 0.0f)}); // Slight noise
}
// 2 outliers
words1.insert({100, pcl::PointXYZ(0.0f, 0.0f, 0.0f)});
words2.insert({100, pcl::PointXYZ(100.0f, 100.0f, 0.0f)});
words1.insert({101, pcl::PointXYZ(1.0f, 1.0f, 0.0f)});
words2.insert({101, pcl::PointXYZ(200.0f, -50.0f, 0.0f)});
pcl::PointCloud<pcl::PointXYZ> cloud1, cloud2;
util3d::extractXYZCorrespondencesRANSAC(words1, words2, cloud1, cloud2);
EXPECT_EQ(cloud1.size(), cloud2.size());
EXPECT_EQ(cloud1.size(), 8); // RANSAC should reject 2 outliers
}
TEST(Util3dCorrespondences, extractXYZCorrespondencesRANSACFailsGracefullyOnTooFewMatches) {
std::multimap<int, pcl::PointXYZ> words1 = {
{1, pcl::PointXYZ(0, 0, 0)},
{2, pcl::PointXYZ(1, 1, 1)},
{3, pcl::PointXYZ(2, 2, 2)}
};
std::multimap<int, pcl::PointXYZ> words2 = {
{1, pcl::PointXYZ(0.1f, 0.1f, 0)},
{2, pcl::PointXYZ(1.1f, 1.1f, 1)},
{3, pcl::PointXYZ(2.1f, 2.1f, 2)}
};
pcl::PointCloud<pcl::PointXYZ> cloud1, cloud2;
util3d::extractXYZCorrespondencesRANSAC(words1, words2, cloud1, cloud2);
EXPECT_TRUE(cloud1.empty());
EXPECT_TRUE(cloud2.empty());
}
TEST(Util3dCorrespondences, extractXYZCorrespondencesValidCorrespondencesAreExtracted) {
// Create simple 5x5 depth images with valid depth
cv::Mat depth1 = cv::Mat::ones(5, 5, CV_32FC1) * 1.0f;
cv::Mat depth2 = cv::Mat::ones(5, 5, CV_32FC1) * 1.5f;
std::list<std::pair<cv::Point2f, cv::Point2f>> matches = {
{cv::Point2f(2, 2), cv::Point2f(2, 2)},
{cv::Point2f(1, 1), cv::Point2f(1, 1)},
{cv::Point2f(3, 3), cv::Point2f(3, 3)}
};
pcl::PointCloud<pcl::PointXYZ> cloud1, cloud2;
float fx = 1.0f, fy = 1.0f, cx = 2.0f, cy = 2.0f;
util3d::extractXYZCorrespondences(matches, depth1, depth2, cx, cy, fx, fy, 2.0f, cloud1, cloud2);
ASSERT_EQ(cloud1.size(), 3);
ASSERT_EQ(cloud2.size(), 3);
// Check one known point
EXPECT_FLOAT_EQ(cloud1[0].z, 1.0f);
EXPECT_FLOAT_EQ(cloud2[0].z, 1.5f);
}
TEST(Util3dCorrespondences, extractXYZCorrespondencesFiltersInvalidDepth) {
cv::Mat depth1 = cv::Mat::ones(5, 5, CV_32FC1) * 1.0f;
cv::Mat depth2 = cv::Mat::ones(5, 5, CV_32FC1) * 1.5f;
depth1.at<float>(2, 2) = 0.0f; // Invalid
depth2.at<float>(1, 1) = std::numeric_limits<float>::quiet_NaN(); // Invalid
std::list<std::pair<cv::Point2f, cv::Point2f>> matches = {
{cv::Point2f(2, 2), cv::Point2f(2, 2)},
{cv::Point2f(1, 1), cv::Point2f(1, 1)},
{cv::Point2f(3, 3), cv::Point2f(3, 3)}
};
pcl::PointCloud<pcl::PointXYZ> cloud1, cloud2;
util3d::extractXYZCorrespondences(matches, depth1, depth2, 2.0f, 2.0f, 1.0f, 1.0f, 2.0f, cloud1, cloud2);
// Only the third match should remain
ASSERT_EQ(cloud1.size(), 1);
ASSERT_EQ(cloud2.size(), 1);
EXPECT_FLOAT_EQ(cloud1[0].z, 1.0f);
EXPECT_FLOAT_EQ(cloud2[0].z, 1.5f);
}
TEST(Util3dCorrespondences, extractXYZCorrespondencesRespectsMaxDepthConstraint) {
cv::Mat depth1 = cv::Mat::ones(5, 5, CV_32FC1) * 3.0f; // Exceeds maxDepth
cv::Mat depth2 = cv::Mat::ones(5, 5, CV_32FC1) * 1.0f;
std::list<std::pair<cv::Point2f, cv::Point2f>> matches = {
{cv::Point2f(2, 2), cv::Point2f(2, 2)},
{cv::Point2f(1, 1), cv::Point2f(1, 1)}
};
pcl::PointCloud<pcl::PointXYZ> cloud1, cloud2;
util3d::extractXYZCorrespondences(matches, depth1, depth2, 2.0f, 2.0f, 1.0f, 1.0f, 2.5f, cloud1, cloud2);
// All points should be rejected due to depth1 being too large
EXPECT_TRUE(cloud1.empty());
EXPECT_TRUE(cloud2.empty());
}
TEST(Util3dCorrespondences, extractXYZCorrespondencesOrgCloudsValidCorrespondencesAreExtracted) {
int width = 5, height = 5;
// Create two organized point clouds
pcl::PointCloud<pcl::PointXYZ> cloud1, cloud2;
cloud1.width = cloud2.width = width;
cloud1.height = cloud2.height = height;
cloud1.is_dense = cloud2.is_dense = false;
cloud1.points.resize(width * height);
cloud2.points.resize(width * height);
// Fill the clouds with some values
for (int v = 0; v < height; ++v) {
for (int u = 0; u < width; ++u) {
int idx = v * width + u;
cloud1.at(idx).x = u;
cloud1.at(idx).y = v;
cloud1.at(idx).z = 1.0f;
cloud2.at(idx).x = u + 0.5f;
cloud2.at(idx).y = v + 0.5f;
cloud2.at(idx).z = 1.5f;
}
}
// Set correspondences to valid pixel positions
std::list<std::pair<cv::Point2f, cv::Point2f>> correspondences = {
{cv::Point2f(1, 1), cv::Point2f(1, 1)},
{cv::Point2f(2, 2), cv::Point2f(2, 2)},
{cv::Point2f(3, 3), cv::Point2f(3, 3)}
};
pcl::PointCloud<pcl::PointXYZ> inliers1, inliers2;
util3d::extractXYZCorrespondences(correspondences, cloud1, cloud2, inliers1, inliers2);
ASSERT_EQ(inliers1.size(), 3);
ASSERT_EQ(inliers2.size(), 3);
EXPECT_FLOAT_EQ(inliers1[0].x, 1);
EXPECT_FLOAT_EQ(inliers1[0].z, 1.0f);
EXPECT_FLOAT_EQ(inliers2[0].x, 1.5f);
EXPECT_FLOAT_EQ(inliers2[0].z, 1.5f);
}
TEST(Util3dCorrespondences, extractXYZCorrespondencesOrgCloudsInvalidPointsAreFilteredOut) {
int width = 3, height = 3;
pcl::PointCloud<pcl::PointXYZ> cloud1, cloud2;
cloud1.width = cloud2.width = width;
cloud1.height = cloud2.height = height;
cloud1.is_dense = cloud2.is_dense = false;
cloud1.points.resize(width * height);
cloud2.points.resize(width * height);
// Set all points to NaN
for (size_t i = 0; i < cloud1.size(); ++i) {
cloud1[i].x = cloud1[i].y = cloud1[i].z = std::numeric_limits<float>::quiet_NaN();
cloud2[i].x = cloud2[i].y = cloud2[i].z = std::numeric_limits<float>::quiet_NaN();
}
// Set one valid point at (1,1)
int idx = 1 * width + 1;
cloud1[idx].x = 1.0f;
cloud1[idx].y = 1.0f;
cloud1[idx].z = 1.0f;
cloud2[idx].x = 2.0f;
cloud2[idx].y = 2.0f;
cloud2[idx].z = 2.0f;
std::list<std::pair<cv::Point2f, cv::Point2f>> correspondences = {
{cv::Point2f(0, 0), cv::Point2f(0, 0)}, // Invalid
{cv::Point2f(1, 1), cv::Point2f(1, 1)}, // Valid
{cv::Point2f(2, 2), cv::Point2f(2, 2)} // Invalid
};
pcl::PointCloud<pcl::PointXYZ> inliers1, inliers2;
util3d::extractXYZCorrespondences(correspondences, cloud1, cloud2, inliers1, inliers2);
ASSERT_EQ(inliers1.size(), 1);
ASSERT_EQ(inliers2.size(), 1);
EXPECT_FLOAT_EQ(inliers1[0].x, 1.0f);
EXPECT_FLOAT_EQ(inliers2[0].x, 2.0f);
}
TEST(Util3dCorrespondences, countUniquePairsNoPairs) {
std::multimap<int, pcl::PointXYZ> wordsA, wordsB;
wordsA.insert({1, pcl::PointXYZ(1, 2, 3)});
wordsB.insert({2, pcl::PointXYZ(1, 2, 3)}); // No overlapping key
EXPECT_EQ(util3d::countUniquePairs(wordsA, wordsB), 0);
}
TEST(Util3dCorrespondences, countUniquePairsOneUniquePair) {
std::multimap<int, pcl::PointXYZ> wordsA, wordsB;
wordsA.insert({1, pcl::PointXYZ(1, 1, 1)});
wordsB.insert({1, pcl::PointXYZ(2, 2, 2)}); // One unique pair
EXPECT_EQ(util3d::countUniquePairs(wordsA, wordsB), 1);
}
TEST(Util3dCorrespondences, countUniquePairsMultipleUniquePairs) {
std::multimap<int, pcl::PointXYZ> wordsA, wordsB;
wordsA.insert({1, pcl::PointXYZ(1, 1, 1)});
wordsA.insert({2, pcl::PointXYZ(2, 2, 2)});
wordsA.insert({3, pcl::PointXYZ(3, 3, 3)});
wordsB.insert({1, pcl::PointXYZ(1, 1, 1)});
wordsB.insert({2, pcl::PointXYZ(2, 2, 2)});
wordsB.insert({3, pcl::PointXYZ(3, 3, 3)});
EXPECT_EQ(util3d::countUniquePairs(wordsA, wordsB), 3);
}
TEST(Util3dCorrespondences, countUniquePairsDuplicatedPointsNotCounted) {
std::multimap<int, pcl::PointXYZ> wordsA, wordsB;
wordsA.insert({1, pcl::PointXYZ(1, 1, 1)});
wordsA.insert({1, pcl::PointXYZ(1.1f, 1.1f, 1.1f)}); // duplicate in A
wordsB.insert({1, pcl::PointXYZ(2, 2, 2)});
EXPECT_EQ(util3d::countUniquePairs(wordsA, wordsB), 0);
wordsA.clear();
wordsB.clear();
wordsA.insert({2, pcl::PointXYZ(1, 1, 1)});
wordsB.insert({2, pcl::PointXYZ(2, 2, 2)});
wordsB.insert({2, pcl::PointXYZ(3, 3, 3)}); // duplicate in B
EXPECT_EQ(util3d::countUniquePairs(wordsA, wordsB), 0);
}
TEST(Util3dCorrespondences, filterMaxDepthFiltersByMaxDepthZ) {
pcl::PointCloud<pcl::PointXYZ> cloud1;
pcl::PointCloud<pcl::PointXYZ> cloud2;
// Add points (some above and some below maxDepth = 5.0)
cloud1.push_back(pcl::PointXYZ(1.0f, 1.0f, 4.0f));
cloud2.push_back(pcl::PointXYZ(1.1f, 1.0f, 4.0f));
cloud1.push_back(pcl::PointXYZ(2.0f, 2.0f, 6.0f)); // exceeds maxDepth
cloud2.push_back(pcl::PointXYZ(2.1f, 2.0f, 6.0f));
cloud1.push_back(pcl::PointXYZ(3.0f, 3.0f, 3.0f));
cloud2.push_back(pcl::PointXYZ(3.1f, 3.0f, 3.0f));
// Filter by maxDepth=5.0 on 'z' axis, no duplicates removal
util3d::filterMaxDepth(cloud1, cloud2, 5.0f, 'z', false);
EXPECT_EQ(cloud1.size(), 2);
EXPECT_EQ(cloud2.size(), 2);
for (const auto& pt : cloud1) {
EXPECT_LT(pt.z, 5.0f);
}
}
TEST(Util3dCorrespondences, filterMaxDepthRemovesDuplicates) {
pcl::PointCloud<pcl::PointXYZ> cloud1;
pcl::PointCloud<pcl::PointXYZ> cloud2;
// Duplicate points in cloud1, but different points in cloud2
cloud1.push_back(pcl::PointXYZ(1.0f, 1.0f, 1.0f));
cloud2.push_back(pcl::PointXYZ(1.1f, 1.0f, 1.0f));
cloud1.push_back(pcl::PointXYZ(1.0f, 1.0f, 1.0f)); // duplicate
cloud2.push_back(pcl::PointXYZ(1.2f, 1.0f, 1.0f));
cloud1.push_back(pcl::PointXYZ(2.0f, 2.0f, 2.0f));
cloud2.push_back(pcl::PointXYZ(2.1f, 2.0f, 2.0f));
// maxDepth large enough to keep all points, removeDuplicates = true
util3d::filterMaxDepth(cloud1, cloud2, 10.0f, 'z', true);
EXPECT_EQ(cloud1.size(), 2);
EXPECT_EQ(cloud2.size(), 2);
// Check that duplicate is removed (only one point with 1.0,1.0,1.0)
int countPoint = 0;
for (const auto& pt : cloud1) {
if (pt.x == 1.0f && pt.y == 1.0f && pt.z == 1.0f) {
countPoint++;
}
}
EXPECT_EQ(countPoint, 1);
}
TEST(Util3dCorrespondences, findCorrespondencesBasicMatching)
{
std::multimap<int, cv::KeyPoint> wordsA, wordsB;
std::list<std::pair<cv::Point2f, cv::Point2f>> pairs;
// Setup wordsA: IDs 1, 2, 3 (only 2 is unique)
wordsA.insert({1, cv::KeyPoint(10.0f, 10.0f, 1)});
wordsA.insert({2, cv::KeyPoint(20.0f, 20.0f, 1)});
wordsA.insert({3, cv::KeyPoint(30.0f, 30.0f, 1)});
wordsA.insert({3, cv::KeyPoint(31.0f, 31.0f, 1)});
// Setup wordsB: IDs 2, 3 (only 2 is unique in both)
wordsB.insert({2, cv::KeyPoint(20.5f, 20.5f, 1)});
wordsB.insert({3, cv::KeyPoint(30.5f, 30.5f, 1)});
wordsB.insert({3, cv::KeyPoint(32.0f, 32.0f, 1)});
util3d::findCorrespondences(wordsA, wordsB, pairs);
ASSERT_EQ(pairs.size(), 1);
EXPECT_EQ(pairs.front().first, cv::Point2f(20.0f, 20.0f));
EXPECT_EQ(pairs.front().second, cv::Point2f(20.5f, 20.5f));
}
TEST(Util3dCorrespondences, findCorrespondencesMatchesWithDepthCheck)
{
std::multimap<int, cv::Point3f> words1, words2;
std::vector<cv::Point3f> inliers1, inliers2;
std::vector<int> correspondences;
// Insert matching and non-matching entries
words1.insert({1, cv::Point3f(1, 1, 1)});
words1.insert({2, cv::Point3f(2, 2, 2)});
words1.insert({3, cv::Point3f(100, 100, 100)}); // out of depth
words2.insert({1, cv::Point3f(1.1f, 1.1f, 1.1f)});
words2.insert({2, cv::Point3f(2.1f, 2.1f, 2.1f)});
words2.insert({3, cv::Point3f(101, 101, 101)}); // out of depth
float maxDepth = 10.0f;
util3d::findCorrespondences(words1, words2, inliers1, inliers2, maxDepth, &correspondences);
ASSERT_EQ(inliers1.size(), 2);
ASSERT_EQ(inliers2.size(), 2);
ASSERT_EQ(correspondences.size(), 2);
EXPECT_EQ(correspondences[0], 1);
EXPECT_EQ(correspondences[1], 2);
}
TEST(Util3dCorrespondences, findCorrespondencesMatchesWithMaxDepth)
{
std::map<int, cv::Point3f> words1, words2;
std::vector<cv::Point3f> inliers1, inliers2;
std::vector<int> correspondences;
words1[1] = cv::Point3f(1, 1, 1);
words1[2] = cv::Point3f(2, 2, 2);
words1[3] = cv::Point3f(100, 100, 100); // Exceeds maxDepth
words2[1] = cv::Point3f(1.2f, 1.2f, 1.2f);
words2[2] = cv::Point3f(2.2f, 2.2f, 2.2f);
words2[3] = cv::Point3f(101, 101, 101); // Exceeds maxDepth
util3d::findCorrespondences(words1, words2, inliers1, inliers2, 10.0f, &correspondences);
ASSERT_EQ(inliers1.size(), 2);
ASSERT_EQ(inliers2.size(), 2);
ASSERT_EQ(correspondences.size(), 2);
EXPECT_EQ(correspondences[0], 1);
EXPECT_EQ(correspondences[1], 2);
}
corelib/test/test_util3d_registration.cpp
-3
View File
@@ -339,7 +339,6 @@ TEST(Util3DRegistration, icpTranslatedTransformConverges)
TEST(Util3DRegistration, icp2DAlignsFlatClouds)
{
pcl::console::setVerbosityLevel(pcl::console::L_DEBUG);
auto cloud_source = std::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
for (float x = 0; x < 5; ++x)
@@ -375,8 +374,6 @@ TEST(Util3DRegistration, icp2DAlignsFlatClouds)
TEST(Util3DRegistration, icpPointToPlaneAlignsTranslatedPlane)
{
// Create a plane point cloud
auto cloud_source_raw = std::make_shared<pcl::PointCloud<pcl::PointXYZ> >();
for (float x = -0.5f; x <= 0.5f; x += 0.1f)