Developer Documentation
 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 //=============================================================================
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