mirror of
https://github.com/introlab/rtabmap.git
synced 2026-10-04 00:57:46 +08:00
519 lines
18 KiB
C++
519 lines
18 KiB
C++
#include "gtest/gtest.h"
|
|
#include "rtabmap/core/util3d.h"
|
|
#include "rtabmap/core/util3d_correspondences.h"
|
|
#include "rtabmap/core/CameraModel.h"
|
|
#include "rtabmap/core/StereoCameraModel.h"
|
|
#include "rtabmap/utilite/UException.h"
|
|
#include "rtabmap/utilite/UConversion.h"
|
|
#include <pcl/io/pcd_io.h>
|
|
|
|
using namespace rtabmap;
|
|
|
|
TEST(Util3dCorrespondencesTest, 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(Util3dCorrespondencesTest, 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(Util3dCorrespondencesTest, 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(Util3dCorrespondencesTest, 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());
|
|
}
|
|
|
|
// Reprojects a fixed non-planar 3D scene in both images of a rectified stereo
|
|
// camera. The two-view geometry must be generic: with a planar scene or a pure
|
|
// image translation, all correspondences are related by a homography and the
|
|
// fundamental matrix is then only defined up to a 1-parameter family
|
|
// (F = [e']x * H for any epipole e'). RANSAC can pick a member of that family
|
|
// which also fits an outlier, making the inlier count depend on floating-point
|
|
// details of the platform and of the OpenCV version. Here the points span a
|
|
// range of depths, so their disparities differ and the geometry is well
|
|
// constrained.
|
|
static void reprojectStereoPair(int index, pcl::PointXYZ & left, pcl::PointXYZ & right)
|
|
{
|
|
static const float points3d[12][3] = {
|
|
{-0.50f, -0.40f, 2.0f}, { 0.40f, -0.30f, 3.5f}, {-0.20f, 0.50f, 2.8f},
|
|
{ 0.60f, 0.20f, 5.0f}, {-0.60f, 0.10f, 4.2f}, { 0.10f, -0.50f, 6.5f},
|
|
{ 0.30f, 0.45f, 3.0f}, {-0.35f, -0.15f, 7.5f}, { 0.50f, -0.05f, 2.2f},
|
|
{-0.10f, 0.30f, 5.8f}, { 0.25f, 0.35f, 4.6f}, {-0.45f, 0.20f, 3.3f}};
|
|
|
|
static const StereoCameraModel model(500.0, 500.0, 320.0, 240.0, 0.12);
|
|
|
|
float uLeft, vLeft, uRight, vRight;
|
|
model.reproject(points3d[index][0], points3d[index][1], points3d[index][2],
|
|
uLeft, vLeft, uRight, vRight);
|
|
|
|
// extractXYZCorrespondencesRANSAC() only uses x and y, as image coordinates
|
|
left = pcl::PointXYZ(uLeft, vLeft, 0.0f);
|
|
right = pcl::PointXYZ(uRight, vRight, 0.0f);
|
|
}
|
|
|
|
TEST(Util3dCorrespondencesTest, ExtractXYZCorrespondencesRANSACAcceptsCleanMatches) {
|
|
std::multimap<int, pcl::PointXYZ> words1;
|
|
std::multimap<int, pcl::PointXYZ> words2;
|
|
|
|
// 12 consistent matches
|
|
for (int i = 0; i < 12; ++i) {
|
|
pcl::PointXYZ left, right;
|
|
reprojectStereoPair(i, left, right);
|
|
words1.insert({i, left});
|
|
words2.insert({i, right});
|
|
}
|
|
|
|
pcl::PointCloud<pcl::PointXYZ> cloud1, cloud2;
|
|
util3d::extractXYZCorrespondencesRANSAC(words1, words2, cloud1, cloud2);
|
|
|
|
EXPECT_EQ(cloud1.size(), cloud2.size());
|
|
EXPECT_EQ(cloud1.size(), 12); // every match is on its epipolar line
|
|
}
|
|
|
|
TEST(Util3dCorrespondencesTest, ExtractXYZCorrespondencesRANSACRejectsOutliers) {
|
|
std::multimap<int, pcl::PointXYZ> words1;
|
|
std::multimap<int, pcl::PointXYZ> words2;
|
|
|
|
// 12 inliers
|
|
for (int i = 0; i < 12; ++i) {
|
|
pcl::PointXYZ left, right;
|
|
reprojectStereoPair(i, left, right);
|
|
words1.insert({i, left});
|
|
words2.insert({i, right});
|
|
}
|
|
|
|
// 3 outliers: correct point in the left image, right point moved far away from
|
|
// the corresponding epipolar line (horizontal on a rectified stereo camera)
|
|
const int outlierSources[3] = {0, 4, 8};
|
|
const float outlierOffsets[3][2] = {{0.0f, 120.0f}, {0.0f, -150.0f}, {40.0f, 90.0f}};
|
|
for (int i = 0; i < 3; ++i) {
|
|
pcl::PointXYZ left, right;
|
|
reprojectStereoPair(outlierSources[i], left, right);
|
|
right.x += outlierOffsets[i][0];
|
|
right.y += outlierOffsets[i][1];
|
|
words1.insert({100+i, left});
|
|
words2.insert({100+i, right});
|
|
}
|
|
|
|
pcl::PointCloud<pcl::PointXYZ> cloud1, cloud2;
|
|
util3d::extractXYZCorrespondencesRANSAC(words1, words2, cloud1, cloud2);
|
|
|
|
EXPECT_EQ(cloud1.size(), cloud2.size());
|
|
EXPECT_EQ(cloud1.size(), 12); // RANSAC should reject the 3 outliers
|
|
|
|
// none of the outliers should have survived
|
|
for (unsigned int i = 0; i < cloud2.size(); ++i) {
|
|
for (int j = 0; j < 3; ++j) {
|
|
pcl::PointXYZ left, right;
|
|
reprojectStereoPair(outlierSources[j], left, right);
|
|
EXPECT_FALSE(cloud2[i].x == right.x + outlierOffsets[j][0] &&
|
|
cloud2[i].y == right.y + outlierOffsets[j][1]);
|
|
}
|
|
}
|
|
}
|
|
|
|
TEST(Util3dCorrespondencesTest, 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(Util3dCorrespondencesTest, 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(Util3dCorrespondencesTest, 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(Util3dCorrespondencesTest, 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(Util3dCorrespondencesTest, 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(Util3dCorrespondencesTest, 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(Util3dCorrespondencesTest, 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(Util3dCorrespondencesTest, 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(Util3dCorrespondencesTest, 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(Util3dCorrespondencesTest, 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(Util3dCorrespondencesTest, 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(Util3dCorrespondencesTest, 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(Util3dCorrespondencesTest, 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(Util3dCorrespondencesTest, 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(Util3dCorrespondencesTest, 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);
|
|
} |