mirror
/
small_gicp
mirror of https://github.com/koide3/small_gicp.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

general factor

This commit is contained in:
k.koide 2024-03-29 11:36:17 +09:00
parent 98f6226fb7
commit 31ad85c2d1
22 changed files with 141 additions and 51 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

75
include/small_gicp/factors/general_factor.hpp Normal file
View File

@ -0,0 +1,75 @@
#pragma once
#include <Eigen/Core>
#include <Eigen/Geometry>
namespace small_gicp {
/// @brief Null factor that gives no constraints.
struct NullFactor {
NullFactor() = default;
/// @brief Update linearized system consisting of linearized per-point factors.
/// @param target Target point cloud
/// @param source Source point cloud
/// @param target_tree Nearest neighbor search for the target point cloud
/// @param T Linearization point
/// @param H [in/out] Linearized precision matrix.
/// @param b [in/out] Linearized information vector.
/// @param e [in/out] Error at the linearization point.
template <typename TargetPointCloud, typename SourcePointCloud, typename TargetTree>
void update_linearized_system(
const TargetPointCloud& target,
const SourcePointCloud& source,
const TargetTree& target_tree,
const Eigen::Isometry3d& T,
Eigen::Matrix<double, 6, 6>* H,
Eigen::Matrix<double, 6, 1>* b,
double* e) const {}
/// @brief Update error consisting of per-point factors.
/// @param target Target point cloud
/// @param source Source point cloud
/// @param T Evaluation point
/// @param e [in/out] Error at the evaluation point.
template <typename TargetPointCloud, typename SourcePointCloud>
void update_error(const TargetPointCloud& target, const SourcePointCloud& source, const Eigen::Isometry3d& T, double* e) const {}
};
/// @brief Factor to restrict the degrees of freedom of optimization (e.g., fixing roll, pitch rotation).
/// @note This factor only enables *soft* constraints. The source point cloud can move to the restricted directions slightly).
struct RestrictDoFFactor {
/// @brief Constructor.
RestrictDoFFactor() {
lambda = 1e9;
mask.setOnes();
}
/// @brief Set rotation mask. (1.0 = active, 0.0 = inactive)
void set_rotation_mask(const Eigen::Array3d& rot_mask) { mask.head<3>() = rot_mask; }
/// @brief Set translation mask. (1.0 = active, 0.0 = inactive)
void set_translation_mask(const Eigen::Array3d& trans_mask) { mask.tail<3>() = trans_mask; }
/// @brief Update linearized system consisting of linearized per-point factors.
template <typename TargetPointCloud, typename SourcePointCloud, typename TargetTree>
void update_linearized_system(
const TargetPointCloud& target,
const SourcePointCloud& source,
const TargetTree& target_tree,
const Eigen::Isometry3d& T,
Eigen::Matrix<double, 6, 6>* H,
Eigen::Matrix<double, 6, 1>* b,
double* e) const {
*H += lambda * (mask - 1.0).abs().matrix().asDiagonal();
}
/// @brief Update error consisting of per-point factors.
template <typename TargetPointCloud, typename SourcePointCloud>
void update_error(const TargetPointCloud& target, const SourcePointCloud& source, const Eigen::Isometry3d& T, double* e) const {}
double lambda; ///< Regularization parameter (Increasing this makes the constraint stronger)
Eigen::Array<double, 6, 1> mask; ///< Mask for restricting DoF (rx, ry, rz, tx, ty, tz)
};
} // namespace small_gicp

2
include/small_gicp/factors/gicp_factor.hpp
View File

@ -8,7 +8,7 @@
namespace small_gicp {
/// @brief GICP (distribution-to-distribution) error factor
/// @brief GICP (distribution-to-distribution) per-point error factor.
struct GICPFactor {
/// @brief Constructor
GICPFactor() : target_index(std::numeric_limits<size_t>::max()), source_index(std::numeric_limits<size_t>::max()), mahalanobis(Eigen::Matrix4d::Zero()) {}

2
include/small_gicp/factors/icp_factor.hpp
View File

@ -8,7 +8,7 @@
namespace small_gicp {
/// @brief Point-to-point error factor
/// @brief Point-to-point per-point error factor.
struct ICPFactor {
ICPFactor() : target_index(std::numeric_limits<size_t>::max()), source_index(std::numeric_limits<size_t>::max()) {}

2
include/small_gicp/factors/plane_icp_factor.hpp
View File

@ -8,7 +8,7 @@
namespace small_gicp {
/// @brief Point-to-plane error factor
/// @brief Point-to-plane per-point error factor.
struct PointToPlaneICPFactor {
PointToPlaneICPFactor() : target_index(std::numeric_limits<size_t>::max()), source_index(std::numeric_limits<size_t>::max()) {}

0
include/small_gicp/points/pcl_point.hpp → include/small_gicp/pcl/pcl_point.hpp
View File

0
include/small_gicp/points/pcl_point_traits.hpp → include/small_gicp/pcl/pcl_point_traits.hpp
View File

24
include/small_gicp/points/point_cloud.hpp
View File

@ -30,13 +30,13 @@ public:
/// @brief Destructor
~PointCloud() {}
/// @brief Number of points
/// @brief Number of points.
size_t size() const { return points.size(); }
/// @brief Check if the point cloud is empty
/// @brief Check if the point cloud is empty.
bool empty() const { return points.empty(); }
/// @brief Resize point/normal/cov buffers
/// @brief Resize point/normal/cov buffers.
/// @param n Number of points
void resize(size_t n) {
points.resize(n);
@ -44,28 +44,28 @@ public:
covs.resize(n);
}
/// @brief Get i-th point
/// @brief Get i-th point.
Eigen::Vector4d& point(size_t i) { return points[i]; }
/// @brief Get i-th normal
/// @brief Get i-th normal.
Eigen::Vector4d& normal(size_t i) { return normals[i]; }
/// @brief Get i-th covariance
/// @brief Get i-th covariance.
Eigen::Matrix4d& cov(size_t i) { return covs[i]; }
/// @brief Get i-th point (const)
/// @brief Get i-th point (const).
const Eigen::Vector4d& point(size_t i) const { return points[i]; }
/// @brief Get i-th normal (const)
/// @brief Get i-th normal (const).
const Eigen::Vector4d& normal(size_t i) const { return normals[i]; }
/// @brief Get i-th covariance (const)
/// @brief Get i-th covariance (const).
const Eigen::Matrix4d& cov(size_t i) const { return covs[i]; }
public:
std::vector<Eigen::Vector4d> points; ///< Point coordinates
std::vector<Eigen::Vector4d> normals; ///< Point normals
std::vector<Eigen::Matrix4d> covs; ///< Point covariances
std::vector<Eigen::Vector4d> points; ///< Point coordinates (x, y, z, 1)
std::vector<Eigen::Vector4d> normals; ///< Point normals (nx, ny, nz, 0)
std::vector<Eigen::Matrix4d> covs; ///< Point covariances (3x3 matrix) + zero padding
};
namespace traits {

20
include/small_gicp/points/traits.hpp
View File

@ -9,43 +9,43 @@ namespace traits {
template <typename T>
struct Traits;
/// @brief Get the number of points
/// @brief Get the number of points.
template <typename T>
size_t size(const T& points) {
return Traits<T>::size(points);
}
/// @brief Check if the point cloud has points
/// @brief Check if the point cloud has points.
template <typename T>
bool has_points(const T& points) {
return Traits<T>::has_points(points);
}
/// @brief Check if the point cloud has normals
/// @brief Check if the point cloud has normals.
template <typename T>
bool has_normals(const T& points) {
return Traits<T>::has_normals(points);
}
/// @brief Check if the point cloud has covariances
/// @brief Check if the point cloud has covariances.
template <typename T>
bool has_covs(const T& points) {
return Traits<T>::has_covs(points);
}
/// @brief Get i-th point
/// @brief Get i-th point. 4D vector is used to take advantage of SIMD intrinsics. The last element must be filled by one (x, y, z, 1).
template <typename T>
auto point(const T& points, size_t i) {
return Traits<T>::point(points, i);
}
/// @brief Get i-th normal
/// @brief Get i-th normal. 4D vector is used to take advantage of SIMD intrinsics. The last element must be filled by zero (nx, ny, nz, 0).
template <typename T>
auto normal(const T& points, size_t i) {
return Traits<T>::normal(points, i);
}
/// @brief Get i-th covariance
/// @brief Get i-th covariance. Only the top-left 3x3 matrix is filled, and the bottom row and the right col must be filled by zero.
template <typename T>
auto cov(const T& points, size_t i) {
return Traits<T>::cov(points, i);
@ -57,19 +57,19 @@ void resize(T& points, size_t n) {
Traits<T>::resize(points, n);
}
/// @brief Set i-th point
/// @brief Set i-th point. (x, y, z, 1)
template <typename T>
void set_point(T& points, size_t i, const Eigen::Vector4d& pt) {
Traits<T>::set_point(points, i, pt);
}
/// @brief Set i-th normal
/// @brief Set i-th normal. (nx, nz, nz, 0)
template <typename T>
void set_normal(T& points, size_t i, const Eigen::Vector4d& pt) {
Traits<T>::set_normal(points, i, pt);
}
/// @brief Set i-th covariance
/// @brief Set i-th covariance. Only the top-left 3x3 matrix should be filled.
template <typename T>
void set_cov(T& points, size_t i, const Eigen::Matrix4d& cov) {
Traits<T>::set_cov(points, i, cov);

21
include/small_gicp/registration/optimizer.hpp
View File

@ -17,7 +17,8 @@ struct GaussNewtonOptimizer {
typename CorrespondenceRejector,
typename TerminationCriteria,
typename Reduction,
typename Factor>
typename Factor,
typename GeneralFactor>
RegistrationResult optimize(
const TargetPointCloud& target,
const SourcePointCloud& source,
@ -26,7 +27,8 @@ struct GaussNewtonOptimizer {
const TerminationCriteria& criteria,
Reduction& reduction,
const Eigen::Isometry3d& init_T,
std::vector<Factor>& factors) const {
std::vector<Factor>& factors,
GeneralFactor& general_factor) const {
//
if (verbose) {
std::cout << "--- GN optimization ---" << std::endl;
@ -34,7 +36,8 @@ struct GaussNewtonOptimizer {
RegistrationResult result(init_T);
for (int i = 0; i < max_iterations && !result.converged; i++) {
const auto [H, b, e] = reduction.linearize(target, source, target_tree, rejector, result.T_target_source, factors);
auto [H, b, e] = reduction.linearize(target, source, target_tree, rejector, result.T_target_source, factors);
general_factor.update_linearized_system(target, source, target_tree, result.T_target_source, &H, &b, &e);
const Eigen::Matrix<double, 6, 1> delta = (H + lambda * Eigen ::Matrix<double, 6, 6>::Identity()).ldlt().solve(-b);
if (verbose) {
@ -70,7 +73,8 @@ struct LevenbergMarquardtOptimizer {
typename CorrespondenceRejector,
typename TerminationCriteria,
typename Reduction,
typename Factor>
typename Factor,
typename GeneralFactor>
RegistrationResult optimize(
const TargetPointCloud& target,
const SourcePointCloud& source,
@ -79,7 +83,8 @@ struct LevenbergMarquardtOptimizer {
const TerminationCriteria& criteria,
Reduction& reduction,
const Eigen::Isometry3d& init_T,
std::vector<Factor>& factors) const {
std::vector<Factor>& factors,
GeneralFactor& general_factor) const {
//
if (verbose) {
std::cout << "--- LM optimization ---" << std::endl;
@ -88,13 +93,15 @@ struct LevenbergMarquardtOptimizer {
double lambda = init_lambda;
RegistrationResult result(init_T);
for (int i = 0; i < max_iterations && !result.converged; i++) {
const auto [H, b, e] = reduction.linearize(target, source, target_tree, rejector, result.T_target_source, factors);
auto [H, b, e] = reduction.linearize(target, source, target_tree, rejector, result.T_target_source, factors);
general_factor.update_linearized_system(target, source, target_tree, result.T_target_source, &H, &b, &e);
bool success = false;
for (int j = 0; j < max_inner_iterations; j++) {
const Eigen::Matrix<double, 6, 1> delta = (H + lambda * Eigen ::Matrix<double, 6, 6>::Identity()).ldlt().solve(-b);
const Eigen::Isometry3d new_T = result.T_target_source * se3_exp(delta);
const double new_e = reduction.error(target, source, new_T, factors);
double new_e = reduction.error(target, source, new_T, factors);
general_factor.update_error(target, source, new_T, &e);
if (verbose) {
std::cout << "iter=" << i << " inner=" << j << " e=" << e << " new_e=" << new_e << " lambda=" << lambda << " dt=" << delta.tail<3>().norm()

1
include/small_gicp/registration/reduction_omp.hpp
View File

@ -5,6 +5,7 @@
namespace small_gicp {
#ifndef _OPENMP
#warning "OpenMP is not available. Parallel reduction will be disabled."
inline int omp_get_thread_num() {
return 0;
}

15
include/small_gicp/registration/registration.hpp
View File

@ -2,6 +2,7 @@
#include <small_gicp/ann/traits.hpp>
#include <small_gicp/points/traits.hpp>
#include <small_gicp/factors/general_factor.hpp>
#include <small_gicp/registration/rejector.hpp>
#include <small_gicp/registration/reduction.hpp>
#include <small_gicp/registration/optimizer.hpp>
@ -10,11 +11,16 @@
namespace small_gicp {
/// @brief Point cloud registration
template <typename Factor, typename Reduction, typename CorrespondenceRejector = DistanceRejector, typename Optimizer = LevenbergMarquardtOptimizer>
/// @brief Point cloud registration.
template <
typename Factor,
typename Reduction,
typename GeneralFactor = NullFactor,
typename CorrespondenceRejector = DistanceRejector,
typename Optimizer = LevenbergMarquardtOptimizer>
struct Registration {
public:
/// @brief Align point clouds
/// @brief Align point clouds.
/// @param target Target point cloud
/// @param source Source point cloud
/// @param target_tree Nearest neighbor search for the target point cloud
@ -24,12 +30,13 @@ public:
RegistrationResult
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);
return optimizer.optimize(target, source, target_tree, rejector, criteria, reduction, init_T, factors, general_factor);
}
public:
TerminationCriteria criteria; ///< Termination criteria
CorrespondenceRejector rejector; ///< Correspondence rejector
GeneralFactor general_factor; ///< General factor
Reduction reduction; ///< Reduction
Optimizer optimizer; ///< Optimizer
};

4
src/downsampling_benchmark.cpp
View File

@ -11,8 +11,8 @@
#include <small_gicp/util/downsampling_tbb.hpp>
#endif
#include <small_gicp/points/point_cloud.hpp>
#include <small_gicp/points/pcl_point.hpp>
#include <small_gicp/points/pcl_point_traits.hpp>
#include <small_gicp/pcl/pcl_point.hpp>
#include <small_gicp/pcl/pcl_point_traits.hpp>
#include <small_gicp/benchmark/benchmark.hpp>
namespace small_gicp {

2
src/odometry_benchmark_small_gicp.cpp
View File

@ -26,7 +26,7 @@ public:
return T;
}
Registration<GICPFactor, SerialReduction, DistanceRejector, LevenbergMarquardtOptimizer> registration;
Registration<GICPFactor, SerialReduction> registration;
registration.rejector.max_dist_sq = params.max_correspondence_distance * params.max_correspondence_distance;
auto result = registration.align(*target_points, *points, *target_tree, Eigen::Isometry3d::Identity());

2
src/odometry_benchmark_small_gicp_omp.cpp
View File

@ -26,7 +26,7 @@ public:
return T;
}
Registration<GICPFactor, ParallelReductionOMP, DistanceRejector, LevenbergMarquardtOptimizer> registration;
Registration<GICPFactor, ParallelReductionOMP> registration;
registration.rejector.max_dist_sq = params.max_correspondence_distance * params.max_correspondence_distance;
registration.reduction.num_threads = params.num_threads;

2
src/odometry_benchmark_small_gicp_tbb.cpp
View File

@ -32,7 +32,7 @@ public:
return T;
}
Registration<GICPFactor, ParallelReductionTBB, DistanceRejector, LevenbergMarquardtOptimizer> registration;
Registration<GICPFactor, ParallelReductionTBB> registration;
registration.rejector.max_dist_sq = params.max_correspondence_distance * params.max_correspondence_distance;
auto result = registration.align(*target_points, *points, *target_tree, Eigen::Isometry3d::Identity());

2
src/odometry_benchmark_small_vgicp_model_tbb.cpp
View File

@ -31,7 +31,7 @@ public:
return T;
}
Registration<GICPFactor, ParallelReductionTBB, DistanceRejector, LevenbergMarquardtOptimizer> registration;
Registration<GICPFactor, ParallelReductionTBB> registration;
auto result = registration.align(*voxelmap, *points, *voxelmap, T);
T = result.T_target_source;

2
src/odometry_benchmark_small_vgicp_omp.cpp
View File

@ -30,7 +30,7 @@ public:
return T;
}
Registration<GICPFactor, ParallelReductionOMP, DistanceRejector, LevenbergMarquardtOptimizer> registration;
Registration<GICPFactor, ParallelReductionOMP> registration;
registration.rejector.max_dist_sq = params.max_correspondence_distance * params.max_correspondence_distance;
auto result = registration.align(*target_voxelmap, *points, *target_voxelmap, Eigen::Isometry3d::Identity());

2
src/odometry_benchmark_small_vgicp_tbb.cpp
View File

@ -34,7 +34,7 @@ public:
return T;
}
Registration<GICPFactor, ParallelReductionTBB, DistanceRejector, LevenbergMarquardtOptimizer> registration;
Registration<GICPFactor, ParallelReductionTBB> registration;
registration.rejector.max_dist_sq = params.max_correspondence_distance * params.max_correspondence_distance;
auto result = registration.align(*target_voxelmap, *points, *target_voxelmap, Eigen::Isometry3d::Identity());

4
src/test/downsampling_test.cpp
View File

@ -4,8 +4,8 @@
#include <pcl/filters/voxel_grid.h>
#include <small_gicp/points/point_cloud.hpp>
#include <small_gicp/points/pcl_point.hpp>
#include <small_gicp/points/pcl_point_traits.hpp>
#include <small_gicp/pcl/pcl_point.hpp>
#include <small_gicp/pcl/pcl_point_traits.hpp>
#include <small_gicp/util/downsampling.hpp>
#include <small_gicp/util/downsampling_omp.hpp>
#include <small_gicp/util/downsampling_tbb.hpp>

2
src/test/kdtree_test.cpp
View File

@ -10,7 +10,7 @@
#include <small_gicp/ann/kdtree_tbb.hpp>
#include <small_gicp/util/downsampling.hpp>
#include <small_gicp/points/point_cloud.hpp>
#include <small_gicp/points/pcl_point_traits.hpp>
#include <small_gicp/pcl/pcl_point_traits.hpp>
#include <small_gicp/benchmark/read_points.hpp>
using namespace small_gicp;

4
src/test/points_test.cpp
View File

@ -3,8 +3,8 @@
#include <random>
#include <ranges>
#include <small_gicp/points/point_cloud.hpp>
#include <small_gicp/points/pcl_point.hpp>
#include <small_gicp/points/pcl_point_traits.hpp>
#include <small_gicp/pcl/pcl_point.hpp>
#include <small_gicp/pcl/pcl_point_traits.hpp>
using namespace small_gicp;

4
src/test/registration_test.cpp
View File

@ -3,8 +3,8 @@
#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/pcl/pcl_point.hpp>
#include <small_gicp/pcl/pcl_point_traits.hpp>
#include <small_gicp/points/point_cloud.hpp>
#include <small_gicp/factors/icp_factor.hpp>