mirror of https://github.com/koide3/small_gicp.git
examples
This commit is contained in:
k.koide
parent
b1916f4733
commit
5e983ac3b2
|
|
@ -16,7 +16,8 @@ set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}")
|
|||
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}")
|
||||
|
||||
option(BUILD_HELPER "Build helper library" ON)
|
||||
option(BUILD_TESTS "Build test" OFF)
|
||||
option(BUILD_TESTS "Build tests" OFF)
|
||||
option(BUILD_EXAMPLES "Build examples" OFF)
|
||||
option(BUILD_BENCHMARKS "Build benchmarks" OFF)
|
||||
option(BUILD_WITH_TBB "Build with TBB" ON)
|
||||
option(BUILD_WITH_PCL "Build with PCL (required for benchmark and test only)" OFF)
|
||||
|
|
@ -124,6 +125,37 @@ if(BUILD_BENCHMARKS)
|
|||
endif()
|
||||
endif()
|
||||
|
||||
#############
|
||||
## Example ##
|
||||
#############
|
||||
if(BUILD_EXAMPLES)
|
||||
find_package(fmt REQUIRED)
|
||||
find_package(PCL REQUIRED)
|
||||
find_package(TBB REQUIRED)
|
||||
find_package(Iridescence REQUIRED)
|
||||
|
||||
file(GLOB EXAMPLE_SOURCES "src/example/*.cpp")
|
||||
foreach(EXAMPLE_SOURCE ${EXAMPLE_SOURCES})
|
||||
get_filename_component(EXAMPLE_NAME ${EXAMPLE_SOURCE} NAME_WE)
|
||||
add_executable(${EXAMPLE_NAME} ${EXAMPLE_SOURCE})
|
||||
target_include_directories(${EXAMPLE_NAME} PUBLIC
|
||||
include
|
||||
${PCL_INCLUDE_DIRS}
|
||||
${TBB_INCLUDE_DIRS}
|
||||
${EIGEN3_INCLUDE_DIR}
|
||||
${Iridescence_INCLUDE_DIRS}
|
||||
)
|
||||
target_link_libraries(${EXAMPLE_NAME}
|
||||
small_gicp
|
||||
fmt::fmt
|
||||
${PCL_LIBRARIES}
|
||||
${TBB_LIBRARIES}
|
||||
${Iridescence_LIBRARIES}
|
||||
)
|
||||
endforeach()
|
||||
|
||||
|
||||
endif()
|
||||
|
||||
##########
|
||||
## Test ##
|
||||
|
|
|
|||
|
|
@ -0,0 +1,245 @@
|
|||
# small_gicp (fast_gicp2)
|
||||
|
||||
**small_gicp** is a header-only C++ library that provides efficient and parallelized fine point cloud registration algorithms (ICP, Point-to-Plane ICP, GICP, VGICP, etc.). It is essentially an optimized and refined version of its predecessor, [fast_gicp](https://github.com/SMRT-AIST/fast_gicp), with the following features.
|
||||
|
||||
- **Highly optimized** : The implementation of the core registration algorithm is further optimized from that in fast_gicp. It can provide up to 2x speed up compared to fast_gicp.
|
||||
- **All parallerized** : small_gicp provides parallelized implementations of several algorithms in the point cloud registration process (Downsampling, KdTree construction, Normal/covariance estimation). As a parallelism backend, either (or both) of [OpenMP](https://www.openmp.org/) and [Intel TBB](https://github.com/oneapi-src/oneTBB) can be used.
|
||||
- **Minimum dependency** : small_gicp is a header-only library and requires only [Eigen](https://eigen.tuxfamily.org/) (and bundled [nanoflann](https://github.com/jlblancoc/nanoflann) and [Sophus](https://github.com/strasdat/Sophus)) at a minimum. Optionally, it provides the [PCL](https://pointclouds.org/) registration interface so that it can be used as a drop-in replacement in many systems.
|
||||
- **Customizable** : small_gicp is implemented with the trait mechanism that allows feeding any custom point cloud class to the registration algorithm. Furthermore, the template-based implementation allows customizing the regisration process with your custom correspondence estimator and registration factors.
|
||||
|
||||
[](https://github.com/koide3/small_gicp/actions/workflows/build.yml)
|
||||
|
||||
## Installation
|
||||
|
||||
This is a header-only library. You can just download and put it in your project directory to use all capabilities.
|
||||
|
||||
Meanwhile, if you just want to perform basic point cloud registration without fine customization, you can build and install the helper library (see `small_gicp/registration/registration_helper.hpp` for details) as follows.
|
||||
|
||||
```cpp
|
||||
sudo apt-get install libeigen3-dev libomp-dev
|
||||
|
||||
cd small_gicp
|
||||
mkdir build && cd build
|
||||
cmake .. -DCMAKE_BUILD_TYPE=Release && make -j
|
||||
sudo make install
|
||||
```
|
||||
|
||||
## Usage
|
||||
|
||||
The following examples assume `using namespace small_gicp`.
|
||||
|
||||
### Using helper library ([01_basic_resigtration.cpp](https://github.com/koide3/small_gicp/blob/master/src/example/01_basic_registration.cpp))
|
||||
|
||||
<details><summary>The helper library (`registration_helper.hpp`) enables processing point clouds represented as `std::vector<Eigen::Vector(3|4)(f|d)>` easily. </summary>
|
||||
|
||||
`small_gicp::align` takes two point clouds (`std::vectors` of `Eigen::Vector(3|4)(f|d)`) and returns a registration result (estimated transformation and some information on the optimization result). This is the easiest way to use **small_gicp** but causes an overhead for duplicated preprocessing.
|
||||
|
||||
```cpp
|
||||
#include <small_gicp/registration/registration_helper.hpp>
|
||||
|
||||
std::vector<Eigen::Vector3d> target_points = ...; // Any of Eigen::Vector(3|4)(f|d) can be used
|
||||
std::vector<Eigen::Vector3d> source_points = ...; //
|
||||
|
||||
RegistrationSetting setting;
|
||||
setting.num_threads = 4; // Number of threads to be used
|
||||
setting.downsampling_resolution = 0.25; // Downsampling resolution
|
||||
setting.max_correspondence_distance = 1.0; // Maximum correspondence distance between points (e.g., triming threshold)
|
||||
|
||||
Eigen::Isometry3d init_T_target_source = Eigen::Isometry3d::Identity();
|
||||
RegistrationResult result = align(target_points, source_points, init_T_target_source, setting);
|
||||
|
||||
Eigen::Isometry3d T = result.T_target_source; // Estimated transformation
|
||||
size_t num_inliers = result.num_inliers; // Number of inlier source points
|
||||
Eigen::Matrix<double, 6, 6> H = result.H; // Final Hessian matrix (6x6)
|
||||
```
|
||||
|
||||
There is also a way to perform preprocessing and registration separately. This enables saving the time for preprocessing in case registration is performed several times for a same point cloud (e.g., typical odometry estimation based on scan-to-scan matching).
|
||||
|
||||
```cpp
|
||||
#include <small_gicp/registration/registration_helper.hpp>
|
||||
|
||||
std::vector<Eigen::Vector3d> target_points = ...; // Any of Eigen::Vector(3|4)(f|d) can be used
|
||||
std::vector<Eigen::Vector3d> source_points = ...; //
|
||||
|
||||
int num_threads = 4; // Number of threads to be used
|
||||
double downsampling_resolution = 0.25; // Downsampling resolution
|
||||
int num_neighbors = 10; // Number of neighbor points used for normal and covariance estimation
|
||||
|
||||
// std::pair<PointCloud::Ptr, KdTree<PointCloud>::Ptr>
|
||||
auto [target, target_tree] = preprocess_points(target_points, downsampling_resolution, num_neighbors, num_threads);
|
||||
auto [source, source_tree] = preprocess_points(source_points, downsampling_resolution, num_neighbors, num_threads);
|
||||
|
||||
RegistrationSetting setting;
|
||||
setting.num_threads = num_threads;
|
||||
setting.max_correspondence_distance = 1.0; // Maximum correspondence distance between points (e.g., triming threshold)
|
||||
|
||||
Eigen::Isometry3d init_T_target_source = Eigen::Isometry3d::Identity();
|
||||
RegistrationResult result = align(*target, *source, *target_tree, init_T_target_source, setting);
|
||||
|
||||
Eigen::Isometry3d T = result.T_target_source; // Estimated transformation
|
||||
size_t num_inliers = result.num_inliers; // Number of inlier source points
|
||||
Eigen::Matrix<double, 6, 6> H = result.H; // Final Hessian matrix (6x6)
|
||||
```
|
||||
|
||||
</details>
|
||||
|
||||
### Using with PCL interface ([02_basic_resigtration_pcl.cpp](https://github.com/koide3/small_gicp/blob/master/src/example/02_basic_resigtration_pcl.cpp))
|
||||
|
||||
<details><summary>The PCL interface allows using small_gicp as a drop-in replacement for `pcl::GeneralizedIterativeClosestPoint`. It is also possible to directly feed `pcl::PointCloud` to `small_gicp::Registration`.</summary>
|
||||
|
||||
```cpp
|
||||
#include <small_gicp/pcl/pcl_registration.hpp>
|
||||
|
||||
pcl::PointCloud<pcl::PointXYZ>::Ptr raw_target = ...;
|
||||
pcl::PointCloud<pcl::PointXYZ>::Ptr raw_source = ...;
|
||||
|
||||
// small_gicp::voxelgrid_downsampling can directly operate on pcl::PointCloud.
|
||||
pcl::PointCloud<pcl::PointXYZ>::Ptr target = voxelgrid_sampling_omp(*raw_target, 0.25);
|
||||
pcl::PointCloud<pcl::PointXYZ>::Ptr source = voxelgrid_sampling_omp(*raw_source, 0.25);
|
||||
|
||||
// RegistrationPCL is derived from pcl::Registration and has mostly the same interface as pcl::GeneralizedIterativeClosestPoint.
|
||||
RegistrationPCL<pcl::PointXYZ, pcl::PointXYZ> reg;
|
||||
reg.setNumThreads(4);
|
||||
reg.setCorrespondenceRandomness(20);
|
||||
reg.setMaxCorrespondenceDistance(1.0);
|
||||
reg.setVoxelResolution(1.0);
|
||||
reg.setRegistrationType("VGICP"); // or "GICP" (default = "GICP")
|
||||
|
||||
// Set input point clouds.
|
||||
reg.setInputTarget(target);
|
||||
reg.setInputSource(source);
|
||||
|
||||
// Align point clouds.
|
||||
auto aligned = pcl::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
|
||||
reg.align(*aligned);
|
||||
|
||||
// Swap source and target and align again.
|
||||
// This is useful when you want to re-use preprocessed point clouds for successive registrations (e.g., odometry estimation).
|
||||
reg.swapSourceAndTarget();
|
||||
reg.align(*aligned);
|
||||
```
|
||||
|
||||
It is also possible to directly feed `pcl::PointCloud` to `small_gicp::Registration`. Because all preprocessed data are exposed in this way, you can easily re-use them to obtain the best efficiency.
|
||||
|
||||
```cpp
|
||||
#include <small_gicp/pcl/pcl_registration.hpp>
|
||||
|
||||
pcl::PointCloud<pcl::PointXYZ>::Ptr raw_target = ...;
|
||||
pcl::PointCloud<pcl::PointXYZ>::Ptr raw_source = ...;
|
||||
|
||||
// Downsample points and convert them into pcl::PointCloud<pcl::PointCovariance>.
|
||||
pcl::PointCloud<pcl::PointCovariance>::Ptr target = voxelgrid_sampling_omp<pcl::PointCloud<pcl::PointXYZ>, pcl::PointCloud<pcl::PointCovariance>>(*raw_target, 0.25);
|
||||
pcl::PointCloud<pcl::PointCovariance>::Ptr source = voxelgrid_sampling_omp<pcl::PointCloud<pcl::PointXYZ>, pcl::PointCloud<pcl::PointCovariance>>(*raw_source, 0.25);
|
||||
|
||||
// Estimate covariances of points.
|
||||
const int num_threads = 4;
|
||||
const int num_neighbors = 20;
|
||||
estimate_covariances_omp(*target, num_neighbors, num_threads);
|
||||
estimate_covariances_omp(*source, num_neighbors, num_threads);
|
||||
|
||||
// Create KdTree for target and source.
|
||||
auto target_tree = std::make_shared<KdTreeOMP<pcl::PointCloud<pcl::PointCovariance>>>(target, num_threads);
|
||||
auto source_tree = std::make_shared<KdTreeOMP<pcl::PointCloud<pcl::PointCovariance>>>(source, num_threads);
|
||||
|
||||
Registration<GICPFactor, ParallelReductionOMP> registration;
|
||||
registration.reduction.num_threads = num_threads;
|
||||
registration.rejector.max_dist_sq = 1.0;
|
||||
|
||||
// Align point clouds. Note that the input point clouds are pcl::PointCloud<pcl::PointCovariance>.
|
||||
auto result = registration.align(*target, *source, *target_tree, Eigen::Isometry3d::Identity());
|
||||
```
|
||||
|
||||
</details>
|
||||
|
||||
### Using `small_gicp::Registration` template (`registration.hpp`)
|
||||
|
||||
<details><summary>If you want to fine-control and customize the registration process, use `small_gicp::Registration` template that allows changing the inner algorithms and parameters.</summary>
|
||||
|
||||
```cpp
|
||||
#include <small_gicp/ann/kdtree_omp.hpp>
|
||||
#include <small_gicp/points/point_cloud.hpp>
|
||||
#include <small_gicp/factors/gicp_factor.hpp>
|
||||
#include <small_gicp/util/normal_estimation_omp.hpp>
|
||||
#include <small_gicp/registration/reduction_omp.hpp>
|
||||
#include <small_gicp/registration/registration.hpp>
|
||||
|
||||
std::vector<Eigen::Vector3d> target_points = ...; // Any of Eigen::Vector(3|4)(f|d) can be used
|
||||
std::vector<Eigen::Vector3d> source_points = ...; //
|
||||
|
||||
int num_threads = 4;
|
||||
double downsampling_resolution = 0.25;
|
||||
int num_neighbors = 10;
|
||||
double max_correspondence_distance = 1.0;
|
||||
|
||||
// Convert to small_gicp::PointCloud
|
||||
auto target = std::make_shared<PointCloud>(target_points);
|
||||
auto source = std::make_shared<PointCloud>(source_points);
|
||||
|
||||
// Downsampling
|
||||
target = voxelgrid_downsampling_omp(*target, downsampling_resolution, num_threads);
|
||||
source = voxelgrid_downsampling_omp(*source, downsampling_resolution, num_threads);
|
||||
|
||||
// Create KdTree
|
||||
auto target_tree = std::make_shared<KdTreeOMP<PointCloud>>(target, num_threads);
|
||||
auto source_tree = std::make_shared<KdTreeOMP<PointCloud>>(source, num_threads);
|
||||
|
||||
// Estimate point covariances
|
||||
estimate_covariances_omp(*target, *target_tree, num_neighbors, num_threads);
|
||||
estimate_covariances_omp(*source, *source_tree, num_neighbors, num_threads);
|
||||
|
||||
// GICP + OMP-based parallel reduction
|
||||
Registration<GICPFactor, ParallelReductionOMP> registration;
|
||||
registration.reduction.num_threads = num_threads;
|
||||
registration.rejector.max_dist_sq = max_correspondence_distance * max_correspondence_distance;
|
||||
|
||||
// Align point clouds
|
||||
Eigen::Isometry3d init_T_target_source = Eigen::Isometry3d::Identity();
|
||||
auto result = registration.align(*target, *source, *target_tree, init_T_target_source);
|
||||
|
||||
Eigen::Isometry3d T = result.T_target_source; // Estimated transformation
|
||||
size_t num_inliers = result.num_inliers; // Number of inlier source points
|
||||
Eigen::Matrix<double, 6, 6> H = result.H; // Final Hessian matrix (6x6)
|
||||
```
|
||||
|
||||
Custom registration example:
|
||||
|
||||
```cpp
|
||||
using PerPointFactor = PointToPlaneICPFactor; // Point-to-plane ICP
|
||||
using GeneralFactor = RestrictDoFFactor; // DoF restriction
|
||||
using Reduction = ParallelReductionTBB; // TBB-based parallel reduction
|
||||
using CorrespondenceRejector = DistanceRejector; // Distance-based correspondence rejection
|
||||
using Optimizer = LevenbergMarquardtOptimizer; // Levenberg marquardt optimizer
|
||||
|
||||
Registration<PerPointFactor, Reduction, GeneralFactor, CorrespondenceRejector, Optimizer> registration;
|
||||
registration.general_factor.set_translation_mask(Eigen::Vector3d(1.0, 1.0, 0.0)); // XY-translation only
|
||||
registration.general_factor.set_ratation_mask(Eigen::Vector3d(0.0, 0.0, 1.0)); // Z-rotation only
|
||||
registration.optimizer.init_lambda = 1e-3; // Initial damping scale
|
||||
```
|
||||
|
||||
</details>
|
||||
|
||||
## Benchmark
|
||||
|
||||
### Downsampling
|
||||
|
||||
- Single-threaded `small_gicp::voxelgrid_sampling` is about 1.3x faster than `pcl::VoxelGrid`.
|
||||
- Multi-threaded `small_gicp::voxelgrid_sampling_tbb` (6 threads) is about 3.2x faster than `pcl::VoxelGrid`.
|
||||
- `small_gicp::voxelgrid_sampling` gives accurate downsampling results (almost identical to those of `pcl::VoxelGrid`) while `pcl::ApproximateVoxelGrid` yields spurious points (up to 2x points).
|
||||
- `small_gicp::voxelgrid_sampling` can process a larger point cloud with a fine voxel resolution compared to `pcl::VoxelGrid`.
|
||||
|
||||
|
||||
|
||||
### Odometry estimation
|
||||
|
||||
- Single-threaded `small_gicp::GICP` is about 2.4x and 1.9x faster than `pcl::GICP` and `fast_gicp::GICP`, respectively.
|
||||
- `small_gicp` shows a better scalability to many-threads situations compared to `fast_gicp`.
|
||||
- `small_gicp::GICP` with TBB flow graph shows an excellent multi-thread scalablity but with a latency degradation.
|
||||
|
||||
|
||||
|
||||
## Papers
|
||||
- Kenji Koide, Masashi Yokozuka, Shuji Oishi, and Atsuhiko Banno, Voxelized GICP for Fast and Accurate 3D Point Cloud Registration, ICRA2021
|
||||
|
||||
## Contact
|
||||
|
||||
[Kenji Koide](https://staff.aist.go.jp/k.koide/), National Institute of Advanced Industrial Science and Technology (AIST)
|
||||
Binary file not shown.
|
After Width: | Height: | Size: 212 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 93 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 441 KiB |
|
|
@ -19,7 +19,7 @@ public:
|
|||
bool visualize = false;
|
||||
int num_threads = 4;
|
||||
int num_neighbors = 20;
|
||||
double downsample_resolution = 0.25;
|
||||
double downsampling_resolution = 0.25;
|
||||
double voxel_resolution = 1.0;
|
||||
double max_correspondence_distance = 1.0;
|
||||
};
|
||||
|
|
@ -54,7 +54,7 @@ public:
|
|||
report();
|
||||
}
|
||||
|
||||
auto downsampled = voxelgrid_sampling(*points[i], params.downsample_resolution);
|
||||
auto downsampled = voxelgrid_sampling(*points[i], params.downsampling_resolution);
|
||||
const Eigen::Isometry3d T = estimate(downsampled);
|
||||
traj.emplace_back(T);
|
||||
|
||||
|
|
|
|||
|
|
@ -0,0 +1,35 @@
|
|||
#pragma once
|
||||
|
||||
#include <pcl/point_cloud.h>
|
||||
#include <small_gicp/points/traits.hpp>
|
||||
|
||||
namespace small_gicp {
|
||||
|
||||
template <typename PointT>
|
||||
struct PointCloudProxy {
|
||||
PointCloudProxy(const pcl::PointCloud<PointT>& cloud, std::vector<Eigen::Matrix4d>& covs) : cloud(cloud), covs(covs) {}
|
||||
|
||||
const pcl::PointCloud<PointT>& cloud;
|
||||
std::vector<Eigen::Matrix4d>& covs;
|
||||
};
|
||||
|
||||
namespace traits {
|
||||
template <typename PointT>
|
||||
struct Traits<PointCloudProxy<PointT>> {
|
||||
using Points = PointCloudProxy<PointT>;
|
||||
|
||||
static size_t size(const Points& points) { return points.cloud.size(); }
|
||||
|
||||
static bool has_points(const Points& points) { return !points.cloud.points.empty(); }
|
||||
static bool has_covs(const Points& points) { return !points.covs.empty(); }
|
||||
|
||||
static const Eigen::Vector4d point(const Points& points, size_t i) { return points.cloud.at(i).getVector4fMap().template cast<double>(); }
|
||||
static const Eigen::Matrix4d& cov(const Points& points, size_t i) { return points.covs[i]; }
|
||||
|
||||
static void resize(Points& points, size_t n) { points.covs.resize(n); }
|
||||
static void set_cov(Points& points, size_t i, const Eigen::Matrix4d& cov) { points.covs[i] = cov; }
|
||||
};
|
||||
|
||||
} // namespace traits
|
||||
|
||||
} // namespace small_gicp
|
||||
|
|
@ -0,0 +1,84 @@
|
|||
#pragma once
|
||||
|
||||
#include <pcl/point_cloud.h>
|
||||
#include <pcl/registration/registration.h>
|
||||
#include <small_gicp/ann/kdtree_omp.hpp>
|
||||
#include <small_gicp/ann/gaussian_voxelmap.hpp>
|
||||
|
||||
namespace small_gicp {
|
||||
|
||||
template <typename PointSource, typename PointTarget>
|
||||
class RegistrationPCL : public pcl::Registration<PointSource, PointTarget, float> {
|
||||
public:
|
||||
using Scalar = float;
|
||||
using Matrix4 = typename pcl::Registration<PointSource, PointTarget, Scalar>::Matrix4;
|
||||
|
||||
using PointCloudSource = typename pcl::Registration<PointSource, PointTarget, Scalar>::PointCloudSource;
|
||||
using PointCloudSourcePtr = typename PointCloudSource::Ptr;
|
||||
using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr;
|
||||
|
||||
using PointCloudTarget = typename pcl::Registration<PointSource, PointTarget, Scalar>::PointCloudTarget;
|
||||
using PointCloudTargetPtr = typename PointCloudTarget::Ptr;
|
||||
using PointCloudTargetConstPtr = typename PointCloudTarget::ConstPtr;
|
||||
|
||||
using Ptr = pcl::shared_ptr<RegistrationPCL<PointSource, PointTarget>>;
|
||||
using ConstPtr = pcl::shared_ptr<const RegistrationPCL<PointSource, PointTarget>>;
|
||||
|
||||
protected:
|
||||
using pcl::Registration<PointSource, PointTarget, Scalar>::reg_name_;
|
||||
using pcl::Registration<PointSource, PointTarget, Scalar>::input_;
|
||||
using pcl::Registration<PointSource, PointTarget, Scalar>::target_;
|
||||
using pcl::Registration<PointSource, PointTarget, Scalar>::corr_dist_threshold_;
|
||||
|
||||
using pcl::Registration<PointSource, PointTarget, Scalar>::nr_iterations_;
|
||||
using pcl::Registration<PointSource, PointTarget, Scalar>::max_iterations_;
|
||||
using pcl::Registration<PointSource, PointTarget, Scalar>::final_transformation_;
|
||||
|
||||
using pcl::Registration<PointSource, PointTarget, Scalar>::transformation_epsilon_;
|
||||
using pcl::Registration<PointSource, PointTarget, Scalar>::converged_;
|
||||
|
||||
public:
|
||||
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; }
|
||||
void setRegistrationType(const std::string& type);
|
||||
|
||||
const Eigen::Matrix<double, 6, 6>& getFinalHessian() const { return final_hessian_; }
|
||||
|
||||
void setInputSource(const PointCloudSourceConstPtr& cloud) override;
|
||||
void setInputTarget(const PointCloudTargetConstPtr& cloud) override;
|
||||
|
||||
void swapSourceAndTarget();
|
||||
void clearSource();
|
||||
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_;
|
||||
|
||||
std::shared_ptr<KdTreeOMP<pcl::PointCloud<PointSource>>> target_tree_;
|
||||
std::shared_ptr<KdTreeOMP<pcl::PointCloud<PointSource>>> source_tree_;
|
||||
|
||||
std::shared_ptr<GaussianVoxelMap> target_voxelmap_;
|
||||
std::shared_ptr<GaussianVoxelMap> source_voxelmap_;
|
||||
|
||||
std::vector<Eigen::Matrix4d> target_covs_;
|
||||
std::vector<Eigen::Matrix4d> source_covs_;
|
||||
|
||||
Eigen::Matrix<double, 6, 6> final_hessian_;
|
||||
};
|
||||
|
||||
} // namespace small_gicp
|
||||
|
||||
#include <small_gicp/pcl/pcl_registration_impl.hpp>
|
||||
|
|
@ -0,0 +1,140 @@
|
|||
#pragma once
|
||||
|
||||
#include <small_gicp/pcl/pcl_registration.hpp>
|
||||
|
||||
#include <small_gicp/pcl/pcl_proxy.hpp>
|
||||
#include <small_gicp/pcl/pcl_point_traits.hpp>
|
||||
#include <small_gicp/util/normal_estimation_omp.hpp>
|
||||
|
||||
#include <small_gicp/factors/gicp_factor.hpp>
|
||||
#include <small_gicp/registration/reduction_omp.hpp>
|
||||
#include <small_gicp/registration/registration.hpp>
|
||||
|
||||
namespace small_gicp {
|
||||
|
||||
template <typename PointSource, typename PointTarget>
|
||||
RegistrationPCL<PointSource, PointTarget>::RegistrationPCL() {
|
||||
reg_name_ = "RegistrationPCL";
|
||||
|
||||
num_threads_ = 4;
|
||||
k_correspondences_ = 20;
|
||||
corr_dist_threshold_ = 1000.0;
|
||||
rotation_epsilon_ = 2e-3;
|
||||
transformation_epsilon_ = 5e-4;
|
||||
|
||||
voxel_resolution_ = 1.0;
|
||||
verbose_ = false;
|
||||
registration_type_ = "GICP";
|
||||
|
||||
final_hessian_.setIdentity();
|
||||
}
|
||||
|
||||
template <typename PointSource, typename PointTarget>
|
||||
RegistrationPCL<PointSource, PointTarget>::~RegistrationPCL() {}
|
||||
|
||||
template <typename PointSource, typename PointTarget>
|
||||
void RegistrationPCL<PointSource, PointTarget>::setInputSource(const PointCloudSourceConstPtr& cloud) {
|
||||
if (input_ == cloud) {
|
||||
return;
|
||||
}
|
||||
|
||||
pcl::Registration<PointSource, PointTarget, Scalar>::setInputSource(cloud);
|
||||
source_tree_ = std::make_shared<KdTreeOMP<pcl::PointCloud<PointSource>>>(input_, num_threads_);
|
||||
source_covs_.clear();
|
||||
source_voxelmap_.reset();
|
||||
}
|
||||
|
||||
template <typename PointSource, typename PointTarget>
|
||||
void RegistrationPCL<PointSource, PointTarget>::setInputTarget(const PointCloudTargetConstPtr& cloud) {
|
||||
if (target_ == cloud) {
|
||||
return;
|
||||
}
|
||||
|
||||
pcl::Registration<PointSource, PointTarget, Scalar>::setInputTarget(cloud);
|
||||
target_tree_ = std::make_shared<KdTreeOMP<pcl::PointCloud<PointTarget>>>(target_, num_threads_);
|
||||
target_covs_.clear();
|
||||
target_voxelmap_.reset();
|
||||
}
|
||||
|
||||
template <typename PointSource, typename PointTarget>
|
||||
void RegistrationPCL<PointSource, PointTarget>::swapSourceAndTarget() {
|
||||
input_.swap(target_);
|
||||
source_tree_.swap(target_tree_);
|
||||
source_covs_.swap(target_covs_);
|
||||
source_voxelmap_.swap(target_voxelmap_);
|
||||
}
|
||||
|
||||
template <typename PointSource, typename PointTarget>
|
||||
void RegistrationPCL<PointSource, PointTarget>::clearSource() {
|
||||
input_.reset();
|
||||
source_tree_.reset();
|
||||
source_covs_.clear();
|
||||
source_voxelmap_.reset();
|
||||
}
|
||||
|
||||
template <typename PointSource, typename PointTarget>
|
||||
void RegistrationPCL<PointSource, PointTarget>::clearTarget() {
|
||||
target_.reset();
|
||||
target_tree_.reset();
|
||||
target_covs_.clear();
|
||||
target_voxelmap_.reset();
|
||||
}
|
||||
|
||||
template <typename PointSource, typename PointTarget>
|
||||
void RegistrationPCL<PointSource, PointTarget>::setRegistrationType(const std::string& type) {
|
||||
if (type == "GICP") {
|
||||
registration_type_ = type;
|
||||
} else if (type == "VGICP") {
|
||||
registration_type_ = type;
|
||||
} else {
|
||||
PCL_ERROR("[RegistrationPCL::setRegistrationType] Invalid registration type: %s\n", type.c_str());
|
||||
}
|
||||
}
|
||||
|
||||
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()) {
|
||||
throw std::invalid_argument("FastGICP: destination cloud cannot be identical to source or target");
|
||||
}
|
||||
|
||||
PointCloudProxy<PointSource> source_proxy(*input_, source_covs_);
|
||||
PointCloudProxy<PointTarget> target_proxy(*target_, target_covs_);
|
||||
|
||||
if (source_covs_.size() != input_->size()) {
|
||||
estimate_covariances_omp(source_proxy, *source_tree_, k_correspondences_, num_threads_);
|
||||
}
|
||||
if (target_covs_.size() != target_->size()) {
|
||||
estimate_covariances_omp(target_proxy, *target_tree_, k_correspondences_, num_threads_);
|
||||
}
|
||||
|
||||
Registration<GICPFactor, ParallelReductionOMP> registration;
|
||||
registration.reduction.num_threads = num_threads_;
|
||||
registration.rejector.max_dist_sq = corr_dist_threshold_ * corr_dist_threshold_;
|
||||
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>()));
|
||||
} else if (registration_type_ == "VGICP") {
|
||||
if (!target_voxelmap_) {
|
||||
target_voxelmap_ = std::make_shared<GaussianVoxelMap>(voxel_resolution_);
|
||||
target_voxelmap_->insert(target_proxy);
|
||||
}
|
||||
if (!source_voxelmap_) {
|
||||
source_voxelmap_ = std::make_shared<GaussianVoxelMap>(voxel_resolution_);
|
||||
source_voxelmap_->insert(source_proxy);
|
||||
}
|
||||
|
||||
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;
|
||||
pcl::transformPointCloud(*input_, output, final_transformation_);
|
||||
}
|
||||
|
||||
} // namespace small_gicp
|
||||
|
|
@ -9,17 +9,17 @@ namespace small_gicp {
|
|||
|
||||
/// @brief Preprocess point cloud (downsampling, kdtree creation, and normal and covariance estimation)
|
||||
/// @param points Input points
|
||||
/// @param downsample_resolution Downsample resolution
|
||||
/// @param downsampling_resolution Downsample resolution
|
||||
/// @param num_neighbors Number of neighbors for normal/covariance estimation
|
||||
/// @param num_threads Number of threads
|
||||
std::pair<PointCloud::Ptr, std::shared_ptr<KdTree<PointCloud>>>
|
||||
preprocess_points(const PointCloud& points, double downsample_resolution, int num_neighbors = 10, int num_threads = 4);
|
||||
preprocess_points(const PointCloud& points, double downsampling_resolution, int num_neighbors = 10, int num_threads = 4);
|
||||
|
||||
/// @brief Preprocess point cloud (downsampling, kdtree creation, and normal and covariance estimation)
|
||||
/// @note This function only accepts Eigen::Vector(3|4)(f|d) as input
|
||||
template <typename T, int D>
|
||||
std::pair<PointCloud::Ptr, std::shared_ptr<KdTree<PointCloud>>>
|
||||
preprocess_points(const std::vector<Eigen::Matrix<T, D, 1>>& points, double downsample_resolution, int num_neighbors = 10, int num_threads = 4);
|
||||
preprocess_points(const std::vector<Eigen::Matrix<T, D, 1>>& points, double downsampling_resolution, int num_neighbors = 10, int num_threads = 4);
|
||||
|
||||
/// @brief Create Gaussian voxel map
|
||||
/// @param points Input points
|
||||
|
|
@ -32,7 +32,7 @@ struct RegistrationSetting {
|
|||
|
||||
RegistrationType type = GICP; ///< Registration type
|
||||
double voxel_resolution = 1.0; ///< Voxel resolution for VGICP
|
||||
double downsample_resolution = 0.25; ///< Downsample resolution (this will be used only in the Eigen-based interface)
|
||||
double downsampling_resolution = 0.25; ///< Downsample resolution (this will be used only in the Eigen-based interface)
|
||||
double max_correspondence_distance = 1.0; ///< Maximum correspondence distance
|
||||
int num_threads = 4; ///< Number of threads
|
||||
};
|
||||
|
|
|
|||
|
|
@ -94,6 +94,8 @@ def main():
|
|||
axes[0].set_ylabel('Processing time [msec/scan]')
|
||||
axes[2].legend(loc='upper center', bbox_to_anchor=(0.5, -0.11), ncol=3)
|
||||
|
||||
fig.savefig('downsampling_threads.svg')
|
||||
|
||||
|
||||
fig, axes = pyplot.subplots(1, 2)
|
||||
fig.set_size_inches(18, 3)
|
||||
|
|
@ -116,6 +118,8 @@ def main():
|
|||
axes[0].grid()
|
||||
axes[1].grid()
|
||||
|
||||
fig.savefig('downsampling_comp.svg')
|
||||
|
||||
pyplot.show()
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -82,6 +82,7 @@ def main():
|
|||
axes[i, j].legend()
|
||||
axes[i, j].grid()
|
||||
|
||||
fig.savefig('odometry_time.svg')
|
||||
pyplot.show()
|
||||
|
||||
if __name__ == "__main__":
|
||||
|
|
|
|||
|
|
@ -0,0 +1,68 @@
|
|||
/// @brief Basic point cloud registration example with small_gicp::align()
|
||||
#include <iostream>
|
||||
#include <small_gicp/benchmark/read_points.hpp>
|
||||
#include <small_gicp/registration/registration_helper.hpp>
|
||||
|
||||
using namespace small_gicp;
|
||||
|
||||
/// @brief Most basic registration example.
|
||||
void example1(const std::vector<Eigen::Vector4f>& target_points, const std::vector<Eigen::Vector4f>& source_points) {
|
||||
RegistrationSetting setting;
|
||||
setting.num_threads = 4; // Number of threads to be used
|
||||
setting.downsampling_resolution = 0.25; // Downsampling resolution
|
||||
setting.max_correspondence_distance = 1.0; // Maximum correspondence distance between points (e.g., triming threshold)
|
||||
|
||||
Eigen::Isometry3d init_T_target_source = Eigen::Isometry3d::Identity();
|
||||
RegistrationResult result = align(target_points, source_points, init_T_target_source, setting);
|
||||
|
||||
std::cout << "--- T_target_source ---" << std::endl << result.T_target_source.matrix() << std::endl;
|
||||
std::cout << "converged:" << result.converged << std::endl;
|
||||
std::cout << "error:" << result.error << std::endl;
|
||||
std::cout << "iterations:" << result.iterations << std::endl;
|
||||
std::cout << "num_inliers:" << result.num_inliers << std::endl;
|
||||
std::cout << "--- H ---" << std::endl << result.H << std::endl;
|
||||
std::cout << "--- b ---" << std::endl << result.b.transpose() << std::endl;
|
||||
}
|
||||
|
||||
/// @brief Example to perform preprocessing and registration separately.
|
||||
void example2(const std::vector<Eigen::Vector4f>& target_points, const std::vector<Eigen::Vector4f>& source_points) {
|
||||
int num_threads = 4; // Number of threads to be used
|
||||
double downsampling_resolution = 0.25; // Downsampling resolution
|
||||
int num_neighbors = 10; // Number of neighbor points used for normal and covariance estimation
|
||||
|
||||
// std::pair<PointCloud::Ptr, KdTree<PointCloud>::Ptr>
|
||||
auto [target, target_tree] = preprocess_points(target_points, downsampling_resolution, num_neighbors, num_threads);
|
||||
auto [source, source_tree] = preprocess_points(source_points, downsampling_resolution, num_neighbors, num_threads);
|
||||
|
||||
RegistrationSetting setting;
|
||||
setting.num_threads = num_threads;
|
||||
setting.max_correspondence_distance = 1.0; // Maximum correspondence distance between points (e.g., triming threshold)
|
||||
|
||||
Eigen::Isometry3d init_T_target_source = Eigen::Isometry3d::Identity();
|
||||
RegistrationResult result = align(*target, *source, *target_tree, init_T_target_source, setting);
|
||||
|
||||
std::cout << "--- T_target_source ---" << std::endl << result.T_target_source.matrix() << std::endl;
|
||||
std::cout << "converged:" << result.converged << std::endl;
|
||||
std::cout << "error:" << result.error << std::endl;
|
||||
std::cout << "iterations:" << result.iterations << std::endl;
|
||||
std::cout << "num_inliers:" << result.num_inliers << std::endl;
|
||||
std::cout << "--- H ---" << std::endl << result.H << std::endl;
|
||||
std::cout << "--- b ---" << std::endl << result.b.transpose() << std::endl;
|
||||
|
||||
// Preprocessed points and trees can be reused for the next registration for efficiency
|
||||
RegistrationResult result2 = align(*source, *target, *source_tree, Eigen::Isometry3d::Identity(), setting);
|
||||
}
|
||||
|
||||
int main(int argc, char** argv) {
|
||||
std::vector<Eigen::Vector4f> target_points = read_ply("data/target.ply");
|
||||
std::vector<Eigen::Vector4f> source_points = read_ply("data/source.ply");
|
||||
if (target_points.empty() || source_points.empty()) {
|
||||
std::cerr << "error: failed to read points from data/(target|source).ply" << std::endl;
|
||||
return 1;
|
||||
}
|
||||
|
||||
example1(target_points, source_points);
|
||||
example2(target_points, source_points);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
|
@ -0,0 +1,107 @@
|
|||
/// @brief Basic point cloud registration example with PCL interfaces
|
||||
#include <small_gicp/pcl/pcl_point.hpp>
|
||||
#include <small_gicp/pcl/pcl_point_traits.hpp>
|
||||
#include <small_gicp/pcl/pcl_registration.hpp>
|
||||
#include <small_gicp/util/downsampling_omp.hpp>
|
||||
#include <small_gicp/benchmark/read_points.hpp>
|
||||
|
||||
#include <glk/pointcloud_buffer_pcl.hpp>
|
||||
#include <guik/viewer/light_viewer.hpp>
|
||||
|
||||
using namespace small_gicp;
|
||||
|
||||
/// @brief Example of using RegistrationPCL that can be used as a drop-in replacement for pcl::GeneralizedIterativeClosestPoint.
|
||||
void example1(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr& raw_target, const pcl::PointCloud<pcl::PointXYZ>::ConstPtr& raw_source) {
|
||||
// small_gicp::voxelgrid_downsampling can directly operate on pcl::PointCloud.
|
||||
pcl::PointCloud<pcl::PointXYZ>::Ptr target = voxelgrid_sampling_omp(*raw_target, 0.25);
|
||||
pcl::PointCloud<pcl::PointXYZ>::Ptr source = voxelgrid_sampling_omp(*raw_source, 0.25);
|
||||
|
||||
// RegistrationPCL is derived from pcl::Registration and has mostly the same interface as pcl::GeneralizedIterativeClosestPoint.
|
||||
RegistrationPCL<pcl::PointXYZ, pcl::PointXYZ> reg;
|
||||
reg.setNumThreads(4);
|
||||
reg.setCorrespondenceRandomness(20);
|
||||
reg.setMaxCorrespondenceDistance(1.0);
|
||||
reg.setVoxelResolution(1.0);
|
||||
reg.setRegistrationType("VGICP"); // or "GICP" (default = "GICP")
|
||||
|
||||
// Set input point clouds.
|
||||
reg.setInputTarget(target);
|
||||
reg.setInputSource(source);
|
||||
|
||||
// Align point clouds.
|
||||
auto aligned = pcl::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
|
||||
reg.align(*aligned);
|
||||
|
||||
std::cout << "--- T_target_source ---" << std::endl << reg.getFinalTransformation() << std::endl;
|
||||
std::cout << "--- H ---" << std::endl << reg.getFinalHessian() << std::endl;
|
||||
|
||||
// Swap source and target and align again.
|
||||
// This is useful when you want to re-use preprocessed point clouds for successive registrations (e.g., odometry estimation).
|
||||
reg.swapSourceAndTarget();
|
||||
reg.align(*aligned);
|
||||
|
||||
std::cout << "--- T_target_source ---" << std::endl << reg.getFinalTransformation().inverse() << std::endl;
|
||||
}
|
||||
|
||||
/// @brief Example to directly feed pcl::PointCloud<pcl::PointCovariance> to small_gicp::Registration.
|
||||
void example2(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr& raw_target, const pcl::PointCloud<pcl::PointXYZ>::ConstPtr& raw_source) {
|
||||
// Downsample points and convert them into pcl::PointCloud<pcl::PointCovariance>.
|
||||
pcl::PointCloud<pcl::PointCovariance>::Ptr target = voxelgrid_sampling_omp<pcl::PointCloud<pcl::PointXYZ>, pcl::PointCloud<pcl::PointCovariance>>(*raw_target, 0.25);
|
||||
pcl::PointCloud<pcl::PointCovariance>::Ptr source = voxelgrid_sampling_omp<pcl::PointCloud<pcl::PointXYZ>, pcl::PointCloud<pcl::PointCovariance>>(*raw_source, 0.25);
|
||||
|
||||
// Estimate covariances of points.
|
||||
const int num_threads = 4;
|
||||
const int num_neighbors = 20;
|
||||
estimate_covariances_omp(*target, num_neighbors, num_threads);
|
||||
estimate_covariances_omp(*source, num_neighbors, num_threads);
|
||||
|
||||
// Create KdTree for target and source.
|
||||
auto target_tree = std::make_shared<KdTreeOMP<pcl::PointCloud<pcl::PointCovariance>>>(target, num_threads);
|
||||
auto source_tree = std::make_shared<KdTreeOMP<pcl::PointCloud<pcl::PointCovariance>>>(source, num_threads);
|
||||
|
||||
Registration<GICPFactor, ParallelReductionOMP> registration;
|
||||
registration.reduction.num_threads = num_threads;
|
||||
registration.rejector.max_dist_sq = 1.0;
|
||||
|
||||
// Align point clouds. Note that the input point clouds are pcl::PointCloud<pcl::PointCovariance>.
|
||||
auto result = registration.align(*target, *source, *target_tree, Eigen::Isometry3d::Identity());
|
||||
|
||||
std::cout << "--- T_target_source ---" << std::endl << result.T_target_source.matrix() << std::endl;
|
||||
std::cout << "converged:" << result.converged << std::endl;
|
||||
std::cout << "error:" << result.error << std::endl;
|
||||
std::cout << "iterations:" << result.iterations << std::endl;
|
||||
std::cout << "num_inliers:" << result.num_inliers << std::endl;
|
||||
std::cout << "--- H ---" << std::endl << result.H << std::endl;
|
||||
std::cout << "--- b ---" << std::endl << result.b.transpose() << std::endl;
|
||||
|
||||
// Because this usage exposes all preprocessed data, you can easily re-use them to obtain the best efficiency.
|
||||
auto result2 = registration.align(*source, *target, *source_tree, Eigen::Isometry3d::Identity());
|
||||
|
||||
std::cout << "--- T_target_source ---" << std::endl << result2.T_target_source.inverse().matrix() << std::endl;
|
||||
}
|
||||
|
||||
int main(int argc, char** argv) {
|
||||
std::vector<Eigen::Vector4f> target_points = read_ply("data/target.ply");
|
||||
std::vector<Eigen::Vector4f> source_points = read_ply("data/source.ply");
|
||||
if (target_points.empty() || source_points.empty()) {
|
||||
std::cerr << "error: failed to read points from data/(target|source).ply" << std::endl;
|
||||
return 1;
|
||||
}
|
||||
|
||||
const auto convert_to_pcl = [](const std::vector<Eigen::Vector4f>& raw_points) {
|
||||
auto points = pcl::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
|
||||
points->resize(raw_points.size());
|
||||
for (size_t i = 0; i < raw_points.size(); i++) {
|
||||
points->at(i).getVector4fMap() = raw_points[i];
|
||||
}
|
||||
return points;
|
||||
};
|
||||
|
||||
pcl::PointCloud<pcl::PointXYZ>::Ptr raw_target = convert_to_pcl(target_points);
|
||||
pcl::PointCloud<pcl::PointXYZ>::Ptr raw_source = convert_to_pcl(source_points);
|
||||
|
||||
example1(raw_target, raw_source);
|
||||
example2(raw_target, raw_source);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
|
@ -43,7 +43,7 @@ int main(int argc, char** argv) {
|
|||
} else if (arg == "--num_neighbors") {
|
||||
params.num_neighbors = std::stoi(argv[i + 1]);
|
||||
} else if (arg == "--downsampling_resolution") {
|
||||
params.downsample_resolution = std::stod(argv[i + 1]);
|
||||
params.downsampling_resolution = std::stod(argv[i + 1]);
|
||||
} else if (arg == "--voxel_resolution") {
|
||||
params.voxel_resolution = std::stod(argv[i + 1]);
|
||||
} else if (arg == "--engine") {
|
||||
|
|
@ -59,7 +59,7 @@ int main(int argc, char** argv) {
|
|||
std::cout << "registration_engine=" << engine << std::endl;
|
||||
std::cout << "num_threads=" << params.num_threads << std::endl;
|
||||
std::cout << "num_neighbors=" << params.num_neighbors << std::endl;
|
||||
std::cout << "downsampling_resolution=" << params.downsample_resolution << std::endl;
|
||||
std::cout << "downsampling_resolution=" << params.downsampling_resolution << std::endl;
|
||||
std::cout << "voxel_resolution=" << params.voxel_resolution << std::endl;
|
||||
std::cout << "visualize=" << params.visualize << std::endl;
|
||||
|
||||
|
|
|
|||
|
|
@ -46,7 +46,7 @@ public:
|
|||
tbb::flow::broadcast_node<InputFrame::Ptr> input_node(graph);
|
||||
tbb::flow::function_node<InputFrame::Ptr, InputFrame::Ptr> preprocess_node(graph, tbb::flow::unlimited, [&](const InputFrame::Ptr& input) {
|
||||
input->sw.start();
|
||||
input->points = voxelgrid_sampling(*input->points, params.downsample_resolution);
|
||||
input->points = voxelgrid_sampling(*input->points, params.downsampling_resolution);
|
||||
input->kdtree = std::make_shared<KdTree<PointCloud>>(input->points);
|
||||
estimate_covariances(*input->points, *input->kdtree, params.num_neighbors);
|
||||
return input;
|
||||
|
|
|
|||
|
|
@ -17,14 +17,14 @@
|
|||
namespace small_gicp {
|
||||
|
||||
// Preprocess points
|
||||
std::pair<PointCloud::Ptr, std::shared_ptr<KdTree<PointCloud>>> preprocess_points(const PointCloud& points, double downsample_resolution, int num_neighbors, int num_threads) {
|
||||
std::pair<PointCloud::Ptr, std::shared_ptr<KdTree<PointCloud>>> preprocess_points(const PointCloud& points, double downsampling_resolution, int num_neighbors, int num_threads) {
|
||||
if (num_threads == 1) {
|
||||
auto downsampled = voxelgrid_sampling(points, downsample_resolution);
|
||||
auto downsampled = voxelgrid_sampling(points, downsampling_resolution);
|
||||
auto kdtree = std::make_shared<KdTree<PointCloud>>(downsampled);
|
||||
estimate_normals_covariances(*downsampled, *kdtree, num_neighbors);
|
||||
return {downsampled, kdtree};
|
||||
} else {
|
||||
auto downsampled = voxelgrid_sampling_omp(points, downsample_resolution);
|
||||
auto downsampled = voxelgrid_sampling_omp(points, downsampling_resolution);
|
||||
auto kdtree = std::make_shared<KdTree<PointCloud>>(downsampled);
|
||||
estimate_normals_covariances_omp(*downsampled, *kdtree, num_neighbors, num_threads);
|
||||
return {downsampled, kdtree};
|
||||
|
|
@ -34,8 +34,8 @@ std::pair<PointCloud::Ptr, std::shared_ptr<KdTree<PointCloud>>> preprocess_point
|
|||
// Preprocess points with Eigen input
|
||||
template <typename T, int D>
|
||||
std::pair<PointCloud::Ptr, std::shared_ptr<KdTree<PointCloud>>>
|
||||
preprocess_points(const std::vector<Eigen::Matrix<T, D, 1>>& points, double downsample_resolution, int num_neighbors, int num_threads) {
|
||||
return preprocess_points(*std::make_shared<PointCloud>(points), downsample_resolution, num_neighbors, num_threads);
|
||||
preprocess_points(const std::vector<Eigen::Matrix<T, D, 1>>& points, double downsampling_resolution, int num_neighbors, int num_threads) {
|
||||
return preprocess_points(*std::make_shared<PointCloud>(points), downsampling_resolution, num_neighbors, num_threads);
|
||||
}
|
||||
|
||||
// Explicit instantiation
|
||||
|
|
@ -55,8 +55,8 @@ GaussianVoxelMap::Ptr create_gaussian_voxelmap(const PointCloud& points, double
|
|||
template <typename T, int D>
|
||||
RegistrationResult
|
||||
align(const std::vector<Eigen::Matrix<T, D, 1>>& target, const std::vector<Eigen::Matrix<T, D, 1>>& source, const Eigen::Isometry3d& init_T, const RegistrationSetting& setting) {
|
||||
auto [target_points, target_tree] = preprocess_points(*std::make_shared<PointCloud>(target), setting.downsample_resolution, 10, setting.num_threads);
|
||||
auto [source_points, source_tree] = preprocess_points(*std::make_shared<PointCloud>(source), setting.downsample_resolution, 10, setting.num_threads);
|
||||
auto [target_points, target_tree] = preprocess_points(*std::make_shared<PointCloud>(target), setting.downsampling_resolution, 10, setting.num_threads);
|
||||
auto [source_points, source_tree] = preprocess_points(*std::make_shared<PointCloud>(source), setting.downsampling_resolution, 10, setting.num_threads);
|
||||
|
||||
if (setting.type == RegistrationSetting::VGICP) {
|
||||
auto target_voxelmap = create_gaussian_voxelmap(*target_points, setting.voxel_resolution);
|
||||
|
|
|
|||
Loading…
Reference in New Issue