Developer Documentation

- Main Page
- Related Pages
- Modules
- Namespaces
- Classes
- Files

- File List
- File Members

All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Properties Friends Macros Modules Pages
ICP.cc
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 * $Revision$ *
45 * $LastChangedBy$ *
46 * $Date$ *
47 * *
48 \*===========================================================================*/
49
50
51
52
53 //=============================================================================
54 //
55 // CLASS ICP - IMPLEMENTATION
56 //
57 //=============================================================================
58
59 #define ICP_C
60
61 //== INCLUDES =================================================================
62
63 #include "ICP.hh"
64 #include <cfloat>
65
66
67 //== NAMESPACES ===============================================================
68
69 namespace ICP {
70
71 //== IMPLEMENTATION ==========================================================
72
73 template < typename VectorT >
74 inline
75 VectorT
76 center_of_gravity(const std::vector< VectorT >& _points ) {
77 VectorT cog(0.0,0.0,0.0);
78
79 for (uint i = 0 ; i < _points.size() ; ++i ) {
80 cog = cog + _points[i];
81 }
82
83 cog = cog / ( typename VectorT::value_type )_points.size();
84
85 return cog;
86 }
87
88
89 template < typename VectorT , typename QuaternionT >
90 void
91 icp(const std::vector< VectorT >& _points1 , const std::vector< VectorT >& _points2 , VectorT& _cog1 , VectorT& _cog2 , double& _scale1 , double& _scale2 , QuaternionT& _rotation )
92 {
93 // compute Mean of Points
94 _cog1 = center_of_gravity(_points1);
95 _cog2 = center_of_gravity(_points2);
96
97
98 VectorT diff;
99 _scale1 = 0.0;
100 _scale2 = 0.0;
101 for ( uint i = 0 ; i < _points1.size() ; ++i ) {
102 diff = _points1[i] - _cog1;
103 _scale1 = std::max( _scale1 , sqrt( diff.norm() ) );
104 diff = _points2[i] - _cog2;
105 _scale2 = std::max( _scale2 , sqrt( diff.norm() ) );
106 }
107
108 double Sxx = 0.0;
109 double Sxy = 0.0;
110 double Sxz = 0.0;
111
112 double Syx = 0.0;
113 double Syy = 0.0;
114 double Syz = 0.0;
115
116 double Szx = 0.0;
117 double Szy = 0.0;
118 double Szz = 0.0;
119
120 for ( uint i = 0 ; i < _points1.size() ; ++i ) {
121 Sxx += ( _points1[i][0] - _cog1[0] ) * ( _points2[i][0] - _cog2[0] );
122 Sxy += ( _points1[i][0] - _cog1[0] ) * ( _points2[i][1] - _cog2[1] );
123 Sxz += ( _points1[i][0] - _cog1[0] ) * ( _points2[i][2] - _cog2[2] );
124
125 Syx += ( _points1[i][1] - _cog1[1] ) * ( _points2[i][0] - _cog2[0] );
126 Syy += ( _points1[i][1] - _cog1[1] ) * ( _points2[i][1] - _cog2[1] );
127 Syz += ( _points1[i][1] - _cog1[1] ) * ( _points2[i][2] - _cog2[2] );
128
129 Szx += ( _points1[i][2] - _cog1[2] ) * ( _points2[i][0] - _cog2[0] );
130 Szy += ( _points1[i][2] - _cog1[2] ) * ( _points2[i][1] - _cog2[1] );
131 Szz += ( _points1[i][2] - _cog1[2] ) * ( _points2[i][2] - _cog2[2] );
132 }
133
134 const double scale = _scale1 * _scale2;
135
136 Sxx = Sxx * 1.0 / scale;
137 Sxy = Sxy * 1.0 / scale;
138 Sxz = Sxz * 1.0 / scale;
139 Syx = Syx * 1.0 / scale;
140 Syy = Syy * 1.0 / scale;
141 Syz = Syz * 1.0 / scale;
142 Szx = Szx * 1.0 / scale;
143 Szy = Szy * 1.0 / scale;
144 Szz = Szz * 1.0 / scale;
145
146 gmm::dense_matrix<double> N; gmm::resize(N,4,4); gmm::clear(N);
147 N(0,0) = Sxx + Syy + Szz;
148 N(0,1) = Syz - Szy;
149 N(0,2) = Szx - Sxz;
150 N(0,3) = Sxy - Syx;
151
152 N(1,0) = Syz - Szy;
153 N(1,1) = Sxx - Syy - Szz;
154 N(1,2) = Sxy + Syx;
155 N(1,3) = Szx + Sxz;
156
157 N(2,0) = Szx - Sxz;
158 N(2,1) = Sxy + Syx;
159 N(2,2) = -Sxx + Syy - Szz;
160 N(2,3) = Syz + Szy;
161
162 N(3,0) = Sxy - Syx;
163 N(3,1) = Szx + Sxz;
164 N(3,2) = Syz + Szy;
165 N(3,3) = -Sxx - Syy + Szz;
166
167 std::vector< double > eigval; eigval.resize(4);
168 gmm::dense_matrix< double > eigvect; gmm::resize(eigvect, 4,4); gmm::clear(eigvect);
169 gmm::symmetric_qr_algorithm(N, eigval, eigvect);
170
171 int gr = 0;
172 double max = -FLT_MAX;
173 for (int i = 0; i < 4; i++)
174 if (eigval[i] > max)
175 {
176 gr = i;
177 max = eigval[i];
178 }
179
180 _rotation[0] = eigvect(0,gr);
181 _rotation[1] = eigvect(1,gr);
182 _rotation[2] = eigvect(2,gr);
183 _rotation[3] = eigvect(3,gr);
184
185 _scale1 *= _scale1;
186 _scale2 *= _scale2;
187 }
188
189 template < typename VectorT , typename QuaternionT >
190 void
191 icp(const std::vector< VectorT >& _points1 , const std::vector< VectorT >& _points2 , VectorT& _cog1 , VectorT& _cog2 , QuaternionT& _rotation )
192 {
193 // compute Mean of Points
194 _cog1 = center_of_gravity(_points1);
195 _cog2 = center_of_gravity(_points2);
196
197 double Sxx = 0.0;
198 double Sxy = 0.0;
199 double Sxz = 0.0;
200 double Syx = 0.0;
201 double Syy = 0.0;
202 double Syz = 0.0;
203 double Szx = 0.0;
204 double Szy = 0.0;
205 double Szz = 0.0;
206
207 for ( uint i = 0 ; i < _points1.size() ; ++i ) {
208 Sxx += ( _points1[i][0] - _cog1[0] ) * ( _points2[i][0] - _cog2[0] );
209 Sxy += ( _points1[i][0] - _cog1[0] ) * ( _points2[i][1] - _cog2[1] );
210 Sxz += ( _points1[i][0] - _cog1[0] ) * ( _points2[i][2] - _cog2[2] );
211
212 Syx += ( _points1[i][1] - _cog1[1] ) * ( _points2[i][0] - _cog2[0] );
213 Syy += ( _points1[i][1] - _cog1[1] ) * ( _points2[i][1] - _cog2[1] );
214 Syz += ( _points1[i][1] - _cog1[1] ) * ( _points2[i][2] - _cog2[2] );
215
216 Szx += ( _points1[i][2] - _cog1[2] ) * ( _points2[i][0] - _cog2[0] );
217 Szy += ( _points1[i][2] - _cog1[2] ) * ( _points2[i][1] - _cog2[1] );
218 Szz += ( _points1[i][2] - _cog1[2] ) * ( _points2[i][2] - _cog2[2] );
219 }
220
221 gmm::dense_matrix<double> N; gmm::resize(N,4,4); gmm::clear(N);
222 N(0,0) = Sxx + Syy + Szz;
223 N(0,1) = Syz - Szy;
224 N(0,2) = Szx - Sxz;
225 N(0,3) = Sxy - Syx;
226
227 N(1,0) = Syz - Szy;
228 N(1,1) = Sxx - Syy - Szz;
229 N(1,2) = Sxy + Syx;
230 N(1,3) = Szx + Sxz;
231
232 N(2,0) = Szx - Sxz;
233 N(2,1) = Sxy + Syx;
234 N(2,2) = -Sxx + Syy - Szz;
235 N(2,3) = Syz + Szy;
236
237 N(3,0) = Sxy - Syx;
238 N(3,1) = Szx + Sxz;
239 N(3,2) = Syz + Szy;
240 N(3,3) = -Sxx - Syy + Szz;
241
242 std::vector< double > eigval; eigval.resize(4);
243 gmm::dense_matrix< double > eigvect; gmm::resize(eigvect, 4,4); gmm::clear(eigvect);
244 gmm::symmetric_qr_algorithm(N, eigval, eigvect);
245
246 int gr = 0;
247 double max = -FLT_MAX;
248 for (int i = 0; i < 4; i++)
249 if (eigval[i] > max)
250 {
251 gr = i;
252 max = eigval[i];
253 }
254
255 _rotation[0] = eigvect(0,gr);
256 _rotation[1] = eigvect(1,gr);
257 _rotation[2] = eigvect(2,gr);
258 _rotation[3] = eigvect(3,gr);
259
260 }
261
262
263 //=============================================================================
264
265 } //namespace ICP
266
267 //=============================================================================
VectorT
ICP
Namespace for ICP.
Definition: ICP.cc:69
ICP.hh
Classes for ICP Algorithm.
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: ICP.cc:91