Skip to content

Commit 62f224f

Browse files
koide3koide3
committed
add python bindings
1 parent 7798ac5 commit 62f224f

File tree

6 files changed

+364
-0
lines changed

6 files changed

+364
-0
lines changed

‎.gitignore

+5
Original file line numberDiff line numberDiff line change
@@ -1,3 +1,8 @@
11
.vscode/*
22
build/*
3+
dist/*
4+
5+
*.so
6+
*.py[cod]
7+
*.egg-info
38
README.html

‎.gitmodules

+3
Original file line numberDiff line numberDiff line change
@@ -7,3 +7,6 @@
77
[submodule "thirdparty/nvbio"]
88
path = thirdparty/nvbio
99
url = https://github.com/NVlabs/nvbio.git
10+
[submodule "thirdparty/pybind11"]
11+
path = thirdparty/pybind11
12+
url = https://github.com/pybind/pybind11.git

‎CMakeLists.txt

+17
Original file line numberDiff line numberDiff line change
@@ -4,6 +4,7 @@ project(fast_gicp)
44
option(BUILD_VGICP_CUDA "Build GPU-powered VGICP" OFF)
55
option(BUILD_apps "Build application programs" ON)
66
option(BUILD_test "Build test programs" OFF)
7+
option(BUILD_PYTHON_BINDINGS "Build python bindings" OFF)
78

89
add_definitions(-msse -msse2 -msse3 -msse4 -msse4.1 -msse4.2)
910
set(CMAKE_C_FLAGS "-msse -msse2 -msse3 -msse4 -msse4.1 -msse4.2")
@@ -73,6 +74,22 @@ if(BUILD_apps)
7374
)
7475
endif()
7576

77+
### Python bindings ###
78+
if(BUILD_PYTHON_BINDINGS)
79+
add_subdirectory(thirdparty/pybind11)
80+
pybind11_add_module(pygicp
81+
src/python/main.cpp
82+
)
83+
target_include_directories(pygicp PUBLIC
84+
include
85+
${PCL_INCLUDE_DIRS}
86+
${EIGEN3_INCLUDE_DIR}
87+
)
88+
target_link_libraries(pygicp PRIVATE
89+
fast_gicp
90+
)
91+
endif()
92+
7693
### CUDA ###
7794
if(BUILD_VGICP_CUDA)
7895
set(CUDA_NVCC_FLAGS "--expt-relaxed-constexpr")

‎setup.py

+116
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,116 @@
1+
# -*- coding: utf-8 -*-
2+
import os
3+
import sys
4+
import glob
5+
import subprocess
6+
7+
from setuptools import setup, Extension
8+
from setuptools.command.build_ext import build_ext
9+
10+
# Convert distutils Windows platform specifiers to CMake -A arguments
11+
PLAT_TO_CMAKE = {
12+
"win32": "Win32",
13+
"win-amd64": "x64",
14+
"win-arm32": "ARM",
15+
"win-arm64": "ARM64",
16+
}
17+
18+
19+
# A CMakeExtension needs a sourcedir instead of a file list.
20+
# The name must be the _single_ output extension from the CMake build.
21+
# If you need multiple extensions, see scikit-build.
22+
class CMakeExtension(Extension):
23+
def __init__(self, name, sourcedir=""):
24+
Extension.__init__(self, name, sources=[])
25+
self.sourcedir = os.path.abspath(sourcedir)
26+
27+
28+
class CMakeBuild(build_ext):
29+
def build_extension(self, ext):
30+
extdir = os.path.abspath(os.path.dirname(self.get_ext_fullpath(ext.name)))
31+
32+
# required for auto-detection of auxiliary "native" libs
33+
if not extdir.endswith(os.path.sep):
34+
extdir += os.path.sep
35+
36+
# cfg = "Debug" if self.debug else "Release"
37+
cfg = "Release"
38+
39+
# CMake lets you override the generator - we need to check this.
40+
# Can be set with Conda-Build, for example.
41+
cmake_generator = os.environ.get("CMAKE_GENERATOR", "")
42+
43+
# Set Python_EXECUTABLE instead if you use PYBIND11_FINDPYTHON
44+
# EXAMPLE_VERSION_INFO shows you how to pass a value into the C++ code
45+
# from Python.
46+
cmake_args = [
47+
"-DCMAKE_LIBRARY_OUTPUT_DIRECTORY={}".format(extdir),
48+
"-DPYTHON_EXECUTABLE={}".format(sys.executable),
49+
"-DEXAMPLE_VERSION_INFO={}".format(self.distribution.get_version()),
50+
"-DCMAKE_BUILD_TYPE={}".format(cfg), # not used on MSVC, but no harm,
51+
"-DBUILD_PYTHON_BINDINGS=ON",
52+
]
53+
build_args = []
54+
55+
if self.compiler.compiler_type != "msvc":
56+
# Using Ninja-build since it a) is available as a wheel and b)
57+
# multithreads automatically. MSVC would require all variables be
58+
# exported for Ninja to pick it up, which is a little tricky to do.
59+
# Users can override the generator with CMAKE_GENERATOR in CMake
60+
# 3.15+.
61+
if not cmake_generator:
62+
pass
63+
# cmake_args += ["-GNinja"]
64+
65+
else:
66+
# Single config generators are handled "normally"
67+
single_config = any(x in cmake_generator for x in {"NMake", "Ninja"})
68+
69+
# CMake allows an arch-in-generator style for backward compatibility
70+
contains_arch = any(x in cmake_generator for x in {"ARM", "Win64"})
71+
72+
# Specify the arch if using MSVC generator, but only if it doesn't
73+
# contain a backward-compatibility arch spec already in the
74+
# generator name.
75+
if not single_config and not contains_arch:
76+
cmake_args += ["-A", PLAT_TO_CMAKE[self.plat_name]]
77+
78+
# Multi-config generators have a different way to specify configs
79+
if not single_config:
80+
cmake_args += [
81+
"-DCMAKE_LIBRARY_OUTPUT_DIRECTORY_{}={}".format(cfg.upper(), extdir)
82+
]
83+
build_args += ["--config", cfg]
84+
85+
# Set CMAKE_BUILD_PARALLEL_LEVEL to control the parallel build level
86+
# across all generators.
87+
if "CMAKE_BUILD_PARALLEL_LEVEL" not in os.environ:
88+
# self.parallel is a Python 3 only way to set parallel jobs by hand
89+
# using -j in the build_ext call, not supported by pip or PyPA-build.
90+
if hasattr(self, "parallel") and self.parallel:
91+
# CMake 3.12+ only.
92+
build_args += ["-j{}".format(self.parallel)]
93+
94+
if not os.path.exists(self.build_temp):
95+
os.makedirs(self.build_temp)
96+
97+
subprocess.check_call(
98+
["cmake", ext.sourcedir] + cmake_args, cwd=self.build_temp
99+
)
100+
subprocess.check_call(
101+
["cmake", "--build", "."] + build_args, cwd=self.build_temp
102+
)
103+
104+
# The information here can also be placed in setup.cfg - better separation of
105+
# logic and declaration, and simpler if you include description/version in a file.
106+
setup(
107+
name="pygicp",
108+
version="0.0.1",
109+
author="k.koide",
110+
author_email="[email protected]",
111+
description="A collection of GICP-based point cloud registration algorithms",
112+
long_description="",
113+
ext_modules=[CMakeExtension("pygicp")],
114+
cmdclass={"build_ext": CMakeBuild},
115+
zip_safe=False,
116+
)

‎src/python/main.cpp

+222
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,222 @@
1+
#include <pybind11/pybind11.h>
2+
#include <pybind11/numpy.h>
3+
#include <pybind11/eigen.h>
4+
#include <pybind11/functional.h>
5+
6+
#include <boost/filesystem.hpp>
7+
#include <fast_gicp/gicp/fast_gicp.hpp>
8+
#include <fast_gicp/gicp/fast_vgicp.hpp>
9+
10+
#ifdef USE_VGICP_CUDA
11+
#include <fast_gicp/ndt/ndt_cuda.hpp>
12+
#include <fast_gicp/gicp/fast_vgicp_cuda.hpp>
13+
#endif
14+
15+
#include <pcl/point_types.h>
16+
#include <pcl/point_cloud.h>
17+
#include <pcl/filters/approximate_voxel_grid.h>
18+
19+
namespace py = pybind11;
20+
21+
fast_gicp::NeighborSearchMethod search_method(const std::string& neighbor_search_method) {
22+
if(neighbor_search_method == "DIRECT1") {
23+
return fast_gicp::NeighborSearchMethod::DIRECT1;
24+
} else if (neighbor_search_method == "DIRECT7") {
25+
return fast_gicp::NeighborSearchMethod::DIRECT7;
26+
} else if (neighbor_search_method == "DIRECT27") {
27+
return fast_gicp::NeighborSearchMethod::DIRECT27;
28+
} else if (neighbor_search_method == "DIRECT_RADIUS") {
29+
return fast_gicp::NeighborSearchMethod::DIRECT_RADIUS;
30+
}
31+
32+
std::cerr << "error: unknown neighbor search method " << neighbor_search_method << std::endl;
33+
return fast_gicp::NeighborSearchMethod::DIRECT1;
34+
}
35+
36+
pcl::PointCloud<pcl::PointXYZ>::Ptr eigen2pcl(const Eigen::Matrix<double, -1, 3>& points) {
37+
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
38+
cloud->resize(points.rows());
39+
40+
for(int i=0; i<points.rows(); i++) {
41+
cloud->at(i).getVector3fMap() = points.row(i).cast<float>();
42+
}
43+
return cloud;
44+
}
45+
46+
Eigen::Matrix<double, -1, 3> downsample(const Eigen::Matrix<double, -1, 3>& points, double downsample_resolution) {
47+
auto cloud = eigen2pcl(points);
48+
49+
pcl::ApproximateVoxelGrid<pcl::PointXYZ> voxelgrid;
50+
voxelgrid.setLeafSize(downsample_resolution, downsample_resolution, downsample_resolution);
51+
52+
pcl::PointCloud<pcl::PointXYZ>::Ptr filtered(new pcl::PointCloud<pcl::PointXYZ>);
53+
voxelgrid.filter(*filtered);
54+
55+
Eigen::Matrix<float, -1, 3> filtered_points(filtered->size(), 3);
56+
for(int i=0; i<filtered->size(); i++) {
57+
filtered_points.row(i) = filtered->at(i).getVector3fMap();
58+
}
59+
60+
return filtered_points.cast<double>();
61+
}
62+
63+
Eigen::Matrix4d align_points(
64+
const Eigen::Matrix<double, -1, 3>& target,
65+
const Eigen::Matrix<double, -1, 3>& source,
66+
const std::string& method,
67+
double downsample_resolution,
68+
int k_correspondences,
69+
double max_correspondence_distance,
70+
double voxel_resolution,
71+
int num_threads,
72+
const std::string& neighbor_search_method,
73+
double neighbor_search_radius,
74+
const Eigen::Matrix4f& initial_guess
75+
) {
76+
pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud = eigen2pcl(target);
77+
pcl::PointCloud<pcl::PointXYZ>::Ptr source_cloud = eigen2pcl(source);
78+
79+
if(downsample_resolution > 0.0) {
80+
pcl::ApproximateVoxelGrid<pcl::PointXYZ> voxelgrid;
81+
voxelgrid.setLeafSize(downsample_resolution, downsample_resolution, downsample_resolution);
82+
83+
pcl::PointCloud<pcl::PointXYZ>::Ptr filtered(new pcl::PointCloud<pcl::PointXYZ>);
84+
voxelgrid.setInputCloud(target_cloud);
85+
voxelgrid.filter(*filtered);
86+
target_cloud.swap(filtered);
87+
88+
voxelgrid.setInputCloud(source_cloud);
89+
voxelgrid.filter(*filtered);
90+
source_cloud.swap(filtered);
91+
}
92+
93+
boost::shared_ptr<fast_gicp::LsqRegistration<pcl::PointXYZ, pcl::PointXYZ>> reg;
94+
95+
if(method == "GICP") {
96+
pcl::shared_ptr<fast_gicp::FastGICP<pcl::PointXYZ, pcl::PointXYZ>> gicp(new fast_gicp::FastGICP<pcl::PointXYZ, pcl::PointXYZ>);
97+
gicp->setMaxCorrespondenceDistance(max_correspondence_distance);
98+
gicp->setCorrespondenceRandomness(k_correspondences);
99+
gicp->setNumThreads(num_threads);
100+
reg = gicp;
101+
} else if (method == "VGICP") {
102+
pcl::shared_ptr<fast_gicp::FastVGICP<pcl::PointXYZ, pcl::PointXYZ>> vgicp(new fast_gicp::FastVGICP<pcl::PointXYZ, pcl::PointXYZ>);
103+
vgicp->setCorrespondenceRandomness(k_correspondences);
104+
vgicp->setResolution(voxel_resolution);
105+
vgicp->setNeighborSearchMethod(search_method(neighbor_search_method));
106+
vgicp->setNumThreads(num_threads);
107+
reg = vgicp;
108+
} else if (method == "VGICP_CUDA") {
109+
#ifdef USE_VGICP_CUDA
110+
pcl::shared_ptr<fast_gicp::FastVGICPCuda<pcl::PointXYZ, pcl::PointXYZ>> vgicp(new fast_gicp::FastVGICPCuda<pcl::PointXYZ, pcl::PointXYZ>);
111+
vgicp->setCorrespondenceRandomness(k_correspondences);
112+
vgicp->setNeighborSearchMethod(search_method(neighbor_search_method), neighbor_search_radius);
113+
vgicp->setResolution(voxel_resolution);
114+
reg = vgicp;
115+
#else
116+
std::cerr << "error: you need to build fast_gicp with BUILD_VGICP_CUDA=ON"
117+
return Eigen::Matrix4d::Identity();
118+
#endif
119+
} else if (method == "NDT_CUDA") {
120+
#ifdef USE_VGICP_CUDA
121+
pcl::shared_ptr<fast_gicp::NDTCuda<pcl::PointXYZ, pcl::PointXYZ>> ndt(new fast_gicp::NDTCuda<pcl::PointXYZ, pcl::PointXYZ>);
122+
ndt->setResolution(voxel_resolution);
123+
ndt->setNeighborSearchMethod(search_method(neighbor_search_method), neighbor_search_radius);
124+
reg = ndt;
125+
#else
126+
std::cerr << "error: you need to build fast_gicp with BUILD_VGICP_CUDA=ON"
127+
return Eigen::Matrix4d::Identity();
128+
#endif
129+
} else {
130+
std::cerr << "error: unknown registration method " << method << std::endl;
131+
return Eigen::Matrix4d::Identity();
132+
}
133+
134+
reg->setInputTarget(target_cloud);
135+
reg->setInputSource(source_cloud);
136+
137+
pcl::PointCloud<pcl::PointXYZ>::Ptr aligned(new pcl::PointCloud<pcl::PointXYZ>);
138+
reg->align(*aligned, initial_guess);
139+
140+
return reg->getFinalTransformation().cast<double>();
141+
}
142+
143+
using LsqRegistration = fast_gicp::LsqRegistration<pcl::PointXYZ, pcl::PointXYZ>;
144+
using FastGICP = fast_gicp::FastGICP<pcl::PointXYZ, pcl::PointXYZ>;
145+
using FastVGICP = fast_gicp::FastVGICP<pcl::PointXYZ, pcl::PointXYZ>;
146+
#ifdef USE_VGICP_CUDA
147+
using FastVGICPCuda = fast_gicp::FastVGICPCuda<pcl::PointXYZ, pcl::PointXYZ>;
148+
using NDTCuda = fast_gicp::NDTCuda<pcl::PointXYZ, pcl::PointXYZ>;
149+
#endif
150+
151+
PYBIND11_MODULE(pygicp, m) {
152+
m.def("downsample", &downsample, "downsample points");
153+
154+
m.def("align_points", &align_points, "align two point sets",
155+
py::arg("target"),
156+
py::arg("source"),
157+
py::arg("method") = "GICP",
158+
py::arg("downsample_resolution") = -1.0,
159+
py::arg("k_correspondences") = 15,
160+
py::arg("max_correspondence_distance") = std::numeric_limits<double>::max(),
161+
py::arg("voxel_resolution") = 1.0,
162+
py::arg("num_threads") = 0,
163+
py::arg("neighbor_search_method") = "DIRECT1",
164+
py::arg("neighbor_search_radius") = 1.5,
165+
py::arg("initial_guess") = Eigen::Matrix4f::Identity()
166+
);
167+
168+
py::class_<LsqRegistration, std::shared_ptr<LsqRegistration>>(m, "LsqRegistration")
169+
.def("set_input_target", [] (LsqRegistration& reg, const Eigen::Matrix<double, -1, 3>& points) { reg.setInputTarget(eigen2pcl(points)); })
170+
.def("set_input_source", [] (LsqRegistration& reg, const Eigen::Matrix<double, -1, 3>& points) { reg.setInputSource(eigen2pcl(points)); })
171+
.def("swap_source_and_target", &LsqRegistration::swapSourceAndTarget)
172+
.def("get_final_hessian", &LsqRegistration::getFinalHessian)
173+
.def("get_final_transformation", &LsqRegistration::getFinalTransformation)
174+
.def("align",
175+
[] (LsqRegistration& reg, const Eigen::Matrix4f& initial_guess) {
176+
pcl::PointCloud<pcl::PointXYZ> aligned;
177+
reg.align(aligned, initial_guess);
178+
return reg.getFinalTransformation();
179+
}, py::arg("initial_guess") = Eigen::Matrix4f::Identity()
180+
)
181+
;
182+
183+
py::class_<FastGICP, LsqRegistration, std::shared_ptr<FastGICP>>(m, "FastGICP")
184+
.def(py::init())
185+
.def("set_num_threads", &FastGICP::setNumThreads)
186+
.def("set_correspondence_randomness", &FastGICP::setCorrespondenceRandomness)
187+
.def("set_max_correspondence_distance", &FastGICP::setMaxCorrespondenceDistance)
188+
;
189+
190+
py::class_<FastVGICP, FastGICP, std::shared_ptr<FastVGICP>>(m, "FastVGICP")
191+
.def(py::init())
192+
.def("set_resolution", &FastVGICP::setResolution)
193+
.def("set_neighbor_search_method", [](FastVGICP& vgicp, const std::string& method) { vgicp.setNeighborSearchMethod(search_method(method)); })
194+
;
195+
196+
#ifdef USE_VGICP_CUDA
197+
py::class_<FastVGICPCuda, LsqRegistration, std::shared_ptr<FastVGICPCuda>>(m, "FastVGICPCuda")
198+
.def(py::init())
199+
.def("set_resolution", &FastVGICPCuda::setResolution)
200+
.def("set_neighbor_search_method",
201+
[](FastVGICPCuda& vgicp, const std::string& method, double radius) { vgicp.setNeighborSearchMethod(search_method(method), radius); }
202+
, py::arg("method") = "DIRECT1", py::arg("radius") = 1.5
203+
)
204+
.def("set_correspondence_randomness", &FastVGICPCuda::setCorrespondenceRandomness)
205+
;
206+
207+
py::class_<NDTCuda, LsqRegistration, std::shared_ptr<NDTCuda>>(m, "NDTCuda")
208+
.def(py::init())
209+
.def("set_neighbor_search_method",
210+
[](NDTCuda& ndt, const std::string& method, double radius) { ndt.setNeighborSearchMethod(search_method(method), radius); }
211+
, py::arg("method") = "DIRECT1", py::arg("radius") = 1.5
212+
)
213+
.def("set_resolution", &NDTCuda::setResolution)
214+
;
215+
#endif
216+
217+
#ifdef VERSION_INFO
218+
m.attr("__version__") = MACRO_STRINGIFY(VERSION_INFO);
219+
#else
220+
m.attr("__version__") = "dev";
221+
#endif
222+
}

‎thirdparty/pybind11

Submodule pybind11 added at 0e01c24

0 commit comments

Comments
 (0)