Developer Documentation
Loading...
Searching...
No Matches
ICPT_impl.hh
1/*===========================================================================*\
2* *
3* OpenFlipper *
4 * Copyright (c) 2001-2015, RWTH-Aachen University *
5 * Department of Computer Graphics and Multimedia *
6 * All rights reserved. *
7 * www.openflipper.org *
8 * *
9 *---------------------------------------------------------------------------*
10 * This file is part of OpenFlipper. *
11 *---------------------------------------------------------------------------*
12 * *
13 * Redistribution and use in source and binary forms, with or without *
14 * modification, are permitted provided that the following conditions *
15 * are met: *
16 * *
17 * 1. Redistributions of source code must retain the above copyright notice, *
18 * this list of conditions and the following disclaimer. *
19 * *
20 * 2. Redistributions in binary form must reproduce the above copyright *
21 * notice, this list of conditions and the following disclaimer in the *
22 * documentation and/or other materials provided with the distribution. *
23 * *
24 * 3. Neither the name of the copyright holder nor the names of its *
25 * contributors may be used to endorse or promote products derived from *
26 * this software without specific prior written permission. *
27 * *
28 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS *
29 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED *
30 * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A *
31 * PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER *
32 * OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, *
33 * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, *
34 * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR *
35 * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF *
36 * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING *
37 * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS *
38 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. *
39* *
40\*===========================================================================*/
41
42
43
44
45
46
47//=============================================================================
48//
49// CLASS ICP - IMPLEMENTATION
50//
51//=============================================================================
52
53#define ICP_C
54
55//== INCLUDES =================================================================
56
57#include "ICP.hh"
58#include <cfloat>
59
60
61//== NAMESPACES ===============================================================
62
63namespace ICP {
64
65//== IMPLEMENTATION ==========================================================
66
67template < typename VectorT >
68inline
69VectorT
70center_of_gravity(const std::vector< VectorT >& _points ) {
71 VectorT cog(0.0,0.0,0.0);
72
73 for (uint i = 0 ; i < _points.size() ; ++i ) {
74 cog = cog + _points[i];
75 }
76
77 cog = cog / ( typename VectorT::value_type )_points.size();
78
79 return cog;
80}
81
82
83template < typename VectorT , typename QuaternionT >
84void
85icp(const std::vector< VectorT >& _points1 , const std::vector< VectorT >& _points2 , VectorT& _cog1 , VectorT& _cog2 , double& _scale1 , double& _scale2 , QuaternionT& _rotation )
86{
87 // compute Mean of Points
88 _cog1 = center_of_gravity(_points1);
89 _cog2 = center_of_gravity(_points2);
90
91
92 VectorT diff;
93 _scale1 = 0.0;
94 _scale2 = 0.0;
95 for ( uint i = 0 ; i < _points1.size() ; ++i ) {
96 diff = _points1[i] - _cog1;
97 _scale1 = std::max( _scale1 , sqrt( diff.norm() ) );
98 diff = _points2[i] - _cog2;
99 _scale2 = std::max( _scale2 , sqrt( diff.norm() ) );
100 }
101
102 double Sxx = 0.0;
103 double Sxy = 0.0;
104 double Sxz = 0.0;
105
106 double Syx = 0.0;
107 double Syy = 0.0;
108 double Syz = 0.0;
109
110 double Szx = 0.0;
111 double Szy = 0.0;
112 double Szz = 0.0;
113
114 for ( uint i = 0 ; i < _points1.size() ; ++i ) {
115 Sxx += ( _points1[i][0] - _cog1[0] ) * ( _points2[i][0] - _cog2[0] );
116 Sxy += ( _points1[i][0] - _cog1[0] ) * ( _points2[i][1] - _cog2[1] );
117 Sxz += ( _points1[i][0] - _cog1[0] ) * ( _points2[i][2] - _cog2[2] );
118
119 Syx += ( _points1[i][1] - _cog1[1] ) * ( _points2[i][0] - _cog2[0] );
120 Syy += ( _points1[i][1] - _cog1[1] ) * ( _points2[i][1] - _cog2[1] );
121 Syz += ( _points1[i][1] - _cog1[1] ) * ( _points2[i][2] - _cog2[2] );
122
123 Szx += ( _points1[i][2] - _cog1[2] ) * ( _points2[i][0] - _cog2[0] );
124 Szy += ( _points1[i][2] - _cog1[2] ) * ( _points2[i][1] - _cog2[1] );
125 Szz += ( _points1[i][2] - _cog1[2] ) * ( _points2[i][2] - _cog2[2] );
126 }
127
128 const double scale = _scale1 * _scale2;
129
130 Sxx = Sxx * 1.0 / scale;
131 Sxy = Sxy * 1.0 / scale;
132 Sxz = Sxz * 1.0 / scale;
133 Syx = Syx * 1.0 / scale;
134 Syy = Syy * 1.0 / scale;
135 Syz = Syz * 1.0 / scale;
136 Szx = Szx * 1.0 / scale;
137 Szy = Szy * 1.0 / scale;
138 Szz = Szz * 1.0 / scale;
139
140 gmm::dense_matrix<double> N; gmm::resize(N,4,4); gmm::clear(N);
141 N(0,0) = Sxx + Syy + Szz;
142 N(0,1) = Syz - Szy;
143 N(0,2) = Szx - Sxz;
144 N(0,3) = Sxy - Syx;
145
146 N(1,0) = Syz - Szy;
147 N(1,1) = Sxx - Syy - Szz;
148 N(1,2) = Sxy + Syx;
149 N(1,3) = Szx + Sxz;
150
151 N(2,0) = Szx - Sxz;
152 N(2,1) = Sxy + Syx;
153 N(2,2) = -Sxx + Syy - Szz;
154 N(2,3) = Syz + Szy;
155
156 N(3,0) = Sxy - Syx;
157 N(3,1) = Szx + Sxz;
158 N(3,2) = Syz + Szy;
159 N(3,3) = -Sxx - Syy + Szz;
160
161 std::vector< double > eigval; eigval.resize(4);
162 gmm::dense_matrix< double > eigvect; gmm::resize(eigvect, 4,4); gmm::clear(eigvect);
163 gmm::symmetric_qr_algorithm(N, eigval, eigvect);
164
165 int gr = 0;
166 double max = -FLT_MAX;
167 for (int i = 0; i < 4; i++)
168 if (eigval[i] > max)
169 {
170 gr = i;
171 max = eigval[i];
172 }
173
174 _rotation[0] = eigvect(0,gr);
175 _rotation[1] = eigvect(1,gr);
176 _rotation[2] = eigvect(2,gr);
177 _rotation[3] = eigvect(3,gr);
178
179 _scale1 *= _scale1;
180 _scale2 *= _scale2;
181}
182
183template < typename VectorT , typename QuaternionT >
184void
185icp(const std::vector< VectorT >& _points1 , const std::vector< VectorT >& _points2 , VectorT& _cog1 , VectorT& _cog2 , QuaternionT& _rotation )
186{
187 // compute Mean of Points
188 _cog1 = center_of_gravity(_points1);
189 _cog2 = center_of_gravity(_points2);
190
191 double Sxx = 0.0;
192 double Sxy = 0.0;
193 double Sxz = 0.0;
194 double Syx = 0.0;
195 double Syy = 0.0;
196 double Syz = 0.0;
197 double Szx = 0.0;
198 double Szy = 0.0;
199 double Szz = 0.0;
200
201 for ( uint i = 0 ; i < _points1.size() ; ++i ) {
202 Sxx += ( _points1[i][0] - _cog1[0] ) * ( _points2[i][0] - _cog2[0] );
203 Sxy += ( _points1[i][0] - _cog1[0] ) * ( _points2[i][1] - _cog2[1] );
204 Sxz += ( _points1[i][0] - _cog1[0] ) * ( _points2[i][2] - _cog2[2] );
205
206 Syx += ( _points1[i][1] - _cog1[1] ) * ( _points2[i][0] - _cog2[0] );
207 Syy += ( _points1[i][1] - _cog1[1] ) * ( _points2[i][1] - _cog2[1] );
208 Syz += ( _points1[i][1] - _cog1[1] ) * ( _points2[i][2] - _cog2[2] );
209
210 Szx += ( _points1[i][2] - _cog1[2] ) * ( _points2[i][0] - _cog2[0] );
211 Szy += ( _points1[i][2] - _cog1[2] ) * ( _points2[i][1] - _cog2[1] );
212 Szz += ( _points1[i][2] - _cog1[2] ) * ( _points2[i][2] - _cog2[2] );
213 }
214
215 gmm::dense_matrix<double> N; gmm::resize(N,4,4); gmm::clear(N);
216 N(0,0) = Sxx + Syy + Szz;
217 N(0,1) = Syz - Szy;
218 N(0,2) = Szx - Sxz;
219 N(0,3) = Sxy - Syx;
220
221 N(1,0) = Syz - Szy;
222 N(1,1) = Sxx - Syy - Szz;
223 N(1,2) = Sxy + Syx;
224 N(1,3) = Szx + Sxz;
225
226 N(2,0) = Szx - Sxz;
227 N(2,1) = Sxy + Syx;
228 N(2,2) = -Sxx + Syy - Szz;
229 N(2,3) = Syz + Szy;
230
231 N(3,0) = Sxy - Syx;
232 N(3,1) = Szx + Sxz;
233 N(3,2) = Syz + Szy;
234 N(3,3) = -Sxx - Syy + Szz;
235
236 std::vector< double > eigval; eigval.resize(4);
237 gmm::dense_matrix< double > eigvect; gmm::resize(eigvect, 4,4); gmm::clear(eigvect);
238 gmm::symmetric_qr_algorithm(N, eigval, eigvect);
239
240 int gr = 0;
241 double max = -FLT_MAX;
242 for (int i = 0; i < 4; i++)
243 if (eigval[i] > max)
244 {
245 gr = i;
246 max = eigval[i];
247 }
248
249 _rotation[0] = eigvect(0,gr);
250 _rotation[1] = eigvect(1,gr);
251 _rotation[2] = eigvect(2,gr);
252 _rotation[3] = eigvect(3,gr);
253
254}
255
256
257//=============================================================================
258
259} //namespace ICP
260
261//=============================================================================
ICP.hh
Classes for ICP Algorithm.
ICP
Namespace for ICP.
Definition ICP.hh:72
ICP::icp
void icp(const std::vector< VectorT > &_points1, const std::vector< VectorT > &_points2, VectorT &_cog1, VectorT &_cog2, double &_scale1, double &_scale2, QuaternionT &_rotation)
Compute rigid transformation from first point set to second point set.
Definition ICPT_impl.hh:85