Developer Documentation
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Properties Friends Macros Groups Pages
ICP.cc
1 /*===========================================================================*\
2 * *
3 * OpenFlipper *
4 * Copyright (C) 2001-2011 by Computer Graphics Group, RWTH Aachen *
5 * www.openflipper.org *
6 * *
7 *--------------------------------------------------------------------------- *
8 * This file is part of OpenFlipper. *
9 * *
10 * OpenFlipper is free software: you can redistribute it and/or modify *
11 * it under the terms of the GNU Lesser General Public License as *
12 * published by the Free Software Foundation, either version 3 of *
13 * the License, or (at your option) any later version with the *
14 * following exceptions: *
15 * *
16 * If other files instantiate templates or use macros *
17 * or inline functions from this file, or you compile this file and *
18 * link it with other files to produce an executable, this file does *
19 * not by itself cause the resulting executable to be covered by the *
20 * GNU Lesser General Public License. This exception does not however *
21 * invalidate any other reasons why the executable file might be *
22 * covered by the GNU Lesser General Public License. *
23 * *
24 * OpenFlipper is distributed in the hope that it will be useful, *
25 * but WITHOUT ANY WARRANTY; without even the implied warranty of *
26 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
27 * GNU Lesser General Public License for more details. *
28 * *
29 * You should have received a copy of the GNU LesserGeneral Public *
30 * License along with OpenFlipper. If not, *
31 * see <http://www.gnu.org/licenses/>. *
32 * *
33 \*===========================================================================*/
34 
35 /*===========================================================================*\
36 * *
37 * $Revision: 10745 $ *
38 * $LastChangedBy: moebius $ *
39 * $Date: 2011-01-26 10:23:50 +0100 (Mi, 26 Jan 2011) $ *
40 * *
41 \*===========================================================================*/
42 
43 
44 
45 
46 //=============================================================================
47 //
48 // CLASS ICP - IMPLEMENTATION
49 //
50 //=============================================================================
51 
52 #define ICP_C
53 
54 //== INCLUDES =================================================================
55 
56 #include "ICP.hh"
57 #include <float.h>
58 
59 
60 //== NAMESPACES ===============================================================
61 
62 namespace ICP {
63 
64 //== IMPLEMENTATION ==========================================================
65 
66 template < typename VectorT >
67 inline
68 VectorT
69 center_of_gravity(const std::vector< VectorT >& _points ) {
70  VectorT cog(0.0,0.0,0.0);
71 
72  for (uint i = 0 ; i < _points.size() ; ++i ) {
73  cog = cog + _points[i];
74  }
75 
76  cog = cog / ( typename VectorT::value_type )_points.size();
77 
78  return cog;
79 }
80 
81 
82 template < typename VectorT , typename QuaternionT >
83 void
84 icp(const std::vector< VectorT >& _points1 , const std::vector< VectorT >& _points2 , VectorT& _cog1 , VectorT& _cog2 , double& _scale1 , double& _scale2 , QuaternionT& _rotation )
85 {
86  // compute Mean of Points
87  _cog1 = center_of_gravity(_points1);
88  _cog2 = center_of_gravity(_points2);
89 
90 
91  VectorT diff;
92  _scale1 = 0.0;
93  _scale2 = 0.0;
94  for ( uint i = 0 ; i < _points1.size() ; ++i ) {
95  diff = _points1[i] - _cog1;
96  _scale1 = std::max( _scale1 , sqrt( diff.norm() ) );
97  diff = _points2[i] - _cog2;
98  _scale2 = std::max( _scale2 , sqrt( diff.norm() ) );
99  }
100 
101  double Sxx = 0.0;
102  double Sxy = 0.0;
103  double Sxz = 0.0;
104 
105  double Syx = 0.0;
106  double Syy = 0.0;
107  double Syz = 0.0;
108 
109  double Szx = 0.0;
110  double Szy = 0.0;
111  double Szz = 0.0;
112 
113  for ( uint i = 0 ; i < _points1.size() ; ++i ) {
114  Sxx += ( _points1[i][0] - _cog1[0] ) * ( _points2[i][0] - _cog2[0] );
115  Sxy += ( _points1[i][0] - _cog1[0] ) * ( _points2[i][1] - _cog2[1] );
116  Sxz += ( _points1[i][0] - _cog1[0] ) * ( _points2[i][2] - _cog2[2] );
117 
118  Syx += ( _points1[i][1] - _cog1[1] ) * ( _points2[i][0] - _cog2[0] );
119  Syy += ( _points1[i][1] - _cog1[1] ) * ( _points2[i][1] - _cog2[1] );
120  Syz += ( _points1[i][1] - _cog1[1] ) * ( _points2[i][2] - _cog2[2] );
121 
122  Szx += ( _points1[i][2] - _cog1[2] ) * ( _points2[i][0] - _cog2[0] );
123  Szy += ( _points1[i][2] - _cog1[2] ) * ( _points2[i][1] - _cog2[1] );
124  Szz += ( _points1[i][2] - _cog1[2] ) * ( _points2[i][2] - _cog2[2] );
125  }
126 
127  const double scale = _scale1 * _scale2;
128 
129  Sxx = Sxx * 1.0 / scale;
130  Sxy = Sxy * 1.0 / scale;
131  Sxz = Sxz * 1.0 / scale;
132  Syx = Syx * 1.0 / scale;
133  Syy = Syy * 1.0 / scale;
134  Syz = Syz * 1.0 / scale;
135  Szx = Szx * 1.0 / scale;
136  Szy = Szy * 1.0 / scale;
137  Szz = Szz * 1.0 / scale;
138 
139  gmm::dense_matrix<double> N; gmm::resize(N,4,4); gmm::clear(N);
140  N(0,0) = Sxx + Syy + Szz;
141  N(0,1) = Syz - Szy;
142  N(0,2) = Szx - Sxz;
143  N(0,3) = Sxy - Syx;
144 
145  N(1,0) = Syz - Szy;
146  N(1,1) = Sxx - Syy - Szz;
147  N(1,2) = Sxy + Syx;
148  N(1,3) = Szx + Sxz;
149 
150  N(2,0) = Szx - Sxz;
151  N(2,1) = Sxy + Syx;
152  N(2,2) = -Sxx + Syy - Szz;
153  N(2,3) = Syz + Szy;
154 
155  N(3,0) = Sxy - Syx;
156  N(3,1) = Szx + Sxz;
157  N(3,2) = Syz + Szy;
158  N(3,3) = -Sxx - Syy + Szz;
159 
160  std::vector< double > eigval; eigval.resize(4);
161  gmm::dense_matrix< double > eigvect; gmm::resize(eigvect, 4,4); gmm::clear(eigvect);
162  gmm::symmetric_qr_algorithm(N, eigval, eigvect);
163 
164  int gr = 0;
165  double max = -FLT_MAX;
166  for (int i = 0; i < 4; i++)
167  if (eigval[i] > max)
168  {
169  gr = i;
170  max = eigval[i];
171  }
172 
173  _rotation[0] = eigvect(0,gr);
174  _rotation[1] = eigvect(1,gr);
175  _rotation[2] = eigvect(2,gr);
176  _rotation[3] = eigvect(3,gr);
177 
178  _scale1 *= _scale1;
179  _scale2 *= _scale2;
180 }
181 
182 
183 //=============================================================================
184 
185 } //namespace ICP
186 
187 //=============================================================================