Developer Documentation
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 
63 namespace ICP {
64 
65 //== IMPLEMENTATION ==========================================================
66 
67 template < typename VectorT >
68 inline
69 VectorT
70 center_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 
83 template < typename VectorT , typename QuaternionT >
84 void
85 icp(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 
183 template < typename VectorT , typename QuaternionT >
184 void
185 icp(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
Namespace for ICP.
Definition: ICP.hh:72
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: ICPT_impl.hh:85