1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
|
#cython: embedsignature=True
#
# Copyright 2014 Netherlands eScience Center
from libcpp cimport bool
cimport numpy as np
import numpy as np
cimport _pcl
cimport pcl_defs as cpp
np.import_array()
cdef extern from "pcl/registration/registration.h" namespace "pcl" nogil:
cdef cppclass Registration[Source, Target]:
cppclass Matrix4:
float *data()
void align(cpp.PointCloud[Source] &) except +
Matrix4 getFinalTransformation() except +
double getFitnessScore() except +
bool hasConverged() except +
void setInputSource(cpp.PointCloudPtr_t) except +
void setInputTarget(cpp.PointCloudPtr_t) except +
void setMaximumIterations(int) except +
cdef extern from "pcl/registration/icp.h" namespace "pcl" nogil:
cdef cppclass IterativeClosestPoint[Source, Target](Registration[Source, Target]):
IterativeClosestPoint() except +
cdef extern from "pcl/registration/gicp.h" namespace "pcl" nogil:
cdef cppclass GeneralizedIterativeClosestPoint[Source, Target](Registration[Source, Target]):
GeneralizedIterativeClosestPoint() except +
cdef extern from "pcl/registration/icp_nl.h" namespace "pcl" nogil:
cdef cppclass IterativeClosestPointNonLinear[Source, Target](Registration[Source, Target]):
IterativeClosestPointNonLinear() except +
cdef object run(Registration[cpp.PointXYZ, cpp.PointXYZ] ®,
_pcl.PointCloud source, _pcl.PointCloud target, max_iter=None):
reg.setInputSource(source.thisptr_shared)
reg.setInputTarget(target.thisptr_shared)
if max_iter is not None:
reg.setMaximumIterations(max_iter)
cdef _pcl.PointCloud result = _pcl.PointCloud()
with nogil:
reg.align(result.thisptr()[0])
# Get transformation matrix and convert from Eigen to NumPy format.
cdef Registration[cpp.PointXYZ, cpp.PointXYZ].Matrix4 mat
mat = reg.getFinalTransformation()
cdef np.ndarray[dtype=np.float32_t, ndim=2, mode='fortran'] transf
cdef np.float32_t *transf_data
transf = np.empty((4, 4), dtype=np.float32, order='fortran')
transf_data = <np.float32_t *>np.PyArray_DATA(transf)
for i in range(16):
transf_data[i] = mat.data()[i]
return reg.hasConverged(), transf, result, reg.getFitnessScore()
def icp(_pcl.PointCloud source, _pcl.PointCloud target, max_iter=None):
"""Align source to target using iterative closest point (ICP).
Parameters
----------
source : PointCloud
Source point cloud.
target : PointCloud
Target point cloud.
max_iter : integer, optional
Maximum number of iterations. If not given, uses the default number
hardwired into PCL.
Returns
-------
converged : bool
Whether the ICP algorithm converged in at most max_iter steps.
transf : np.ndarray, shape = [4, 4]
Transformation matrix.
estimate : PointCloud
Transformed version of source.
fitness : float
Sum of squares error in the estimated transformation.
"""
cdef IterativeClosestPoint[cpp.PointXYZ, cpp.PointXYZ] icp
return run(icp, source, target, max_iter)
def gicp(_pcl.PointCloud source, _pcl.PointCloud target, max_iter=None):
"""Align source to target using generalized iterative closest point (GICP).
Parameters
----------
source : PointCloud
Source point cloud.
target : PointCloud
Target point cloud.
max_iter : integer, optional
Maximum number of iterations. If not given, uses the default number
hardwired into PCL.
Returns
-------
converged : bool
Whether the ICP algorithm converged in at most max_iter steps.
transf : np.ndarray, shape = [4, 4]
Transformation matrix.
estimate : PointCloud
Transformed version of source.
fitness : float
Sum of squares error in the estimated transformation.
"""
cdef GeneralizedIterativeClosestPoint[cpp.PointXYZ, cpp.PointXYZ] gicp
return run(gicp, source, target, max_iter)
def icp_nl(_pcl.PointCloud source, _pcl.PointCloud target, max_iter=None):
"""Align source to target using generalized non-linear ICP (ICP-NL).
Parameters
----------
source : PointCloud
Source point cloud.
target : PointCloud
Target point cloud.
max_iter : integer, optional
Maximum number of iterations. If not given, uses the default number
hardwired into PCL.
Returns
-------
converged : bool
Whether the ICP algorithm converged in at most max_iter steps.
transf : np.ndarray, shape = [4, 4]
Transformation matrix.
estimate : PointCloud
Transformed version of source.
fitness : float
Sum of squares error in the estimated transformation.
"""
cdef IterativeClosestPointNonLinear[cpp.PointXYZ, cpp.PointXYZ] icp_nl
return run(icp_nl, source, target, max_iter)
|
