Skip to content

Commit

Permalink
refactor RegistrationPCL
Browse files Browse the repository at this point in the history
  • Loading branch information
@koide3
koide3 committed Apr 8, 2024
1 parent d09f920 commit f5dcd11
Show file tree
Hide file tree
Showing 4 changed files with 120 additions and 27 deletions.
56 changes: 38 additions & 18 deletions include/small_gicp/pcl/pcl_registration.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,6 +6,7 @@
#include <pcl/registration/registration.h>
#include <small_gicp/ann/kdtree_omp.hpp>
#include <small_gicp/ann/gaussian_voxelmap.hpp>
#include <small_gicp/registration/registration_result.hpp>

namespace small_gicp {

Expand Down Expand Up @@ -43,42 +44,61 @@ class RegistrationPCL : public pcl::Registration<PointSource, PointTarget, float
RegistrationPCL();
virtual ~RegistrationPCL();

void setNumThreads(int n) { num_threads_ = n; }
void setCorrespondenceRandomness(int k) { k_correspondences_ = k; }
void setVoxelResolution(double r) { voxel_resolution_ = r; }
void setRotationEpsilon(double eps) { rotation_epsilon_ = eps; }
/// @brief Set the number of threads to use.
void setNumThreads(int n);
/// @brief Set the number of neighbors for covariance estimation.
/// @note This is equivalent to `setNumNeighborsForCovariance`. Just exists for compatibility with pcl::GICP.
void setCorrespondenceRandomness(int k);
/// @brief Set the number of neighbors for covariance estimation.
void setNumNeighborsForCovariance(int k);
/// @brief Set the voxel resolution for VGICP.
void setVoxelResolution(double r);
/// @brief Set rotation epsilon for convergence check.
void setRotationEpsilon(double eps);
/// @brief Set registration type ("GICP" or "VGICP").
void setRegistrationType(const std::string& type);
/// @brief Set the verbosity flag.
void setVerbosity(bool verbose);

const Eigen::Matrix<double, 6, 6>& getFinalHessian() const { return final_hessian_; }
/// @brief Get the final Hessian matrix ([rx, ry, rz, tx, ty, tz]).
const Eigen::Matrix<double, 6, 6>& getFinalHessian() const;

/// @brief Get the detailed registration result.
const RegistrationResult& getRegistrationResult() const;

/// @brief Set the input source (aligned) point cloud.
void setInputSource(const PointCloudSourceConstPtr& cloud) override;
/// @brief Set the input target (fixed) point cloud.
void setInputTarget(const PointCloudTargetConstPtr& cloud) override;

/// @brief Swap source and target point clouds and their augmented data (KdTrees, covariances, and voxelmaps).
void swapSourceAndTarget();
/// @brief Clear source point cloud.
void clearSource();
/// @brief Clear target point cloud.
void clearTarget();

protected:
virtual void computeTransformation(PointCloudSource& output, const Matrix4& guess) override;

protected:
int num_threads_;
int k_correspondences_;
double rotation_epsilon_;
double voxel_resolution_;
bool verbose_;
std::string registration_type_;
int num_threads_; ///< Number of threads to use.
int k_correspondences_; ///< Number of neighbors for covariance estimation.
double rotation_epsilon_; ///< Rotation epsilon for convergence check.
double voxel_resolution_; ///< Voxel resolution for VGICP.
std::string registration_type_; ///< Registration type ("GICP" or "VGICP").
bool verbose_; ///< Verbosity flag.

std::shared_ptr<KdTreeOMP<pcl::PointCloud<PointSource>>> target_tree_;
std::shared_ptr<KdTreeOMP<pcl::PointCloud<PointSource>>> source_tree_;
std::shared_ptr<KdTreeOMP<pcl::PointCloud<PointSource>>> target_tree_; ///< KdTree for target point cloud.
std::shared_ptr<KdTreeOMP<pcl::PointCloud<PointSource>>> source_tree_; ///< KdTree for source point cloud.

std::shared_ptr<GaussianVoxelMap> target_voxelmap_;
std::shared_ptr<GaussianVoxelMap> source_voxelmap_;
std::shared_ptr<GaussianVoxelMap> target_voxelmap_; ///< VoxelMap for target point cloud.
std::shared_ptr<GaussianVoxelMap> source_voxelmap_; ///< VoxelMap for source point cloud.

std::vector<Eigen::Matrix4d> target_covs_;
std::vector<Eigen::Matrix4d> source_covs_;
std::vector<Eigen::Matrix4d> target_covs_; ///< Covariances of target points
std::vector<Eigen::Matrix4d> source_covs_; ///< Covariances of source points.

Eigen::Matrix<double, 6, 6> final_hessian_;
RegistrationResult result_; ///< Registration result.
};

} // namespace small_gicp
Expand Down
65 changes: 58 additions & 7 deletions include/small_gicp/pcl/pcl_registration_impl.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -33,8 +33,6 @@ RegistrationPCL<PointSource, PointTarget>::RegistrationPCL() {
voxel_resolution_ = 1.0;
verbose_ = false;
registration_type_ = "GICP";

final_hessian_.setIdentity();
}

template <typename PointSource, typename PointTarget>
Expand Down Expand Up @@ -88,6 +86,45 @@ void RegistrationPCL<PointSource, PointTarget>::clearTarget() {
target_voxelmap_.reset();
}

template <typename PointSource, typename PointTarget>
void RegistrationPCL<PointSource, PointTarget>::setNumThreads(int n) {
if (n <= 0) {
PCL_ERROR("[RegistrationPCL::setNumThreads] Invalid number of threads: %d\n", n);
n = 1;
}

num_threads_ = n;
}

template <typename PointSource, typename PointTarget>
void RegistrationPCL<PointSource, PointTarget>::setCorrespondenceRandomness(int k) {
setNumNeighborsForCovariance(k);
}

template <typename PointSource, typename PointTarget>
void RegistrationPCL<PointSource, PointTarget>::setNumNeighborsForCovariance(int k) {
if (k < 5) {
PCL_ERROR("[RegistrationPCL::setNumNeighborsForCovariance] Invalid number of neighbors: %d\n", k);
k = 5;
}
k_correspondences_ = k;
}

template <typename PointSource, typename PointTarget>
void RegistrationPCL<PointSource, PointTarget>::setVoxelResolution(double r) {
if (voxel_resolution_ <= 0) {
PCL_ERROR("[RegistrationPCL::setVoxelResolution] Invalid voxel resolution: %f\n", r);
r = 1.0;
}

voxel_resolution_ = r;
}

template <typename PointSource, typename PointTarget>
void RegistrationPCL<PointSource, PointTarget>::setRotationEpsilon(double eps) {
rotation_epsilon_ = eps;
}

template <typename PointSource, typename PointTarget>
void RegistrationPCL<PointSource, PointTarget>::setRegistrationType(const std::string& type) {
if (type == "GICP") {
Expand All @@ -99,6 +136,21 @@ void RegistrationPCL<PointSource, PointTarget>::setRegistrationType(const std::s
}
}

template <typename PointSource, typename PointTarget>
void RegistrationPCL<PointSource, PointTarget>::setVerbosity(bool verbose) {
verbose_ = verbose;
}

template <typename PointSource, typename PointTarget>
const Eigen::Matrix<double, 6, 6>& RegistrationPCL<PointSource, PointTarget>::getFinalHessian() const {
return result_.H;
}

template <typename PointSource, typename PointTarget>
const RegistrationResult& RegistrationPCL<PointSource, PointTarget>::getRegistrationResult() const {
return result_;
}

template <typename PointSource, typename PointTarget>
void RegistrationPCL<PointSource, PointTarget>::computeTransformation(PointCloudSource& output, const Matrix4& guess) {
if (output.points.data() == input_->points.data() || output.points.data() == target_->points.data()) {
Expand All @@ -123,9 +175,8 @@ void RegistrationPCL<PointSource, PointTarget>::computeTransformation(PointCloud
registration.optimizer.verbose = verbose_;
registration.optimizer.max_iterations = max_iterations_;

RegistrationResult result(Eigen::Isometry3d::Identity());
if (registration_type_ == "GICP") {
result = registration.align(target_proxy, source_proxy, *target_tree_, Eigen::Isometry3d(guess.template cast<double>()));
result_ = registration.align(target_proxy, source_proxy, *target_tree_, Eigen::Isometry3d(guess.template cast<double>()));
} else if (registration_type_ == "VGICP") {
if (!target_voxelmap_) {
target_voxelmap_ = std::make_shared<GaussianVoxelMap>(voxel_resolution_);
Expand All @@ -136,14 +187,14 @@ void RegistrationPCL<PointSource, PointTarget>::computeTransformation(PointCloud
source_voxelmap_->insert(source_proxy);
}

result = registration.align(*target_voxelmap_, source_proxy, *target_voxelmap_, Eigen::Isometry3d(guess.template cast<double>()));
result_ = registration.align(*target_voxelmap_, source_proxy, *target_voxelmap_, Eigen::Isometry3d(guess.template cast<double>()));
} else {
PCL_ERROR("[RegistrationPCL::computeTransformation] Invalid registration type: %s\n", registration_type_.c_str());
return;
}

final_transformation_ = result.T_target_source.matrix().template cast<float>();
final_hessian_ = result.H;
converged_ = result_.converged;
final_transformation_ = result_.T_target_source.matrix().template cast<float>();
pcl::transformPointCloud(*input_, output, final_transformation_);
}

Expand Down
2 changes: 1 addition & 1 deletion include/small_gicp/registration/registration_result.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,7 +9,7 @@ namespace small_gicp {

/// @brief Registration result
struct RegistrationResult {
RegistrationResult(const Eigen::Isometry3d& T)
RegistrationResult(const Eigen::Isometry3d& T = Eigen::Isometry3d::Identity())
: T_target_source(T),
converged(false),
iterations(0),
Expand Down
24 changes: 23 additions & 1 deletion src/test/registration_test.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -178,8 +178,12 @@ TEST_F(RegistrationTest, LoadCheck) {
// PCL interface test
TEST_F(RegistrationTest, PCLInterfaceTest) {
RegistrationPCL<pcl::PointNormalCovariance, pcl::PointNormalCovariance> registration;
registration.setRegistrationType("GICP");
registration.setNumThreads(2);
registration.setCorrespondenceRandomness(20);
registration.setRotationEpsilon(1e-4);
registration.setTransformationEpsilon(1e-4);
registration.setMaxCorrespondenceDistance(1.0);
registration.setRegistrationType("GICP");

// Forward align
registration.setInputTarget(target_pcl);
Expand Down Expand Up @@ -209,6 +213,15 @@ TEST_F(RegistrationTest, PCLInterfaceTest) {
EXPECT_EQ(aligned.size(), source_pcl->size());
EXPECT_TRUE(compare_transformation(T_target_source, Eigen::Isometry3d(registration.getFinalTransformation().cast<double>())));

Eigen::Matrix<double, 6, 6> H = registration.getFinalHessian();
Eigen::SelfAdjointEigenSolver<Eigen::Matrix<double, 6, 6>> eig(H);
EXPECT_GT(eig.eigenvalues()[0], 10.0);
for (int i = 0; i < 6; i++) {
for (int j = i + 1; j < 6; j++) {
EXPECT_NEAR(H(i, j), H(j, i), 1e-3);
}
}

registration.setRegistrationType("VGICP");
registration.setVoxelResolution(1.0);

Expand All @@ -221,6 +234,15 @@ TEST_F(RegistrationTest, PCLInterfaceTest) {
EXPECT_EQ(aligned.size(), source_pcl->size());
EXPECT_TRUE(compare_transformation(T_target_source, Eigen::Isometry3d(registration.getFinalTransformation().cast<double>())));

H = registration.getFinalHessian();
eig.compute(H);
EXPECT_GT(eig.eigenvalues()[0], 10.0);
for (int i = 0; i < 6; i++) {
for (int j = i + 1; j < 6; j++) {
EXPECT_NEAR(H(i, j), H(j, i), 1e-3);
}
}

// Swap and backward align
registration.swapSourceAndTarget();
registration.align(aligned);
Expand Down

0 comments on commit f5dcd11

Please sign in to comment.