mirror of https://github.com/koide3/small_gicp.git
registration test
This commit is contained in:
k.koide
parent
ddf5f533b2
commit
a7fc74a944
|
|
@ -134,12 +134,15 @@ if(BUILD_TESTS)
|
|||
${PCL_INCLUDE_DIRS}
|
||||
${TBB_INCLUDE_DIRS}
|
||||
${EIGEN3_INCLUDE_DIR}
|
||||
${Iridescence_INCLUDE_DIRS}
|
||||
)
|
||||
target_link_libraries(${TEST_NAME}
|
||||
small_gicp
|
||||
fmt::fmt
|
||||
GTest::gtest_main
|
||||
${PCL_LIBRARIES}
|
||||
${TBB_LIBRARIES}
|
||||
${Iridescence_LIBRARIES}
|
||||
)
|
||||
|
||||
gtest_discover_tests(${TEST_NAME} WORKING_DIRECTORY ${CMAKE_SOURCE_DIR})
|
||||
|
|
|
|||
|
|
@ -0,0 +1,4 @@
|
|||
0.999925 0.0121483 -0.00177009 0.488882
|
||||
-0.0121523 0.999924 -0.00228657 0.121214
|
||||
0.00174218 0.00230791 0.999996 -0.0253342
|
||||
0 0 0 1
|
||||
|
|
@ -26,7 +26,7 @@ struct GaussNewtonOptimizer {
|
|||
const TerminationCriteria& criteria,
|
||||
Reduction& reduction,
|
||||
const Eigen::Isometry3d& init_T,
|
||||
std::vector<Factor>& factors) {
|
||||
std::vector<Factor>& factors) const {
|
||||
//
|
||||
if (verbose) {
|
||||
std::cout << "--- GN optimization ---" << std::endl;
|
||||
|
|
@ -79,7 +79,7 @@ struct LevenbergMarquardtOptimizer {
|
|||
const TerminationCriteria& criteria,
|
||||
Reduction& reduction,
|
||||
const Eigen::Isometry3d& init_T,
|
||||
std::vector<Factor>& factors) {
|
||||
std::vector<Factor>& factors) const {
|
||||
//
|
||||
if (verbose) {
|
||||
std::cout << "--- LM optimization ---" << std::endl;
|
||||
|
|
|
|||
|
|
@ -21,7 +21,7 @@ struct SerialReduction {
|
|||
const TargetTree& target_tree,
|
||||
const CorrespondenceRejector& rejector,
|
||||
const Eigen::Isometry3d& T,
|
||||
std::vector<Factor>& factors) {
|
||||
std::vector<Factor>& factors) const {
|
||||
Eigen::Matrix<double, 6, 6> sum_H = Eigen::Matrix<double, 6, 6>::Zero();
|
||||
Eigen::Matrix<double, 6, 1> sum_b = Eigen::Matrix<double, 6, 1>::Zero();
|
||||
double sum_e = 0.0;
|
||||
|
|
@ -50,7 +50,7 @@ struct SerialReduction {
|
|||
/// @param factors Factors to be evaluated
|
||||
/// @return Sum of the evaluated errors
|
||||
template <typename TargetPointCloud, typename SourcePointCloud, typename Factor>
|
||||
double error(const TargetPointCloud& target, const SourcePointCloud& source, const Eigen::Isometry3d& T, std::vector<Factor>& factors) {
|
||||
double error(const TargetPointCloud& target, const SourcePointCloud& source, const Eigen::Isometry3d& T, std::vector<Factor>& factors) const {
|
||||
double sum_e = 0.0;
|
||||
for (size_t i = 0; i < factors.size(); i++) {
|
||||
sum_e += factors[i].error(target, source, T);
|
||||
|
|
|
|||
|
|
@ -15,7 +15,7 @@ struct ParallelReductionOMP {
|
|||
const TargetTree& target_tree,
|
||||
const CorrespondenceRejector& rejector,
|
||||
const Eigen::Isometry3d& T,
|
||||
std::vector<Factor>& factors) {
|
||||
std::vector<Factor>& factors) const {
|
||||
std::vector<Eigen::Matrix<double, 6, 6>> Hs(num_threads, Eigen::Matrix<double, 6, 6>::Zero());
|
||||
std::vector<Eigen::Matrix<double, 6, 1>> bs(num_threads, Eigen::Matrix<double, 6, 1>::Zero());
|
||||
std::vector<double> es(num_threads, 0.0);
|
||||
|
|
@ -46,7 +46,7 @@ struct ParallelReductionOMP {
|
|||
}
|
||||
|
||||
template <typename TargetPointCloud, typename SourcePointCloud, typename Factor>
|
||||
double error(const TargetPointCloud& target, const SourcePointCloud& source, const Eigen::Isometry3d& T, std::vector<Factor>& factors) {
|
||||
double error(const TargetPointCloud& target, const SourcePointCloud& source, const Eigen::Isometry3d& T, std::vector<Factor>& factors) const {
|
||||
double sum_e = 0.0;
|
||||
|
||||
#pragma omp parallel for num_threads(num_threads) schedule(guided, 8) reduction(+ : sum_e)
|
||||
|
|
|
|||
|
|
@ -119,7 +119,7 @@ struct ParallelReductionTBB {
|
|||
const TargetTree& target_tree,
|
||||
const CorrespondenceRejector& rejector,
|
||||
const Eigen::Isometry3d& T,
|
||||
std::vector<Factor>& factors) {
|
||||
std::vector<Factor>& factors) const {
|
||||
//
|
||||
LinearizeSum<TargetPointCloud, SourcePointCloud, TargetTree, CorrespondenceRejector, Factor> sum(target, source, target_tree, rejector, T, factors);
|
||||
|
||||
|
|
@ -129,7 +129,7 @@ struct ParallelReductionTBB {
|
|||
}
|
||||
|
||||
template <typename TargetPointCloud, typename SourcePointCloud, typename Factor>
|
||||
double error(const TargetPointCloud& target, const SourcePointCloud& source, const Eigen::Isometry3d& T, std::vector<Factor>& factors) {
|
||||
double error(const TargetPointCloud& target, const SourcePointCloud& source, const Eigen::Isometry3d& T, std::vector<Factor>& factors) const {
|
||||
ErrorSum<TargetPointCloud, SourcePointCloud, Factor> sum(target, source, T, factors);
|
||||
tbb::parallel_reduce(tbb::blocked_range<size_t>(0, factors.size(), 16), sum);
|
||||
return sum.e;
|
||||
|
|
|
|||
|
|
@ -22,7 +22,7 @@ public:
|
|||
/// @return Registration result
|
||||
template <typename TargetPointCloud, typename SourcePointCloud, typename TargetTree>
|
||||
RegistrationResult
|
||||
align(const TargetPointCloud& target, const SourcePointCloud& source, const TargetTree& target_tree, const Eigen::Isometry3d& init_T = Eigen::Isometry3d::Identity()) {
|
||||
align(const TargetPointCloud& target, const SourcePointCloud& source, const TargetTree& target_tree, const Eigen::Isometry3d& init_T = Eigen::Isometry3d::Identity()) const {
|
||||
std::vector<Factor> factors(traits::size(source));
|
||||
return optimizer.optimize(target, source, target_tree, rejector, criteria, reduction, init_T, factors);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -0,0 +1,236 @@
|
|||
#include <fmt/format.h>
|
||||
#include <gtest/gtest.h>
|
||||
#include <pcl/point_types.h>
|
||||
#include <pcl/point_cloud.h>
|
||||
#include <small_gicp/util/downsampling.hpp>
|
||||
#include <small_gicp/util/normal_estimation_omp.hpp>
|
||||
#include <small_gicp/points/pcl_point.hpp>
|
||||
#include <small_gicp/points/pcl_point_traits.hpp>
|
||||
#include <small_gicp/points/point_cloud.hpp>
|
||||
|
||||
#include <small_gicp/factors/icp_factor.hpp>
|
||||
#include <small_gicp/factors/gicp_factor.hpp>
|
||||
#include <small_gicp/factors/plane_icp_factor.hpp>
|
||||
#include <small_gicp/registration/reduction_omp.hpp>
|
||||
#include <small_gicp/registration/reduction_tbb.hpp>
|
||||
#include <small_gicp/registration/registration.hpp>
|
||||
|
||||
#include <small_gicp/benchmark/read_points.hpp>
|
||||
#include <small_gicp/registration/registration_helper.hpp>
|
||||
|
||||
#include <guik/viewer/light_viewer.hpp>
|
||||
|
||||
using namespace small_gicp;
|
||||
|
||||
class RegistrationTest : public testing::Test, public testing::WithParamInterface<std::tuple<const char*, const char*>> {
|
||||
public:
|
||||
void SetUp() override {
|
||||
// Load points
|
||||
const double downsampling_resolution = 0.3;
|
||||
|
||||
target = std::make_shared<PointCloud>(read_ply("data/target.ply"));
|
||||
target = voxelgrid_sampling(*target, downsampling_resolution);
|
||||
estimate_normals_covariances_omp(*target);
|
||||
|
||||
target_pcl = pcl::make_shared<pcl::PointCloud<pcl::PointNormalCovariance>>();
|
||||
target_pcl->resize(target->size());
|
||||
for (size_t i = 0; i < target->size(); i++) {
|
||||
target_pcl->at(i).getVector4fMap() = target->point(i).cast<float>();
|
||||
}
|
||||
estimate_normals_covariances_omp(*target_pcl);
|
||||
|
||||
source = std::make_shared<PointCloud>(read_ply("data/source.ply"));
|
||||
source = voxelgrid_sampling(*source, downsampling_resolution);
|
||||
estimate_normals_covariances_omp(*source);
|
||||
|
||||
source_pcl = pcl::make_shared<pcl::PointCloud<pcl::PointNormalCovariance>>();
|
||||
source_pcl->resize(source->size());
|
||||
for (size_t i = 0; i < source->size(); i++) {
|
||||
source_pcl->at(i).getVector4fMap() = source->point(i).cast<float>();
|
||||
}
|
||||
estimate_normals_covariances_omp(*source_pcl);
|
||||
|
||||
target_tree = std::make_shared<KdTree<PointCloud>>(target);
|
||||
source_tree = std::make_shared<KdTree<PointCloud>>(source);
|
||||
target_voxelmap = std::make_shared<GaussianVoxelMap>(1.0);
|
||||
target_voxelmap->insert(*target);
|
||||
source_voxelmap = std::make_shared<GaussianVoxelMap>(1.0);
|
||||
source_voxelmap->insert(*source);
|
||||
|
||||
std::mt19937 mt;
|
||||
std::uniform_real_distribution<> udist(-1.0, 1.0);
|
||||
|
||||
const int num_noise_poses = 4;
|
||||
T_noise.resize(num_noise_poses);
|
||||
for (int i = 0; i < num_noise_poses; i++) {
|
||||
T_noise[i] = Eigen::Isometry3d::Identity();
|
||||
if (i != 0) {
|
||||
T_noise[i].translation() = Eigen::Vector3d(udist(mt), udist(mt), udist(mt)) * 0.5;
|
||||
T_noise[i].linear() = Eigen::AngleAxisd(udist(mt) * 10.0 * M_PI / 180.0, Eigen::Vector3d(udist(mt), udist(mt), udist(mt)).normalized()).toRotationMatrix();
|
||||
}
|
||||
}
|
||||
|
||||
const int num_shifted_poses = 2;
|
||||
T_source_shifted.resize(num_shifted_poses);
|
||||
shifted.resize(num_shifted_poses);
|
||||
for (int i = 0; i < num_shifted_poses; i++) {
|
||||
T_source_shifted[i] = Eigen::Isometry3d::Identity();
|
||||
T_source_shifted[i].translation() = Eigen::Vector3d(udist(mt), udist(mt), udist(mt)) * 2.0;
|
||||
T_source_shifted[i].linear() = Eigen::AngleAxisd(udist(mt) * M_PI_2 + M_PI_2, Eigen::Vector3d(udist(mt), udist(mt), udist(mt)).normalized()).toRotationMatrix();
|
||||
|
||||
const Eigen::Isometry3d T_shifted_source = T_source_shifted[i].inverse();
|
||||
|
||||
shifted[i] = std::make_shared<PointCloud>(source->points);
|
||||
std::ranges::transform(source->points, shifted[i]->points.begin(), [&](const auto& p) -> Eigen::Vector4d { return T_shifted_source * p; });
|
||||
estimate_normals_covariances_omp(*shifted[i]);
|
||||
}
|
||||
|
||||
std::ifstream ifs("data/T_target_source.txt");
|
||||
if (!ifs) {
|
||||
std::cerr << "error: failed to open T_target_source.txt" << std::endl;
|
||||
}
|
||||
for (int i = 0; i < 4; i++) {
|
||||
for (int j = 0; j < 4; j++) {
|
||||
ifs >> T_target_source(i, j);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
template <typename Registration>
|
||||
void test_registration(Registration& registration) {
|
||||
for (int i = 0; i < T_noise.size(); i++) {
|
||||
auto result = registration.align(*target, *source, *target_tree, T_noise[i]);
|
||||
EXPECT_TRUE(compare_transformation(T_target_source, result.T_target_source)) << "Forward transformation T_noise=" << i;
|
||||
|
||||
result = registration.align(*target_pcl, *source_pcl, *target_tree, T_noise[i]);
|
||||
EXPECT_TRUE(compare_transformation(T_target_source, result.T_target_source)) << "Forward transformation (pcl) T_noise=" << i;
|
||||
|
||||
result = registration.align(*source, *target, *source_tree, T_noise[i]);
|
||||
EXPECT_TRUE(compare_transformation(T_target_source.inverse(), result.T_target_source)) << "Inverse transformation T_noise=" << i;
|
||||
|
||||
result = registration.align(*source_pcl, *target_pcl, *source_tree, T_noise[i]);
|
||||
EXPECT_TRUE(compare_transformation(T_target_source.inverse(), result.T_target_source)) << "Inverse transformation (pcl) T_noise=" << i;
|
||||
|
||||
for (int j = 0; j < T_source_shifted.size(); j++) {
|
||||
const Eigen::Isometry3d T_target_shifted = T_target_source * T_source_shifted[j];
|
||||
auto result = registration.align(*target, *shifted[j], *target_tree, T_source_shifted[j] * T_noise[i]);
|
||||
EXPECT_TRUE(compare_transformation(T_target_shifted, result.T_target_source)) << "Forward transformation T_source_shifted=" << j << " T_noise=" << i;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
template <typename Registration>
|
||||
void test_registration_vgicp(Registration& registration) {
|
||||
for (int i = 0; i < T_noise.size(); i++) {
|
||||
auto result = registration.align(*target_voxelmap, *source, *target_voxelmap, T_noise[i]);
|
||||
EXPECT_TRUE(compare_transformation(T_target_source, result.T_target_source)) << "Forward transformation T_noise=" << i;
|
||||
|
||||
result = registration.align(*source_voxelmap, *target, *source_voxelmap, T_noise[i]);
|
||||
EXPECT_TRUE(compare_transformation(T_target_source.inverse(), result.T_target_source)) << "Inverse transformation T_noise=" << i;
|
||||
|
||||
for (int j = 0; j < T_source_shifted.size(); j++) {
|
||||
const Eigen::Isometry3d T_target_shifted = T_target_source * T_source_shifted[j];
|
||||
auto result = registration.align(*target_voxelmap, *shifted[j], *target_voxelmap, T_source_shifted[j] * T_noise[i]);
|
||||
EXPECT_TRUE(compare_transformation(T_target_shifted, result.T_target_source)) << "Forward transformation T_source_shifted=" << j << " T_noise=" << i;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bool compare_transformation(const Eigen::Isometry3d& T1, const Eigen::Isometry3d& T2) {
|
||||
const Eigen::Isometry3d e = T1.inverse() * T2;
|
||||
const double error_rot = Eigen::AngleAxisd(e.linear()).angle();
|
||||
const double error_trans = e.translation().norm();
|
||||
|
||||
const double rot_tol = 2.5 * M_PI / 180.0;
|
||||
const double trans_tol = 0.2;
|
||||
|
||||
EXPECT_NEAR(error_rot, 0.0, rot_tol);
|
||||
EXPECT_NEAR(error_trans, 0.0, trans_tol);
|
||||
|
||||
return error_rot < rot_tol && error_trans < trans_tol;
|
||||
}
|
||||
|
||||
protected:
|
||||
PointCloud::Ptr target; ///< Target points
|
||||
PointCloud::Ptr source; ///< Source points
|
||||
pcl::PointCloud<pcl::PointNormalCovariance>::Ptr target_pcl; ///< Target points (pcl)
|
||||
pcl::PointCloud<pcl::PointNormalCovariance>::Ptr source_pcl; ///< Source points (pcl)
|
||||
|
||||
KdTree<PointCloud>::Ptr target_tree; ///< Nearest neighbor search for the target points
|
||||
KdTree<PointCloud>::Ptr source_tree; ///< Nearest neighbor search for the source points
|
||||
GaussianVoxelMap::Ptr target_voxelmap; ///< Gaussian voxel map for the target points
|
||||
GaussianVoxelMap::Ptr source_voxelmap; ///< Gaussian voxel map for the target points
|
||||
|
||||
Eigen::Isometry3d T_target_source; ///< Ground truth transformation
|
||||
|
||||
std::vector<Eigen::Isometry3d> T_noise;
|
||||
std::vector<Eigen::Isometry3d> T_source_shifted;
|
||||
std::vector<PointCloud::Ptr> shifted;
|
||||
};
|
||||
|
||||
// Load check
|
||||
TEST_F(RegistrationTest, LoadCheck) {
|
||||
EXPECT_FALSE(target->empty());
|
||||
EXPECT_FALSE(source->empty());
|
||||
EXPECT_FALSE(target_pcl->empty());
|
||||
EXPECT_FALSE(source_pcl->empty());
|
||||
}
|
||||
|
||||
INSTANTIATE_TEST_SUITE_P(
|
||||
RegistrationTest,
|
||||
RegistrationTest,
|
||||
testing::Combine(testing::Values("ICP", "PLANE_ICP", "GICP", "VGICP"), testing::Values("SERIAL", "TBB", "OMP")),
|
||||
[](const auto& info) { return fmt::format("{}_{}", std::get<0>(info.param), std::get<1>(info.param)); });
|
||||
|
||||
TEST_P(RegistrationTest, EmptyTest) {
|
||||
const std::string factor = std::get<0>(GetParam());
|
||||
const std::string reduction = std::get<1>(GetParam());
|
||||
|
||||
if (factor == "ICP") {
|
||||
if (reduction == "SERIAL") {
|
||||
Registration<ICPFactor, SerialReduction> reg;
|
||||
test_registration(reg);
|
||||
} else if (reduction == "TBB") {
|
||||
Registration<ICPFactor, ParallelReductionTBB> reg;
|
||||
test_registration(reg);
|
||||
} else if (reduction == "OMP") {
|
||||
Registration<ICPFactor, ParallelReductionOMP> reg;
|
||||
test_registration(reg);
|
||||
}
|
||||
} else if (factor == "PLANE_ICP") {
|
||||
if (reduction == "SERIAL") {
|
||||
Registration<PointToPlaneICPFactor, SerialReduction> reg;
|
||||
test_registration(reg);
|
||||
} else if (reduction == "TBB") {
|
||||
Registration<PointToPlaneICPFactor, ParallelReductionTBB> reg;
|
||||
test_registration(reg);
|
||||
} else if (reduction == "OMP") {
|
||||
Registration<PointToPlaneICPFactor, ParallelReductionOMP> reg;
|
||||
test_registration(reg);
|
||||
}
|
||||
} else if (factor == "GICP") {
|
||||
if (reduction == "SERIAL") {
|
||||
Registration<GICPFactor, SerialReduction> reg;
|
||||
test_registration(reg);
|
||||
} else if (reduction == "TBB") {
|
||||
Registration<GICPFactor, ParallelReductionTBB> reg;
|
||||
test_registration(reg);
|
||||
} else if (reduction == "OMP") {
|
||||
Registration<GICPFactor, ParallelReductionOMP> reg;
|
||||
test_registration(reg);
|
||||
}
|
||||
} else if (factor == "VGICP") {
|
||||
if (reduction == "SERIAL") {
|
||||
Registration<GICPFactor, SerialReduction> reg;
|
||||
test_registration_vgicp(reg);
|
||||
} else if (reduction == "TBB") {
|
||||
Registration<GICPFactor, ParallelReductionTBB> reg;
|
||||
test_registration_vgicp(reg);
|
||||
} else if (reduction == "OMP") {
|
||||
Registration<GICPFactor, ParallelReductionOMP> reg;
|
||||
test_registration_vgicp(reg);
|
||||
}
|
||||
} else {
|
||||
std::cerr << "error: unknown factor type " << factor << std::endl;
|
||||
}
|
||||
}
|
||||
Loading…
Reference in New Issue