mirror of https://github.com/koide3/small_gicp.git
Pymore (#27)
* fix wrong use of points.size() * enhance python interface * license
This commit is contained in:
koide3
GitHub
parent
c8c0288252
commit
8483660297
|
|
@ -45,6 +45,7 @@ jobs:
|
|||
- name: Test (Python)
|
||||
run: |
|
||||
pytest src/example/basic_registration.py --cov=src --cov-report=xml
|
||||
pytest src/test/python_test.py --cov=src --cov-report=xml
|
||||
|
||||
- name: Upload coverage reports to Codecov
|
||||
uses: codecov/codecov-action@v4.0.1
|
||||
|
|
|
|||
|
|
@ -35,3 +35,4 @@ jobs:
|
|||
cd ..
|
||||
pip install . --user
|
||||
pytest src/example/basic_registration.py
|
||||
pytest src/test/python_test.py
|
||||
|
|
|
|||
|
|
@ -126,7 +126,18 @@ if(BUILD_PYTHON_BINDINGS)
|
|||
find_package(Python COMPONENTS Interpreter Development REQUIRED)
|
||||
find_package(pybind11 CONFIG REQUIRED)
|
||||
|
||||
pybind11_add_module(small_gicp_python src/python/python.cpp)
|
||||
pybind11_add_module(small_gicp_python
|
||||
src/python/pointcloud.cpp
|
||||
src/python/kdtree.cpp
|
||||
src/python/voxelmap.cpp
|
||||
src/python/preprocess.cpp
|
||||
src/python/result.cpp
|
||||
src/python/align.cpp
|
||||
src/python/factors.cpp
|
||||
src/python/misc.cpp
|
||||
|
||||
src/python/python.cpp
|
||||
)
|
||||
set_target_properties(small_gicp_python PROPERTIES OUTPUT_NAME small_gicp)
|
||||
target_link_libraries(small_gicp_python PRIVATE small_gicp)
|
||||
endif()
|
||||
|
|
|
|||
|
|
@ -263,7 +263,7 @@ Example A : Perform registration with numpy arrays
|
|||
# - downsampling_resolution : Downsampling resolution
|
||||
# - max_correspondence_distance : Maximum correspondence distance
|
||||
# - num_threads : Number of threads
|
||||
result = small_gicp.align_points(target_raw_numpy, source_raw_numpy, downsampling_resolution=0.25)
|
||||
result = small_gicp.align(target_raw_numpy, source_raw_numpy, downsampling_resolution=0.25)
|
||||
|
||||
result.T_target_source # Estimated transformation (4x4 numpy array)
|
||||
result.converged # If true, the optimization converged successfully
|
||||
|
|
@ -279,13 +279,13 @@ Example B : Perform preprocessing and registration separately
|
|||
```python
|
||||
# Preprocess point clouds
|
||||
# Arguments
|
||||
# - points_numpy : Nx4 or Nx3 numpy array of the target point cloud
|
||||
# - points : Nx4 or Nx3 numpy array of the target point cloud
|
||||
# Optional arguments
|
||||
# - downsampling_resolution : Downsampling resolution
|
||||
# - num_neighbors : Number of neighbors for normal and covariance estimation
|
||||
# - num_threads : Number of threads
|
||||
target, target_tree = small_gicp.preprocess_points(points_numpy=target_raw_numpy, downsampling_resolution=0.25)
|
||||
source, source_tree = small_gicp.preprocess_points(points_numpy=source_raw_numpy, downsampling_resolution=0.25)
|
||||
target, target_tree = small_gicp.preprocess_points(target_raw_numpy, downsampling_resolution=0.25)
|
||||
source, source_tree = small_gicp.preprocess_points(source_raw_numpy, downsampling_resolution=0.25)
|
||||
|
||||
# `target` and `source` are small_gicp.PointCloud with the following methods
|
||||
target.size() # Number of points
|
||||
|
|
|
|||
|
|
@ -24,6 +24,7 @@ RUN ctest -j$(nproc)
|
|||
WORKDIR /root/small_gicp
|
||||
RUN pip install .
|
||||
RUN pytest src/example/basic_registration.py
|
||||
RUN pytest src/test/python_test.py
|
||||
|
||||
WORKDIR /
|
||||
|
||||
|
|
|
|||
|
|
@ -34,6 +34,7 @@ RUN ctest -j$(nproc)
|
|||
WORKDIR /root/small_gicp
|
||||
RUN pip install .
|
||||
RUN pytest src/example/basic_registration.py
|
||||
RUN pytest src/test/python_test.py
|
||||
|
||||
WORKDIR /
|
||||
|
||||
|
|
|
|||
|
|
@ -114,6 +114,7 @@ public:
|
|||
std::conditional_t<HasCovs, std::vector<Eigen::Matrix4d>, Empty> covs;
|
||||
};
|
||||
|
||||
using FlatContainerPoints = FlatContainer<false, false>;
|
||||
using FlatContainerNormal = FlatContainer<true, false>;
|
||||
using FlatContainerCov = FlatContainer<false, true>;
|
||||
using FlatContainerNormalCov = FlatContainer<true, true>;
|
||||
|
|
|
|||
|
|
@ -0,0 +1,19 @@
|
|||
// SPDX-FileCopyrightText: Copyright 2024 Kenji Koide
|
||||
// SPDX-License-Identifier: MIT
|
||||
#pragma once
|
||||
|
||||
#include <Eigen/Core>
|
||||
#include <small_gicp/points/traits.hpp>
|
||||
|
||||
namespace small_gicp {
|
||||
namespace traits {
|
||||
|
||||
template <>
|
||||
struct Traits<Eigen::MatrixXd> {
|
||||
static size_t size(const Eigen::MatrixXd& points) { return points.rows(); }
|
||||
static bool has_points(const Eigen::MatrixXd& points) { return points.rows(); }
|
||||
static Eigen::Vector4d point(const Eigen::MatrixXd& points, size_t i) { return Eigen::Vector4d(points(i, 0), points(i, 1), points(i, 2), 1.0); }
|
||||
};
|
||||
|
||||
} // namespace traits
|
||||
} // namespace small_gicp
|
||||
|
|
@ -29,7 +29,7 @@ std::shared_ptr<OutputPointCloud> voxelgrid_sampling(const InputPointCloud& poin
|
|||
const size_t coord_bit_mask = (1 << 21) - 1; // Bit mask
|
||||
const int coord_offset = 1 << (coord_bit_size - 1); // Coordinate offset to make values positive
|
||||
|
||||
std::vector<std::pair<std::uint64_t, size_t>> coord_pt(points.size());
|
||||
std::vector<std::pair<std::uint64_t, size_t>> coord_pt(traits::size(points));
|
||||
for (size_t i = 0; i < traits::size(points); i++) {
|
||||
const Eigen::Array4i coord = fast_floor(traits::point(points, i) * inv_leaf_size) + coord_offset;
|
||||
if ((coord < 0).any() || (coord > coord_bit_mask).any()) {
|
||||
|
|
|
|||
|
|
@ -29,7 +29,7 @@ std::shared_ptr<OutputPointCloud> voxelgrid_sampling_omp(const InputPointCloud&
|
|||
const size_t coord_bit_mask = (1 << 21) - 1; // Bit mask
|
||||
const int coord_offset = 1 << (coord_bit_size - 1); // Coordinate offset to make values positive
|
||||
|
||||
std::vector<std::pair<std::uint64_t, size_t>> coord_pt(points.size());
|
||||
std::vector<std::pair<std::uint64_t, size_t>> coord_pt(traits::size(points));
|
||||
#pragma omp parallel for num_threads(num_threads) schedule(guided, 32)
|
||||
for (size_t i = 0; i < traits::size(points); i++) {
|
||||
const Eigen::Array4i coord = fast_floor(traits::point(points, i) * inv_leaf_size) + coord_offset;
|
||||
|
|
|
|||
|
|
@ -29,7 +29,7 @@ std::shared_ptr<OutputPointCloud> voxelgrid_sampling_tbb(const InputPointCloud&
|
|||
const size_t coord_bit_mask = (1 << 21) - 1; // Bit mask
|
||||
const int coord_offset = 1 << (coord_bit_size - 1); // Coordinate offset to make values positive
|
||||
|
||||
std::vector<std::pair<std::uint64_t, size_t>> coord_pt(points.size());
|
||||
std::vector<std::pair<std::uint64_t, size_t>> coord_pt(traits::size(points));
|
||||
tbb::parallel_for(tbb::blocked_range<size_t>(0, traits::size(points), 64), [&](const tbb::blocked_range<size_t>& range) {
|
||||
for (size_t i = range.begin(); i != range.end(); i++) {
|
||||
const Eigen::Array4i coord = fast_floor(traits::point(points, i) * inv_leaf_size) + coord_offset;
|
||||
|
|
|
|||
|
|
@ -20,7 +20,7 @@ def example_numpy1(target_raw_numpy : numpy.ndarray, source_raw_numpy : numpy.nd
|
|||
# - downsampling_resolution : Downsampling resolution
|
||||
# - max_correspondence_distance : Maximum correspondence distance
|
||||
# - num_threads : Number of threads
|
||||
result = small_gicp.align_points(target_raw_numpy, source_raw_numpy, downsampling_resolution=0.25)
|
||||
result = small_gicp.align(target_raw_numpy, source_raw_numpy, downsampling_resolution=0.25)
|
||||
|
||||
return result.T_target_source
|
||||
|
||||
|
|
@ -30,13 +30,13 @@ def example_numpy2(target_raw_numpy : numpy.ndarray, source_raw_numpy : numpy.nd
|
|||
|
||||
# Preprocess point clouds
|
||||
# Arguments
|
||||
# - points_numpy : Nx4 or Nx3 numpy array of the target point cloud
|
||||
# - points : Nx4 or Nx3 numpy array of the target point cloud
|
||||
# Optional arguments
|
||||
# - downsampling_resolution : Downsampling resolution
|
||||
# - num_neighbors : Number of neighbors for normal and covariance estimation
|
||||
# - num_threads : Number of threads
|
||||
target, target_tree = small_gicp.preprocess_points(points_numpy=target_raw_numpy, downsampling_resolution=0.25)
|
||||
source, source_tree = small_gicp.preprocess_points(points_numpy=source_raw_numpy, downsampling_resolution=0.25)
|
||||
target, target_tree = small_gicp.preprocess_points(target_raw_numpy, downsampling_resolution=0.25)
|
||||
source, source_tree = small_gicp.preprocess_points(source_raw_numpy, downsampling_resolution=0.25)
|
||||
|
||||
# Align point clouds
|
||||
# Arguments
|
||||
|
|
|
|||
|
|
@ -0,0 +1,137 @@
|
|||
// SPDX-FileCopyrightText: Copyright 2024 Kenji Koide
|
||||
// SPDX-License-Identifier: MIT
|
||||
#include <iostream>
|
||||
|
||||
#include <pybind11/pybind11.h>
|
||||
#include <pybind11/stl.h>
|
||||
#include <pybind11/numpy.h>
|
||||
#include <pybind11/eigen.h>
|
||||
#include <pybind11/functional.h>
|
||||
|
||||
#include <small_gicp/points/point_cloud.hpp>
|
||||
#include <small_gicp/ann/kdtree_omp.hpp>
|
||||
|
||||
#include <small_gicp/factors/gicp_factor.hpp>
|
||||
#include <small_gicp/registration/reduction_omp.hpp>
|
||||
#include <small_gicp/registration/registration.hpp>
|
||||
#include <small_gicp/registration/registration_helper.hpp>
|
||||
|
||||
namespace py = pybind11;
|
||||
using namespace small_gicp;
|
||||
|
||||
void define_align(py::module& m) {
|
||||
// align
|
||||
m.def(
|
||||
"align",
|
||||
[](
|
||||
const Eigen::MatrixXd& target_points,
|
||||
const Eigen::MatrixXd& source_points,
|
||||
const Eigen::Matrix4d& init_T_target_source,
|
||||
const std::string& registration_type,
|
||||
double voxel_resolution,
|
||||
double downsampling_resolution,
|
||||
double max_corresponding_distance,
|
||||
int num_threads) {
|
||||
if (target_points.cols() != 3 && target_points.cols() != 4) {
|
||||
std::cerr << "target_points must be Nx3 or Nx4" << std::endl;
|
||||
return RegistrationResult(Eigen::Isometry3d::Identity());
|
||||
}
|
||||
if (source_points.cols() != 3 && source_points.cols() != 4) {
|
||||
std::cerr << "source_points must be Nx3 or Nx4" << std::endl;
|
||||
return RegistrationResult(Eigen::Isometry3d::Identity());
|
||||
}
|
||||
|
||||
RegistrationSetting setting;
|
||||
if (registration_type == "ICP") {
|
||||
setting.type = RegistrationSetting::ICP;
|
||||
} else if (registration_type == "PLANE_ICP") {
|
||||
setting.type = RegistrationSetting::PLANE_ICP;
|
||||
} else if (registration_type == "GICP") {
|
||||
setting.type = RegistrationSetting::GICP;
|
||||
} else if (registration_type == "VGICP") {
|
||||
setting.type = RegistrationSetting::VGICP;
|
||||
} else {
|
||||
std::cerr << "invalid registration type" << std::endl;
|
||||
return RegistrationResult(Eigen::Isometry3d::Identity());
|
||||
}
|
||||
|
||||
setting.voxel_resolution = voxel_resolution;
|
||||
setting.downsampling_resolution = downsampling_resolution;
|
||||
setting.max_correspondence_distance = max_corresponding_distance;
|
||||
setting.num_threads = num_threads;
|
||||
|
||||
std::vector<Eigen::Vector4d> target(target_points.rows());
|
||||
if (target_points.cols() == 3) {
|
||||
for (size_t i = 0; i < target_points.rows(); i++) {
|
||||
target[i] << target_points.row(i).transpose(), 1.0;
|
||||
}
|
||||
} else {
|
||||
for (size_t i = 0; i < target_points.rows(); i++) {
|
||||
target[i] << target_points.row(i).transpose();
|
||||
}
|
||||
}
|
||||
|
||||
std::vector<Eigen::Vector4d> source(source_points.rows());
|
||||
if (source_points.cols() == 3) {
|
||||
for (size_t i = 0; i < source_points.rows(); i++) {
|
||||
source[i] << source_points.row(i).transpose(), 1.0;
|
||||
}
|
||||
} else {
|
||||
for (size_t i = 0; i < source_points.rows(); i++) {
|
||||
source[i] << source_points.row(i).transpose();
|
||||
}
|
||||
}
|
||||
|
||||
return align(target, source, Eigen::Isometry3d(init_T_target_source), setting);
|
||||
},
|
||||
py::arg("target_points"),
|
||||
py::arg("source_points"),
|
||||
py::arg("init_T_target_source") = Eigen::Matrix4d::Identity(),
|
||||
py::arg("registration_type") = "GICP",
|
||||
py::arg("voxel_resolution") = 1.0,
|
||||
py::arg("downsampling_resolution") = 0.25,
|
||||
py::arg("max_corresponding_distance") = 1.0,
|
||||
py::arg("num_threads") = 1);
|
||||
|
||||
// align
|
||||
m.def(
|
||||
"align",
|
||||
[](
|
||||
const PointCloud::ConstPtr& target,
|
||||
const PointCloud::ConstPtr& source,
|
||||
KdTreeOMP<PointCloud>::ConstPtr target_tree,
|
||||
const Eigen::Matrix4d& init_T_target_source,
|
||||
double max_correspondence_distance,
|
||||
int num_threads) {
|
||||
Registration<GICPFactor, ParallelReductionOMP> registration;
|
||||
registration.rejector.max_dist_sq = max_correspondence_distance * max_correspondence_distance;
|
||||
registration.reduction.num_threads = num_threads;
|
||||
|
||||
if (target_tree == nullptr) {
|
||||
target_tree = std::make_shared<KdTreeOMP<PointCloud>>(target, num_threads);
|
||||
}
|
||||
return registration.align(*target, *source, *target_tree, Eigen::Isometry3d(init_T_target_source));
|
||||
},
|
||||
py::arg("target"),
|
||||
py::arg("source"),
|
||||
py::arg("target_tree") = nullptr,
|
||||
py::arg("init_T_target_source") = Eigen::Matrix4d::Identity(),
|
||||
py::arg("max_correspondence_distance") = 1.0,
|
||||
py::arg("num_threads") = 1);
|
||||
|
||||
// align
|
||||
m.def(
|
||||
"align",
|
||||
[](const GaussianVoxelMap& target_voxelmap, const PointCloud& source, const Eigen::Matrix4d& init_T_target_source, double max_correspondence_distance, int num_threads) {
|
||||
Registration<GICPFactor, ParallelReductionOMP> registration;
|
||||
registration.rejector.max_dist_sq = max_correspondence_distance * max_correspondence_distance;
|
||||
registration.reduction.num_threads = num_threads;
|
||||
|
||||
return registration.align(target_voxelmap, source, target_voxelmap, Eigen::Isometry3d(init_T_target_source));
|
||||
},
|
||||
py::arg("target_voxelmap"),
|
||||
py::arg("source"),
|
||||
py::arg("init_T_target_source") = Eigen::Matrix4d::Identity(),
|
||||
py::arg("max_correspondence_distance") = 1.0,
|
||||
py::arg("num_threads") = 1);
|
||||
}
|
||||
|
|
@ -0,0 +1,86 @@
|
|||
// SPDX-FileCopyrightText: Copyright 2024 Kenji Koide
|
||||
// SPDX-License-Identifier: MIT
|
||||
#include <iostream>
|
||||
|
||||
#include <pybind11/pybind11.h>
|
||||
#include <pybind11/stl.h>
|
||||
#include <pybind11/numpy.h>
|
||||
#include <pybind11/eigen.h>
|
||||
#include <pybind11/functional.h>
|
||||
|
||||
#include <small_gicp/factors/icp_factor.hpp>
|
||||
#include <small_gicp/factors/plane_icp_factor.hpp>
|
||||
#include <small_gicp/factors/gicp_factor.hpp>
|
||||
#include <small_gicp/points/point_cloud.hpp>
|
||||
#include <small_gicp/ann/kdtree_omp.hpp>
|
||||
#include <small_gicp/registration/rejector.hpp>
|
||||
|
||||
namespace py = pybind11;
|
||||
using namespace small_gicp;
|
||||
|
||||
void define_factors(py::module& m) {
|
||||
// DistanceRejector
|
||||
py::class_<DistanceRejector>(m, "DistanceRejector") //
|
||||
.def(py::init<>())
|
||||
.def("set_max_distance", [](DistanceRejector& rejector, double dist) { rejector.max_dist_sq = dist * dist; });
|
||||
|
||||
// ICPFactor
|
||||
py::class_<ICPFactor>(m, "ICPFactor") //
|
||||
.def(py::init<>())
|
||||
.def(
|
||||
"linearize",
|
||||
[](
|
||||
ICPFactor& factor,
|
||||
const PointCloud& target,
|
||||
const PointCloud& source,
|
||||
const KdTreeOMP<PointCloud>& kdtree,
|
||||
const Eigen::Matrix4d& T,
|
||||
size_t source_index,
|
||||
const DistanceRejector& rejector) -> std::tuple<bool, Eigen::Matrix<double, 6, 6>, Eigen::Matrix<double, 6, 1>, double> {
|
||||
Eigen::Matrix<double, 6, 6> H = Eigen::Matrix<double, 6, 6>::Zero();
|
||||
Eigen::Matrix<double, 6, 1> b = Eigen::Matrix<double, 6, 1>::Zero();
|
||||
double e = 0.0;
|
||||
bool succ = factor.linearize(target, source, kdtree, Eigen::Isometry3d(T), source_index, rejector, &H, &b, &e);
|
||||
return std::make_tuple(succ, H, b, e);
|
||||
});
|
||||
|
||||
// PointToPlaneICPFactor
|
||||
py::class_<PointToPlaneICPFactor>(m, "PointToPlaneICPFactor") //
|
||||
.def(py::init<>())
|
||||
.def(
|
||||
"linearize",
|
||||
[](
|
||||
PointToPlaneICPFactor& factor,
|
||||
const PointCloud& target,
|
||||
const PointCloud& source,
|
||||
const KdTreeOMP<PointCloud>& kdtree,
|
||||
const Eigen::Matrix4d& T,
|
||||
size_t source_index,
|
||||
const DistanceRejector& rejector) -> std::tuple<bool, Eigen::Matrix<double, 6, 6>, Eigen::Matrix<double, 6, 1>, double> {
|
||||
Eigen::Matrix<double, 6, 6> H = Eigen::Matrix<double, 6, 6>::Zero();
|
||||
Eigen::Matrix<double, 6, 1> b = Eigen::Matrix<double, 6, 1>::Zero();
|
||||
double e = 0.0;
|
||||
bool succ = factor.linearize(target, source, kdtree, Eigen::Isometry3d(T), source_index, rejector, &H, &b, &e);
|
||||
return std::make_tuple(succ, H, b, e);
|
||||
});
|
||||
|
||||
// GICPFactor
|
||||
py::class_<GICPFactor>(m, "GICPFactor") //
|
||||
.def(py::init<>())
|
||||
.def(
|
||||
"linearize",
|
||||
[](
|
||||
GICPFactor& factor,
|
||||
const PointCloud& target,
|
||||
const PointCloud& source,
|
||||
const KdTreeOMP<PointCloud>& kdtree,
|
||||
const Eigen::Matrix4d& T,
|
||||
size_t source_index,
|
||||
const DistanceRejector& rejector) -> std::tuple<bool, Eigen::Matrix<double, 6, 6>, Eigen::Matrix<double, 6, 1>, double> {
|
||||
Eigen::Matrix<double, 6, 6> H = Eigen::Matrix<double, 6, 6>::Zero();
|
||||
Eigen::Matrix<double, 6, 1> b = Eigen::Matrix<double, 6, 1>::Zero();
|
||||
double e = 0.0;
|
||||
bool succ = factor.linearize(target, source, kdtree, Eigen::Isometry3d(T), source_index, rejector, &H, &b, &e);
|
||||
return std::make_tuple(succ, H, b, e);
|
||||
});
|
||||
}
|
||||
|
|
@ -0,0 +1,35 @@
|
|||
// SPDX-FileCopyrightText: Copyright 2024 Kenji Koide
|
||||
// SPDX-License-Identifier: MIT
|
||||
#include <iostream>
|
||||
|
||||
#include <pybind11/pybind11.h>
|
||||
#include <pybind11/stl.h>
|
||||
#include <pybind11/numpy.h>
|
||||
#include <pybind11/eigen.h>
|
||||
#include <pybind11/functional.h>
|
||||
|
||||
#include <small_gicp/points/point_cloud.hpp>
|
||||
#include <small_gicp/ann/kdtree_omp.hpp>
|
||||
|
||||
namespace py = pybind11;
|
||||
using namespace small_gicp;
|
||||
|
||||
void define_kdtree(py::module& m) {
|
||||
// KdTree
|
||||
py::class_<KdTreeOMP<PointCloud>, std::shared_ptr<KdTreeOMP<PointCloud>>>(m, "KdTree") //
|
||||
.def(py::init<PointCloud::ConstPtr, int>(), py::arg("points"), py::arg("num_threads") = 1)
|
||||
.def(
|
||||
"nearest_neighbor_search",
|
||||
[](const KdTreeOMP<PointCloud>& kdtree, const Eigen::Vector3d& pt) {
|
||||
size_t k_index = -1;
|
||||
double k_sq_dist = std::numeric_limits<double>::max();
|
||||
const size_t found = traits::nearest_neighbor_search(kdtree, Eigen::Vector4d(pt.x(), pt.y(), pt.z(), 1.0), &k_index, &k_sq_dist);
|
||||
return std::make_tuple(found, k_index, k_sq_dist);
|
||||
})
|
||||
.def("knn_search", [](const KdTreeOMP<PointCloud>& kdtree, const Eigen::Vector3d& pt, int k) {
|
||||
std::vector<size_t> k_indices(k, -1);
|
||||
std::vector<double> k_sq_dists(k, std::numeric_limits<double>::max());
|
||||
const size_t found = traits::knn_search(kdtree, Eigen::Vector4d(pt.x(), pt.y(), pt.z(), 1.0), k, k_indices.data(), k_sq_dists.data());
|
||||
return std::make_pair(k_indices, k_sq_dists);
|
||||
});
|
||||
}
|
||||
|
|
@ -0,0 +1,27 @@
|
|||
// SPDX-FileCopyrightText: Copyright 2024 Kenji Koide
|
||||
// SPDX-License-Identifier: MIT
|
||||
#include <iostream>
|
||||
|
||||
#include <pybind11/pybind11.h>
|
||||
#include <pybind11/stl.h>
|
||||
#include <pybind11/numpy.h>
|
||||
#include <pybind11/eigen.h>
|
||||
#include <pybind11/functional.h>
|
||||
|
||||
#include <small_gicp/points/point_cloud.hpp>
|
||||
#include <small_gicp/benchmark/read_points.hpp>
|
||||
|
||||
namespace py = pybind11;
|
||||
using namespace small_gicp;
|
||||
|
||||
void define_misc(py::module& m) {
|
||||
// read_ply
|
||||
m.def(
|
||||
"read_ply",
|
||||
[](const std::string& filename) {
|
||||
const auto points = read_ply(filename);
|
||||
return std::make_shared<PointCloud>(points);
|
||||
},
|
||||
"Read PLY file",
|
||||
py::arg("filename"));
|
||||
}
|
||||
|
|
@ -0,0 +1,53 @@
|
|||
// SPDX-FileCopyrightText: Copyright 2024 Kenji Koide
|
||||
// SPDX-License-Identifier: MIT
|
||||
#include <iostream>
|
||||
|
||||
#include <pybind11/pybind11.h>
|
||||
#include <pybind11/stl.h>
|
||||
#include <pybind11/numpy.h>
|
||||
#include <pybind11/eigen.h>
|
||||
#include <pybind11/functional.h>
|
||||
|
||||
#include <small_gicp/points/point_cloud.hpp>
|
||||
|
||||
namespace py = pybind11;
|
||||
using namespace small_gicp;
|
||||
|
||||
void define_pointcloud(py::module& m) {
|
||||
// PointCloud
|
||||
py::class_<PointCloud, std::shared_ptr<PointCloud>>(m, "PointCloud") //
|
||||
.def(py::init([](const Eigen::MatrixXd& points) {
|
||||
if (points.cols() != 3 && points.cols() != 4) {
|
||||
std::cerr << "points must be Nx3 or Nx4" << std::endl;
|
||||
return std::make_shared<PointCloud>();
|
||||
}
|
||||
|
||||
auto pc = std::make_shared<PointCloud>();
|
||||
pc->resize(points.rows());
|
||||
if (points.cols() == 3) {
|
||||
for (size_t i = 0; i < points.rows(); i++) {
|
||||
pc->point(i) << points.row(i).transpose(), 1.0;
|
||||
}
|
||||
} else {
|
||||
for (size_t i = 0; i < points.rows(); i++) {
|
||||
pc->point(i) << points.row(i).transpose();
|
||||
}
|
||||
}
|
||||
|
||||
return pc;
|
||||
})) //
|
||||
.def("__repr__", [](const PointCloud& points) { return "small_gicp.PointCloud (" + std::to_string(points.size()) + " points)"; })
|
||||
.def("__len__", [](const PointCloud& points) { return points.size(); })
|
||||
.def("empty", &PointCloud::empty)
|
||||
.def("size", &PointCloud::size)
|
||||
.def(
|
||||
"points",
|
||||
[](const PointCloud& points) -> Eigen::MatrixXd { return Eigen::Map<const Eigen::Matrix<double, -1, -1, Eigen::RowMajor>>(points.points[0].data(), points.size(), 4); })
|
||||
.def(
|
||||
"normals",
|
||||
[](const PointCloud& points) -> Eigen::MatrixXd { return Eigen::Map<const Eigen::Matrix<double, -1, -1, Eigen::RowMajor>>(points.normals[0].data(), points.size(), 4); })
|
||||
.def("covs", [](const PointCloud& points) { return points.covs; })
|
||||
.def("point", [](const PointCloud& points, size_t i) -> Eigen::Vector4d { return points.point(i); })
|
||||
.def("normal", [](const PointCloud& points, size_t i) -> Eigen::Vector4d { return points.normal(i); })
|
||||
.def("cov", [](const PointCloud& points, size_t i) -> Eigen::Matrix4d { return points.cov(i); });
|
||||
}
|
||||
|
|
@ -0,0 +1,161 @@
|
|||
// SPDX-FileCopyrightText: Copyright 2024 Kenji Koide
|
||||
// SPDX-License-Identifier: MIT
|
||||
#include <iostream>
|
||||
|
||||
#include <pybind11/pybind11.h>
|
||||
#include <pybind11/stl.h>
|
||||
#include <pybind11/numpy.h>
|
||||
#include <pybind11/eigen.h>
|
||||
#include <pybind11/functional.h>
|
||||
|
||||
#include <small_gicp/points/eigen.hpp>
|
||||
#include <small_gicp/points/point_cloud.hpp>
|
||||
#include <small_gicp/ann/kdtree_omp.hpp>
|
||||
#include <small_gicp/util/downsampling.hpp>
|
||||
#include <small_gicp/util/downsampling_omp.hpp>
|
||||
#include <small_gicp/util/normal_estimation.hpp>
|
||||
#include <small_gicp/util/normal_estimation_omp.hpp>
|
||||
|
||||
namespace py = pybind11;
|
||||
using namespace small_gicp;
|
||||
|
||||
void define_preprocess(py::module& m) {
|
||||
// voxelgrid_sampling
|
||||
m.def(
|
||||
"voxelgrid_sampling",
|
||||
[](const PointCloud& points, double resolution, int num_threads) {
|
||||
if (num_threads == 1) {
|
||||
return voxelgrid_sampling(points, resolution);
|
||||
}
|
||||
return voxelgrid_sampling_omp(points, resolution, num_threads);
|
||||
},
|
||||
"Voxelgrid sampling",
|
||||
py::arg("points"),
|
||||
py::arg("downsampling_resolution"),
|
||||
py::arg("num_threads") = 1);
|
||||
|
||||
// voxelgrid_sampling (numpy)
|
||||
m.def(
|
||||
"voxelgrid_sampling",
|
||||
[](const Eigen::MatrixXd& points, double resolution, int num_threads) -> std::shared_ptr<PointCloud> {
|
||||
if (points.cols() != 3 && points.cols() != 4) {
|
||||
std::cerr << "points must be Nx3 or Nx4" << std::endl;
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
if (num_threads == 1) {
|
||||
return voxelgrid_sampling<Eigen::MatrixXd, PointCloud>(points, resolution);
|
||||
} else {
|
||||
return voxelgrid_sampling_omp<Eigen::MatrixXd, PointCloud>(points, resolution, num_threads);
|
||||
}
|
||||
},
|
||||
"Voxelgrid sampling",
|
||||
py::arg("points"),
|
||||
py::arg("downsampling_resolution"),
|
||||
py::arg("num_threads") = 1);
|
||||
|
||||
// estimate_normals
|
||||
m.def(
|
||||
"estimate_normals",
|
||||
[](PointCloud::Ptr points, std::shared_ptr<KdTreeOMP<PointCloud>> tree, int num_neighbors, int num_threads) {
|
||||
if (tree == nullptr) {
|
||||
tree = std::make_shared<KdTreeOMP<PointCloud>>(points, num_threads);
|
||||
}
|
||||
|
||||
if (num_threads == 1) {
|
||||
estimate_normals(*points, *tree, num_neighbors);
|
||||
} else {
|
||||
estimate_normals_omp(*points, *tree, num_neighbors, num_threads);
|
||||
}
|
||||
},
|
||||
py::arg("points"),
|
||||
py::arg("tree") = nullptr,
|
||||
py::arg("num_neighbors") = 20,
|
||||
py::arg("num_threads") = 1);
|
||||
|
||||
// estimate_covariances
|
||||
m.def(
|
||||
"estimate_covariances",
|
||||
[](PointCloud::Ptr points, std::shared_ptr<KdTreeOMP<PointCloud>> tree, int num_neighbors, int num_threads) {
|
||||
if (tree == nullptr) {
|
||||
tree = std::make_shared<KdTreeOMP<PointCloud>>(points, num_threads);
|
||||
}
|
||||
|
||||
if (num_threads == 1) {
|
||||
estimate_covariances(*points, *tree, num_neighbors);
|
||||
} else {
|
||||
estimate_covariances_omp(*points, *tree, num_neighbors, num_threads);
|
||||
}
|
||||
},
|
||||
py::arg("points"),
|
||||
py::arg("tree") = nullptr,
|
||||
py::arg("num_neighbors") = 20,
|
||||
py::arg("num_threads") = 1);
|
||||
|
||||
// estimate_normals_covariances
|
||||
m.def(
|
||||
"estimate_normals_covariances",
|
||||
[](PointCloud::Ptr points, std::shared_ptr<KdTreeOMP<PointCloud>> tree, int num_neighbors, int num_threads) {
|
||||
if (tree == nullptr) {
|
||||
tree = std::make_shared<KdTreeOMP<PointCloud>>(points, num_threads);
|
||||
}
|
||||
|
||||
if (num_threads == 1) {
|
||||
estimate_normals_covariances(*points, *tree, num_neighbors);
|
||||
} else {
|
||||
estimate_normals_covariances_omp(*points, *tree, num_neighbors, num_threads);
|
||||
}
|
||||
},
|
||||
py::arg("points"),
|
||||
py::arg("tree") = nullptr,
|
||||
py::arg("num_neighbors") = 20,
|
||||
py::arg("num_threads") = 1);
|
||||
|
||||
// preprocess_points (numpy)
|
||||
m.def(
|
||||
"preprocess_points",
|
||||
[](const Eigen::MatrixXd& points_numpy, double downsampling_resolution, int num_neighbors, int num_threads) -> std::pair<PointCloud::Ptr, KdTreeOMP<PointCloud>::Ptr> {
|
||||
if (points_numpy.cols() != 3 && points_numpy.cols() != 4) {
|
||||
std::cerr << "points_numpy must be Nx3 or Nx4" << std::endl;
|
||||
return {nullptr, nullptr};
|
||||
}
|
||||
|
||||
auto points = std::make_shared<PointCloud>();
|
||||
points->resize(points_numpy.rows());
|
||||
for (size_t i = 0; i < points_numpy.rows(); i++) {
|
||||
if (points_numpy.cols() == 3) {
|
||||
points->point(i) << points_numpy.row(i).transpose(), 1.0;
|
||||
} else {
|
||||
points->point(i) << points_numpy.row(i).transpose();
|
||||
}
|
||||
}
|
||||
|
||||
auto downsampled = voxelgrid_sampling_omp(*points, downsampling_resolution, num_threads);
|
||||
auto kdtree = std::make_shared<KdTreeOMP<PointCloud>>(downsampled, num_threads);
|
||||
estimate_normals_covariances_omp(*downsampled, *kdtree, num_neighbors, num_threads);
|
||||
return {downsampled, kdtree};
|
||||
},
|
||||
py::arg("points"),
|
||||
py::arg("downsampling_resolution") = 0.25,
|
||||
py::arg("num_neighbors") = 10,
|
||||
py::arg("num_threads") = 1);
|
||||
|
||||
// preprocess_points
|
||||
m.def(
|
||||
"preprocess_points",
|
||||
[](const PointCloud& points, double downsampling_resolution, int num_neighbors, int num_threads) -> std::pair<PointCloud::Ptr, KdTreeOMP<PointCloud>::Ptr> {
|
||||
if (points.empty()) {
|
||||
std::cerr << "warning: points is empty" << std::endl;
|
||||
return {nullptr, nullptr};
|
||||
}
|
||||
|
||||
auto downsampled = voxelgrid_sampling_omp(points, downsampling_resolution, num_threads);
|
||||
auto kdtree = std::make_shared<KdTreeOMP<PointCloud>>(downsampled, num_threads);
|
||||
estimate_normals_covariances_omp(*downsampled, *kdtree, num_neighbors, num_threads);
|
||||
return {downsampled, kdtree};
|
||||
},
|
||||
py::arg("points"),
|
||||
py::arg("downsampling_resolution") = 0.25,
|
||||
py::arg("num_neighbors") = 10,
|
||||
py::arg("num_threads") = 1);
|
||||
}
|
||||
|
|
@ -1,346 +1,27 @@
|
|||
// SPDX-FileCopyrightText: Copyright 2024 Kenji Koide
|
||||
// SPDX-License-Identifier: MIT
|
||||
#include <pybind11/pybind11.h>
|
||||
#include <pybind11/stl.h>
|
||||
#include <pybind11/numpy.h>
|
||||
#include <pybind11/eigen.h>
|
||||
#include <pybind11/functional.h>
|
||||
|
||||
#include <small_gicp/ann/kdtree_omp.hpp>
|
||||
#include <small_gicp/points/point_cloud.hpp>
|
||||
#include <small_gicp/util/downsampling.hpp>
|
||||
#include <small_gicp/util/downsampling_omp.hpp>
|
||||
#include <small_gicp/util/normal_estimation.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>
|
||||
#include <small_gicp/registration/registration_helper.hpp>
|
||||
#include <small_gicp/benchmark/read_points.hpp>
|
||||
|
||||
namespace py = pybind11;
|
||||
using namespace small_gicp;
|
||||
|
||||
void define_pointcloud(py::module& m);
|
||||
void define_kdtree(py::module& m);
|
||||
void define_voxelmap(py::module& m);
|
||||
void define_preprocess(py::module& m);
|
||||
void define_result(py::module& m);
|
||||
void define_align(py::module& m);
|
||||
void define_factors(py::module& m);
|
||||
void define_misc(py::module& m);
|
||||
|
||||
PYBIND11_MODULE(small_gicp, m) {
|
||||
m.doc() = "Small GICP";
|
||||
|
||||
// PointCloud
|
||||
py::class_<PointCloud, std::shared_ptr<PointCloud>>(m, "PointCloud") //
|
||||
.def(py::init([](const Eigen::MatrixXd& points) {
|
||||
if (points.cols() != 3 && points.cols() != 4) {
|
||||
std::cerr << "points must be Nx3 or Nx4" << std::endl;
|
||||
return std::make_shared<PointCloud>();
|
||||
}
|
||||
|
||||
auto pc = std::make_shared<PointCloud>();
|
||||
pc->resize(points.rows());
|
||||
if (points.cols() == 3) {
|
||||
for (size_t i = 0; i < points.rows(); i++) {
|
||||
pc->point(i) << points.row(i).transpose(), 1.0;
|
||||
}
|
||||
} else {
|
||||
for (size_t i = 0; i < points.rows(); i++) {
|
||||
pc->point(i) << points.row(i).transpose();
|
||||
}
|
||||
}
|
||||
|
||||
return pc;
|
||||
})) //
|
||||
.def("__repr__", [](const PointCloud& points) { return "small_gicp.PointCloud (" + std::to_string(points.size()) + " points)"; })
|
||||
.def("size", &PointCloud::size)
|
||||
.def(
|
||||
"points",
|
||||
[](const PointCloud& points) -> Eigen::MatrixXd { return Eigen::Map<const Eigen::Matrix<double, -1, -1, Eigen::RowMajor>>(points.points[0].data(), points.size(), 4); })
|
||||
.def(
|
||||
"normals",
|
||||
[](const PointCloud& points) -> Eigen::MatrixXd { return Eigen::Map<const Eigen::Matrix<double, -1, -1, Eigen::RowMajor>>(points.normals[0].data(), points.size(), 4); })
|
||||
.def("covs", [](const PointCloud& points) { return points.covs; })
|
||||
.def("point", [](const PointCloud& points, size_t i) -> Eigen::Vector4d { return points.point(i); })
|
||||
.def("normal", [](const PointCloud& points, size_t i) -> Eigen::Vector4d { return points.normal(i); })
|
||||
.def("cov", [](const PointCloud& points, size_t i) -> Eigen::Matrix4d { return points.cov(i); });
|
||||
|
||||
// KdTree
|
||||
py::class_<KdTreeOMP<PointCloud>, std::shared_ptr<KdTreeOMP<PointCloud>>>(m, "KdTree") //
|
||||
.def(
|
||||
py::init([](const PointCloud::ConstPtr& points, int num_threads) { return std::make_shared<KdTreeOMP<PointCloud>>(points, num_threads); }),
|
||||
py::arg("points"),
|
||||
py::arg("num_threads") = 1)
|
||||
.def(
|
||||
"nearest_neighbor_search",
|
||||
[](const KdTreeOMP<PointCloud>& kdtree, const Eigen::Vector3d& pt) {
|
||||
size_t k_index = -1;
|
||||
double k_sq_dist = std::numeric_limits<double>::max();
|
||||
const size_t found = traits::nearest_neighbor_search(kdtree, Eigen::Vector4d(pt.x(), pt.y(), pt.z(), 1.0), &k_index, &k_sq_dist);
|
||||
return std::make_tuple(found, k_index, k_sq_dist);
|
||||
})
|
||||
.def("knn_search", [](const KdTreeOMP<PointCloud>& kdtree, const Eigen::Vector3d& pt, int k) {
|
||||
std::vector<size_t> k_indices(k, -1);
|
||||
std::vector<double> k_sq_dists(k, std::numeric_limits<double>::max());
|
||||
const size_t found = traits::knn_search(kdtree, Eigen::Vector4d(pt.x(), pt.y(), pt.z(), 1.0), k, k_indices.data(), k_sq_dists.data());
|
||||
return std::make_pair(k_indices, k_sq_dists);
|
||||
});
|
||||
|
||||
// GaussianVoxelMap
|
||||
py::class_<GaussianVoxelMap, std::shared_ptr<GaussianVoxelMap>>(m, "GaussianVoxelMap") //
|
||||
.def(py::init([](double voxel_resolution) { return std::make_shared<GaussianVoxelMap>(voxel_resolution); }), py::arg("voxel_resolution"))
|
||||
.def(
|
||||
"insert",
|
||||
[](GaussianVoxelMap& voxelmap, const PointCloud& points, const Eigen::Matrix4d& T) { voxelmap.insert(points, Eigen::Isometry3d(T)); },
|
||||
py::arg("points"),
|
||||
py::arg("T") = Eigen::Matrix4d::Identity());
|
||||
|
||||
// RegistrationResult
|
||||
py::class_<RegistrationResult>(m, "RegistrationResult") //
|
||||
.def(
|
||||
"__repr__",
|
||||
[](const RegistrationResult& result) {
|
||||
std::stringstream sst;
|
||||
sst << "small_gicp.RegistrationResult\n";
|
||||
sst << "converted=" << result.converged << "\n";
|
||||
sst << "iterations=" << result.iterations << "\n";
|
||||
sst << "num_inliers=" << result.num_inliers << "\n";
|
||||
sst << "T_target_source=\n" << result.T_target_source.matrix() << "\n";
|
||||
sst << "H=\n" << result.H << "\n";
|
||||
sst << "b=\n" << result.b.transpose() << "\n";
|
||||
sst << "error= " << result.error << "\n";
|
||||
return sst.str();
|
||||
})
|
||||
.def_property_readonly("T_target_source", [](const RegistrationResult& result) -> Eigen::Matrix4d { return result.T_target_source.matrix(); })
|
||||
.def_readonly("converged", &RegistrationResult::converged)
|
||||
.def_readonly("iterations", &RegistrationResult::iterations)
|
||||
.def_readonly("num_inliers", &RegistrationResult::num_inliers)
|
||||
.def_readonly("H", &RegistrationResult::H)
|
||||
.def_readonly("b", &RegistrationResult::b)
|
||||
.def_readonly("error", &RegistrationResult::error);
|
||||
|
||||
// read_ply
|
||||
m.def(
|
||||
"read_ply",
|
||||
[](const std::string& filename) {
|
||||
const auto points = read_ply(filename);
|
||||
return std::make_shared<PointCloud>(points);
|
||||
},
|
||||
"Read PLY file",
|
||||
py::arg("filename"));
|
||||
|
||||
// voxelgrid_sampling
|
||||
m.def(
|
||||
"voxelgrid_sampling",
|
||||
[](const PointCloud& points, double resolution, int num_threads) {
|
||||
if (num_threads == 1) {
|
||||
return voxelgrid_sampling(points, resolution);
|
||||
}
|
||||
return voxelgrid_sampling_omp(points, resolution, num_threads);
|
||||
},
|
||||
"Voxelgrid sampling",
|
||||
py::arg("points"),
|
||||
py::arg("downsampling_resolution"),
|
||||
py::arg("num_threads") = 1);
|
||||
|
||||
// estimate_normals
|
||||
m.def(
|
||||
"estimate_normals",
|
||||
[](PointCloud::Ptr points, std::shared_ptr<KdTreeOMP<PointCloud>> tree, int num_neighbors, int num_threads) {
|
||||
if (tree == nullptr) {
|
||||
tree = std::make_shared<KdTreeOMP<PointCloud>>(points, num_threads);
|
||||
}
|
||||
|
||||
if (num_threads == 1) {
|
||||
estimate_normals(*points, *tree, num_neighbors);
|
||||
} else {
|
||||
estimate_normals_omp(*points, *tree, num_neighbors, num_threads);
|
||||
}
|
||||
},
|
||||
py::arg("points"),
|
||||
py::arg("tree") = nullptr,
|
||||
py::arg("num_neighbors") = 20,
|
||||
py::arg("num_threads") = 1);
|
||||
|
||||
// estimate_covariances
|
||||
m.def(
|
||||
"estimate_covariances",
|
||||
[](PointCloud::Ptr points, std::shared_ptr<KdTreeOMP<PointCloud>> tree, int num_neighbors, int num_threads) {
|
||||
if (tree == nullptr) {
|
||||
tree = std::make_shared<KdTreeOMP<PointCloud>>(points, num_threads);
|
||||
}
|
||||
|
||||
if (num_threads == 1) {
|
||||
estimate_covariances(*points, *tree, num_neighbors);
|
||||
} else {
|
||||
estimate_covariances_omp(*points, *tree, num_neighbors, num_threads);
|
||||
}
|
||||
},
|
||||
py::arg("points"),
|
||||
py::arg("tree") = nullptr,
|
||||
py::arg("num_neighbors") = 20,
|
||||
py::arg("num_threads") = 1);
|
||||
|
||||
// estimate_normals_covariances
|
||||
m.def(
|
||||
"estimate_normals_covariances",
|
||||
[](PointCloud::Ptr points, std::shared_ptr<KdTreeOMP<PointCloud>> tree, int num_neighbors, int num_threads) {
|
||||
if (tree == nullptr) {
|
||||
tree = std::make_shared<KdTreeOMP<PointCloud>>(points, num_threads);
|
||||
}
|
||||
|
||||
if (num_threads == 1) {
|
||||
estimate_normals_covariances(*points, *tree, num_neighbors);
|
||||
} else {
|
||||
estimate_normals_covariances_omp(*points, *tree, num_neighbors, num_threads);
|
||||
}
|
||||
},
|
||||
py::arg("points"),
|
||||
py::arg("tree") = nullptr,
|
||||
py::arg("num_neighbors") = 20,
|
||||
py::arg("num_threads") = 1);
|
||||
|
||||
// preprocess_points
|
||||
m.def(
|
||||
"preprocess_points",
|
||||
[](PointCloud::ConstPtr points, const Eigen::MatrixXd points_numpy, double downsampling_resolution, int num_neighbors, int num_threads)
|
||||
-> std::pair<PointCloud::Ptr, KdTreeOMP<PointCloud>::Ptr> {
|
||||
if (points == nullptr && points_numpy.size() == 0) {
|
||||
std::cerr << "points or points_numpy must be provided" << std::endl;
|
||||
return {nullptr, nullptr};
|
||||
}
|
||||
|
||||
if (!points) {
|
||||
if (points_numpy.cols() != 3 && points_numpy.cols() != 4) {
|
||||
std::cerr << "points_numpy must be Nx3 or Nx4" << std::endl;
|
||||
return {nullptr, nullptr};
|
||||
}
|
||||
|
||||
auto pts = std::make_shared<PointCloud>();
|
||||
pts->resize(points_numpy.rows());
|
||||
for (size_t i = 0; i < points_numpy.rows(); i++) {
|
||||
if (points_numpy.cols() == 3) {
|
||||
pts->point(i) << points_numpy.row(i).transpose(), 1.0;
|
||||
} else {
|
||||
pts->point(i) << points_numpy.row(i).transpose();
|
||||
}
|
||||
}
|
||||
points = pts;
|
||||
}
|
||||
|
||||
auto downsampled = voxelgrid_sampling_omp(*points, downsampling_resolution, num_threads);
|
||||
auto kdtree = std::make_shared<KdTreeOMP<PointCloud>>(downsampled, num_threads);
|
||||
estimate_normals_covariances_omp(*downsampled, *kdtree, num_neighbors, num_threads);
|
||||
return {downsampled, kdtree};
|
||||
},
|
||||
py::arg("points") = nullptr,
|
||||
py::arg("points_numpy") = Eigen::MatrixXd(),
|
||||
py::arg("downsampling_resolution") = 0.25,
|
||||
py::arg("num_neighbors") = 10,
|
||||
py::arg("num_threads") = 1);
|
||||
|
||||
// align_points
|
||||
m.def(
|
||||
"align_points",
|
||||
[](
|
||||
const Eigen::MatrixXd& target_points,
|
||||
const Eigen::MatrixXd& source_points,
|
||||
const Eigen::Matrix4d& init_T_target_source,
|
||||
const std::string& registration_type,
|
||||
double voxel_resolution,
|
||||
double downsampling_resolution,
|
||||
double max_corresponding_distance,
|
||||
int num_threads) {
|
||||
if (target_points.cols() != 3 && target_points.cols() != 4) {
|
||||
std::cerr << "target_points must be Nx3 or Nx4" << std::endl;
|
||||
return RegistrationResult(Eigen::Isometry3d::Identity());
|
||||
}
|
||||
if (source_points.cols() != 3 && source_points.cols() != 4) {
|
||||
std::cerr << "source_points must be Nx3 or Nx4" << std::endl;
|
||||
return RegistrationResult(Eigen::Isometry3d::Identity());
|
||||
}
|
||||
|
||||
RegistrationSetting setting;
|
||||
if (registration_type == "ICP") {
|
||||
setting.type = RegistrationSetting::ICP;
|
||||
} else if (registration_type == "PLANE_ICP") {
|
||||
setting.type = RegistrationSetting::PLANE_ICP;
|
||||
} else if (registration_type == "GICP") {
|
||||
setting.type = RegistrationSetting::GICP;
|
||||
} else if (registration_type == "VGICP") {
|
||||
setting.type = RegistrationSetting::VGICP;
|
||||
} else {
|
||||
std::cerr << "invalid registration type" << std::endl;
|
||||
return RegistrationResult(Eigen::Isometry3d::Identity());
|
||||
}
|
||||
|
||||
setting.voxel_resolution = voxel_resolution;
|
||||
setting.downsampling_resolution = downsampling_resolution;
|
||||
setting.max_correspondence_distance = max_corresponding_distance;
|
||||
setting.num_threads = num_threads;
|
||||
|
||||
std::vector<Eigen::Vector4d> target(target_points.rows());
|
||||
if (target_points.cols() == 3) {
|
||||
for (size_t i = 0; i < target_points.rows(); i++) {
|
||||
target[i] << target_points.row(i).transpose(), 1.0;
|
||||
}
|
||||
} else {
|
||||
for (size_t i = 0; i < target_points.rows(); i++) {
|
||||
target[i] << target_points.row(i).transpose();
|
||||
}
|
||||
}
|
||||
|
||||
std::vector<Eigen::Vector4d> source(source_points.rows());
|
||||
if (source_points.cols() == 3) {
|
||||
for (size_t i = 0; i < source_points.rows(); i++) {
|
||||
source[i] << source_points.row(i).transpose(), 1.0;
|
||||
}
|
||||
} else {
|
||||
for (size_t i = 0; i < source_points.rows(); i++) {
|
||||
source[i] << source_points.row(i).transpose();
|
||||
}
|
||||
}
|
||||
|
||||
return align(target, source, Eigen::Isometry3d(init_T_target_source), setting);
|
||||
},
|
||||
py::arg("target_points"),
|
||||
py::arg("source_points"),
|
||||
py::arg("init_T_target_source") = Eigen::Matrix4d::Identity(),
|
||||
py::arg("registration_type") = "GICP",
|
||||
py::arg("voxel_resolution") = 1.0,
|
||||
py::arg("downsampling_resolution") = 0.25,
|
||||
py::arg("max_corresponding_distance") = 1.0,
|
||||
py::arg("num_threads") = 1);
|
||||
|
||||
// align
|
||||
m.def(
|
||||
"align",
|
||||
[](
|
||||
PointCloud::ConstPtr target,
|
||||
PointCloud::ConstPtr source,
|
||||
KdTreeOMP<PointCloud>::ConstPtr target_tree,
|
||||
const Eigen::Matrix4d& init_T_target_source,
|
||||
GaussianVoxelMap::ConstPtr target_voxelmap,
|
||||
double max_correspondence_distance,
|
||||
int num_threads) {
|
||||
if (target == nullptr && target_voxelmap == nullptr) {
|
||||
std::cerr << "target or target_voxelmap must be provided" << std::endl;
|
||||
return RegistrationResult(Eigen::Isometry3d::Identity());
|
||||
}
|
||||
|
||||
Registration<GICPFactor, ParallelReductionOMP> registration;
|
||||
registration.rejector.max_dist_sq = max_correspondence_distance * max_correspondence_distance;
|
||||
registration.reduction.num_threads = num_threads;
|
||||
|
||||
if (target) {
|
||||
if (target_tree == nullptr) {
|
||||
target_tree = std::make_shared<KdTreeOMP<PointCloud>>(target, num_threads);
|
||||
}
|
||||
auto result = registration.align(*target, *source, *target_tree, Eigen::Isometry3d(init_T_target_source));
|
||||
return result;
|
||||
} else {
|
||||
return registration.align(*target_voxelmap, *source, *target_voxelmap, Eigen::Isometry3d(init_T_target_source));
|
||||
}
|
||||
},
|
||||
py::arg("target") = nullptr,
|
||||
py::arg("source") = nullptr,
|
||||
py::arg("target_tree") = nullptr,
|
||||
py::arg("init_T_target_source") = Eigen::Matrix4d::Identity(),
|
||||
py::arg("target_voxelmap") = nullptr,
|
||||
py::arg("max_correspondence_distance") = 1.0,
|
||||
py::arg("num_threads") = 1);
|
||||
define_pointcloud(m);
|
||||
define_kdtree(m);
|
||||
define_voxelmap(m);
|
||||
define_preprocess(m);
|
||||
define_result(m);
|
||||
define_align(m);
|
||||
define_factors(m);
|
||||
define_misc(m);
|
||||
}
|
||||
|
|
@ -0,0 +1,40 @@
|
|||
// SPDX-FileCopyrightText: Copyright 2024 Kenji Koide
|
||||
// SPDX-License-Identifier: MIT
|
||||
#include <iostream>
|
||||
|
||||
#include <pybind11/pybind11.h>
|
||||
#include <pybind11/stl.h>
|
||||
#include <pybind11/numpy.h>
|
||||
#include <pybind11/eigen.h>
|
||||
#include <pybind11/functional.h>
|
||||
|
||||
#include <small_gicp/registration/registration_result.hpp>
|
||||
|
||||
namespace py = pybind11;
|
||||
using namespace small_gicp;
|
||||
|
||||
void define_result(py::module& m) {
|
||||
// RegistrationResult
|
||||
py::class_<RegistrationResult>(m, "RegistrationResult") //
|
||||
.def(
|
||||
"__repr__",
|
||||
[](const RegistrationResult& result) {
|
||||
std::stringstream sst;
|
||||
sst << "small_gicp.RegistrationResult\n";
|
||||
sst << "converted=" << result.converged << "\n";
|
||||
sst << "iterations=" << result.iterations << "\n";
|
||||
sst << "num_inliers=" << result.num_inliers << "\n";
|
||||
sst << "T_target_source=\n" << result.T_target_source.matrix() << "\n";
|
||||
sst << "H=\n" << result.H << "\n";
|
||||
sst << "b=\n" << result.b.transpose() << "\n";
|
||||
sst << "error= " << result.error << "\n";
|
||||
return sst.str();
|
||||
})
|
||||
.def_property_readonly("T_target_source", [](const RegistrationResult& result) -> Eigen::Matrix4d { return result.T_target_source.matrix(); })
|
||||
.def_readonly("converged", &RegistrationResult::converged)
|
||||
.def_readonly("iterations", &RegistrationResult::iterations)
|
||||
.def_readonly("num_inliers", &RegistrationResult::num_inliers)
|
||||
.def_readonly("H", &RegistrationResult::H)
|
||||
.def_readonly("b", &RegistrationResult::b)
|
||||
.def_readonly("error", &RegistrationResult::error);
|
||||
}
|
||||
|
|
@ -0,0 +1,36 @@
|
|||
// SPDX-FileCopyrightText: Copyright 2024 Kenji Koide
|
||||
// SPDX-License-Identifier: MIT
|
||||
#include <iostream>
|
||||
|
||||
#include <pybind11/pybind11.h>
|
||||
#include <pybind11/stl.h>
|
||||
#include <pybind11/numpy.h>
|
||||
#include <pybind11/eigen.h>
|
||||
#include <pybind11/functional.h>
|
||||
|
||||
#include <small_gicp/points/point_cloud.hpp>
|
||||
#include <small_gicp/ann/kdtree_omp.hpp>
|
||||
#include <small_gicp/ann/gaussian_voxelmap.hpp>
|
||||
|
||||
namespace py = pybind11;
|
||||
using namespace small_gicp;
|
||||
|
||||
void define_voxelmap(py::module& m) {
|
||||
// GaussianVoxelMap
|
||||
py::class_<GaussianVoxelMap>(m, "GaussianVoxelMap") //
|
||||
.def(py::init<double>())
|
||||
.def(
|
||||
"__repr__",
|
||||
[](const GaussianVoxelMap& voxelmap) {
|
||||
std::stringstream sst;
|
||||
sst << "small_gicp.GaussianVoxelMap (" << 1.0 / voxelmap.inv_leaf_size << " m / " << voxelmap.size() << " voxels)" << std::endl;
|
||||
return sst.str();
|
||||
})
|
||||
.def("__len__", [](const GaussianVoxelMap& voxelmap) { return voxelmap.size(); })
|
||||
.def("size", &GaussianVoxelMap::size)
|
||||
.def(
|
||||
"insert",
|
||||
[](GaussianVoxelMap& voxelmap, const PointCloud& points, const Eigen::Matrix4d& T) { voxelmap.insert(points, Eigen::Isometry3d(T)); },
|
||||
py::arg("points"),
|
||||
py::arg("T") = Eigen::Matrix4d::Identity());
|
||||
}
|
||||
|
|
@ -0,0 +1,164 @@
|
|||
#!/usr/bin/python3
|
||||
# SPDX-FileCopyrightText: Copyright 2024 Kenji Koide
|
||||
# SPDX-License-Identifier: MIT
|
||||
import numpy
|
||||
from scipy.spatial.transform import Rotation
|
||||
|
||||
import small_gicp
|
||||
|
||||
|
||||
# Basic registation example with small_gicp.PointCloud
|
||||
def example_small1(target_raw_numpy : numpy.ndarray, source_raw_numpy : numpy.ndarray):
|
||||
# Convert numpy arrays (Nx3 or Nx4) to small_gicp.PointCloud
|
||||
target_raw = small_gicp.PointCloud(target_raw_numpy)
|
||||
source_raw = small_gicp.PointCloud(source_raw_numpy)
|
||||
|
||||
# Preprocess point clouds
|
||||
target, target_tree = small_gicp.preprocess_points(target_raw, downsampling_resolution=0.25)
|
||||
source, source_tree = small_gicp.preprocess_points(source_raw, downsampling_resolution=0.25)
|
||||
|
||||
result = small_gicp.align(target, source, target_tree)
|
||||
|
||||
return result.T_target_source
|
||||
|
||||
# Example to perform each preprocessing and registration separately
|
||||
def example_small2(target_raw_numpy : numpy.ndarray, source_raw_numpy : numpy.ndarray):
|
||||
# Convert numpy arrays (Nx3 or Nx4) to small_gicp.PointCloud
|
||||
target_raw = small_gicp.PointCloud(target_raw_numpy)
|
||||
source_raw = small_gicp.PointCloud(source_raw_numpy)
|
||||
|
||||
# Downsampling
|
||||
target = small_gicp.voxelgrid_sampling(target_raw, 0.25)
|
||||
source = small_gicp.voxelgrid_sampling(source_raw, 0.25)
|
||||
|
||||
# KdTree construction
|
||||
target_tree = small_gicp.KdTree(target)
|
||||
source_tree = small_gicp.KdTree(source)
|
||||
|
||||
# Estimate covariances
|
||||
small_gicp.estimate_covariances(target, target_tree)
|
||||
small_gicp.estimate_covariances(source, source_tree)
|
||||
|
||||
# Align point clouds
|
||||
result = small_gicp.align(target, source, target_tree)
|
||||
|
||||
return result.T_target_source
|
||||
|
||||
|
||||
### Following functions are for testing ###
|
||||
|
||||
# Verity the estimated transformation matrix (for testing)
|
||||
def verify_result(T_target_source, gt_T_target_source):
|
||||
error = numpy.linalg.inv(T_target_source) @ gt_T_target_source
|
||||
error_trans = numpy.linalg.norm(error[:3, 3])
|
||||
error_rot = Rotation.from_matrix(error[:3, :3]).magnitude()
|
||||
|
||||
assert error_trans < 0.05
|
||||
assert error_rot < 0.05
|
||||
|
||||
import pytest
|
||||
|
||||
# Load the point clouds and the ground truth transformation matrix
|
||||
@pytest.fixture(scope='module', autouse=True)
|
||||
def load_points():
|
||||
gt_T_target_source = numpy.loadtxt('data/T_target_source.txt') # Load the ground truth transformation matrix
|
||||
target_raw = small_gicp.read_ply(('data/target.ply')) # Read the target point cloud (small_gicp.PointCloud)
|
||||
source_raw = small_gicp.read_ply(('data/source.ply')) # Read the source point cloud (small_gicp.PointCloud)
|
||||
|
||||
target_raw_numpy = target_raw.points() # Nx4 numpy array of the target point cloud
|
||||
source_raw_numpy = source_raw.points() # Nx4 numpy array of the source point cloud
|
||||
|
||||
yield (gt_T_target_source, target_raw_numpy, source_raw_numpy)
|
||||
|
||||
# Check if the point clouds are loaded correctly
|
||||
def test_load_points(load_points):
|
||||
gt_T_target_source, target_raw_numpy, source_raw_numpy = load_points
|
||||
assert gt_T_target_source.shape[0] == 4 and gt_T_target_source.shape[1] == 4
|
||||
assert len(target_raw_numpy) > 0 and target_raw_numpy.shape[1] == 4
|
||||
assert len(source_raw_numpy) > 0 and source_raw_numpy.shape[1] == 4
|
||||
|
||||
# Basic point cloud test
|
||||
def test_points(load_points):
|
||||
_, points_numpy, _ = load_points
|
||||
|
||||
points = small_gicp.PointCloud(points_numpy)
|
||||
assert points.size() == points_numpy.shape[0]
|
||||
assert numpy.all(numpy.abs(points.points() - points_numpy) < 1e-6)
|
||||
|
||||
points = small_gicp.PointCloud(points_numpy[:, :3])
|
||||
assert points.size() == points_numpy.shape[0]
|
||||
assert numpy.all(numpy.abs(points.points() - points_numpy) < 1e-6)
|
||||
|
||||
for i in range(10):
|
||||
assert numpy.all(numpy.abs(points.point(i) - points_numpy[i]) < 1e-6)
|
||||
|
||||
|
||||
# Downsampling test
|
||||
def test_downsampling(load_points):
|
||||
_, points_numpy, _ = load_points
|
||||
|
||||
downsampled = small_gicp.voxelgrid_sampling(points_numpy, 0.25)
|
||||
assert downsampled.size() > 0
|
||||
|
||||
downsampled2 = small_gicp.voxelgrid_sampling(points_numpy, 0.25, num_threads=2)
|
||||
assert abs(1.0 - downsampled.size() / downsampled2.size()) < 0.05
|
||||
|
||||
downsampled2 = small_gicp.voxelgrid_sampling(small_gicp.PointCloud(points_numpy), 0.25)
|
||||
assert downsampled.size() == downsampled2.size()
|
||||
|
||||
downsampled2 = small_gicp.voxelgrid_sampling(small_gicp.PointCloud(points_numpy), 0.25, num_threads=2)
|
||||
assert abs(1.0 - downsampled.size() / downsampled2.size()) < 0.05
|
||||
|
||||
# Preprocess test
|
||||
def test_preprocess(load_points):
|
||||
_, points_numpy, _ = load_points
|
||||
|
||||
downsampled, _ = small_gicp.preprocess_points(points_numpy, downsampling_resolution=0.25)
|
||||
assert downsampled.size() > 0
|
||||
|
||||
downsampled2, _ = small_gicp.preprocess_points(points_numpy, downsampling_resolution=0.25, num_threads=2)
|
||||
assert abs(1.0 - downsampled.size() / downsampled2.size()) < 0.05
|
||||
|
||||
downsampled2, _ = small_gicp.preprocess_points(small_gicp.PointCloud(points_numpy), downsampling_resolution=0.25)
|
||||
assert downsampled.size() == downsampled2.size()
|
||||
|
||||
downsampled2, _ = small_gicp.preprocess_points(small_gicp.PointCloud(points_numpy), downsampling_resolution=0.25, num_threads=2)
|
||||
assert abs(1.0 - downsampled.size() / downsampled2.size()) < 0.05
|
||||
|
||||
# Voxelmap test
|
||||
def test_voxelmap(load_points):
|
||||
_, points_numpy, _ = load_points
|
||||
|
||||
downsampled = small_gicp.voxelgrid_sampling(points_numpy, 0.25)
|
||||
small_gicp.estimate_covariances(downsampled)
|
||||
|
||||
voxelmap = small_gicp.GaussianVoxelMap(0.5)
|
||||
voxelmap.insert(downsampled)
|
||||
|
||||
assert voxelmap.size() > 0
|
||||
assert voxelmap.size() == len(voxelmap)
|
||||
|
||||
# Registration test
|
||||
def test_registration(load_points):
|
||||
gt_T_target_source, target_raw_numpy, source_raw_numpy = load_points
|
||||
|
||||
result = small_gicp.align(target_raw_numpy, source_raw_numpy, downsampling_resolution=0.25)
|
||||
verify_result(result.T_target_source, gt_T_target_source)
|
||||
|
||||
result = small_gicp.align(target_raw_numpy, source_raw_numpy, downsampling_resolution=0.25, num_threads=2)
|
||||
verify_result(result.T_target_source, gt_T_target_source)
|
||||
|
||||
target, target_tree = small_gicp.preprocess_points(target_raw_numpy, downsampling_resolution=0.25)
|
||||
source, source_tree = small_gicp.preprocess_points(source_raw_numpy, downsampling_resolution=0.25)
|
||||
|
||||
result = small_gicp.align(target, source)
|
||||
verify_result(result.T_target_source, gt_T_target_source)
|
||||
|
||||
result = small_gicp.align(target, source, target_tree)
|
||||
verify_result(result.T_target_source, gt_T_target_source)
|
||||
|
||||
target_voxelmap = small_gicp.GaussianVoxelMap(0.5)
|
||||
target_voxelmap.insert(target)
|
||||
|
||||
result = small_gicp.align(target_voxelmap, source)
|
||||
verify_result(result.T_target_source, gt_T_target_source)
|
||||
Loading…
Reference in New Issue