iLab Neuromorphic Robotics Toolkit  0.1
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
EigenDecompositionImpl.H
Go to the documentation of this file.
1 /*! @file
2  @author Shane Grant
3  @copyright GNU Public License (GPL v3)
4  @section License
5  @verbatim
6  // ////////////////////////////////////////////////////////////////////////
7  // The iLab Neuromorphic Robotics Toolkit (NRT) //
8  // Copyright 2010-2012 by the University of Southern California (USC) //
9  // and the iLab at USC. //
10  // //
11  // iLab - University of Southern California //
12  // Hedco Neurociences Building, Room HNB-10 //
13  // Los Angeles, Ca 90089-2520 - USA //
14  // //
15  // See http://ilab.usc.edu for information about this project. //
16  // ////////////////////////////////////////////////////////////////////////
17  // This file is part of The iLab Neuromorphic Robotics Toolkit. //
18  // //
19  // The iLab Neuromorphic Robotics Toolkit is free software: you can //
20  // redistribute it and/or modify it under the terms of the GNU General //
21  // Public License as published by the Free Software Foundation, either //
22  // version 3 of the License, or (at your option) any later version. //
23  // //
24  // The iLab Neuromorphic Robotics Toolkit is distributed in the hope //
25  // that it will be useful, but WITHOUT ANY WARRANTY; without even the //
26  // implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR //
27  // PURPOSE. See the GNU General Public License for more details. //
28  // //
29  // You should have received a copy of the GNU General Public License //
30  // along with The iLab Neuromorphic Robotics Toolkit. If not, see //
31  // <http://www.gnu.org/licenses/>. //
32  // ////////////////////////////////////////////////////////////////////////
33  @endverbatim */
34 
35 /*
36  * Software License Agreement (BSD License)
37  *
38  * Point Cloud Library (PCL) - www.pointclouds.org
39  * Copyright (c) 2010-2012, Willow Garage, Inc.
40  * Copyright (C) 2010 Gael Guennebaud <gael.guennebaud@inria.fr>
41  * Copyright (C) 2009 Hauke Heibel <hauke.heibel@gmail.com>
42  * Copyright (c) 2012-, Open Perception, Inc.
43  *
44  * All rights reserved.
45  *
46  * Redistribution and use in source and binary forms, with or without
47  * modification, are permitted provided that the following conditions
48  * are met:
49  *
50  * * Redistributions of source code must retain the above copyright
51  * notice, this list of conditions and the following disclaimer.
52  * * Redistributions in binary form must reproduce the above
53  * copyright notice, this list of conditions and the following
54  * disclaimer in the documentation and/or other materials provided
55  * with the distribution.
56  * * Neither the name of the copyright holder(s) nor the names of its
57  * contributors may be used to endorse or promote products derived
58  * from this software without specific prior written permission.
59  *
60  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
61  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
62  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
63  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
64  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
65  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
66  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
67  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
68  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
69  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
70  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
71  * POSSIBILITY OF SUCH DAMAGE. */
72 
73 #include <nrt/config.h>
74 #ifdef NRT_HAVE_EIGEN3
75 
76 #ifndef INCLUDE_NRT_EIGEN_EIGENDECOMPOSITION_IMPL_H_
77 #define INCLUDE_NRT_EIGEN_EIGENDECOMPOSITION_IMPL_H_
78 
79 #include <array>
80 
81 namespace
82 {
83  /** \brief Compute the roots of a quadratic polynom x^2 + b*x + c = 0
84  * \param[in] b linear parameter
85  * \param[in] c constant parameter
86  * \param[out] roots solutions of x^2 + b*x + c = 0
87  */
88  template<class T> inline
89  Eigen::Matrix<T, 3, 1> computeRoots2( T const b, T const c )
90  {
91  Eigen::Matrix<T, 3, 1> retVal;
92  retVal(0) = static_cast<T>( 0 );
93 
94  T d = static_cast<T>( b * b - 4.0 * c );
95  if( d < static_cast<T>( 0 ) )
96  d = static_cast<T>( 0 );
97 
98  T sd = std::sqrt( d );
99 
100  retVal(2) = 0.5f * ( b + sd );
101  retVal(1) = 0.5f * ( b - sd );
102 
103  return retVal;
104  }
105 
106  /** \brief computes the roots of the characteristic polynomial of the input matrix m, which are the eigenvalues
107  * \param[in] m input matrix
108  * \param[out] roots roots of the characteristic polynomial of the input matrix m, which are the eigenvalues
109  */
110  template<class T> inline
111  Eigen::Matrix<T, 3, 1> computeRoots( Eigen::Matrix<T, 3, 3> const & m )
112  {
113  Eigen::Matrix<T, 3, 1> roots;
114 
115  // The characteristic equation is x^3 - c2*x^2 + c1*x - c0 = 0. The
116  // eigenvalues are the roots to this equation, all guaranteed to be
117  // real-valued, because the matrix is symmetric.
118  T const c0 = m (0, 0) * m (1, 1) * m (2, 2)
119  + T(2) * m (0, 1) * m (0, 2) * m (1, 2)
120  - m (0, 0) * m (1, 2) * m (1, 2)
121  - m (1, 1) * m (0, 2) * m (0, 2)
122  - m (2, 2) * m (0, 1) * m (0, 1);
123  T const c1 = m (0, 0) * m (1, 1) -
124  m (0, 1) * m (0, 1) +
125  m (0, 0) * m (2, 2) -
126  m (0, 2) * m (0, 2) +
127  m (1, 1) * m (2, 2) -
128  m (1, 2) * m (1, 2);
129  T const c2 = m (0, 0) + m (1, 1) + m (2, 2);
130 
131  if( std::abs( c0 ) < std::numeric_limits<T>::epsilon() )// one root is 0 -> quadratic equation
132  roots = computeRoots2( c2, c1 );
133  else
134  {
135  const T s_inv3 = static_cast<T>( 1.0 / 3.0 );
136  const T s_sqrt3 = std::sqrt( static_cast<T>( 3.0 ) );
137 
138  // Construct the parameters used in classifying the roots of the equation
139  // and in solving the equation for the roots in closed form.
140  T c2_over_3 = c2 * s_inv3;
141  T a_over_3 = ( c1 - c2 * c2_over_3 ) * s_inv3;
142  if( a_over_3 > static_cast<T>( 0 ) )
143  a_over_3 = static_cast<T>( 0 );
144 
145  T half_b = static_cast<T>( 0.5 ) * ( c0 + c2_over_3 * (static_cast<T>( 2 ) * c2_over_3 * c2_over_3 - c1 ) );
146 
147  T q = half_b * half_b + a_over_3 * a_over_3*a_over_3;
148  if( q > static_cast<T>( 0 ) )
149  q = static_cast<T>( 0 );
150 
151  // Compute the eigenvalues by solving for the roots of the polynomial.
152  T rho = std::sqrt( -a_over_3 );
153  T theta = std::atan2( std::sqrt( -q ), half_b ) * s_inv3;
154  T cos_theta = std::cos( theta );
155  T sin_theta = std::sin( theta );
156  roots(0) = c2_over_3 + static_cast<T>( 2 ) * rho * cos_theta;
157  roots(1) = c2_over_3 - rho * ( cos_theta + s_sqrt3 * sin_theta );
158  roots(2) = c2_over_3 - rho * ( cos_theta - s_sqrt3 * sin_theta );
159 
160  // Sort in increasing order.
161  if( roots(0) >= roots(1) )
162  std::swap( roots(0), roots(1) );
163  if( roots(1) >= roots(2) )
164  {
165  std::swap( roots(1), roots(2) );
166  if( roots(0) >= roots(1) )
167  std::swap( roots (0), roots (1) );
168  }
169 
170  if( roots (0) <= 0 ) // eigenval for symetric positive semi-definite matrix can not be negative! Set it to 0
171  roots = computeRoots2(c2, c1);
172  }
173 
174  return roots;
175  }
176 } // end anon namespace
177 
178 
179 template <class T> inline
180 void nrt::eigenDecomposition( Eigen::Matrix<T, 3, 3> const & matrix,
181  Optional<Eigen::Matrix<T, 3, 1> &> eigenValues,
182  Optional<Eigen::Matrix<T, 3, 3> &> eigenVectors )
183 {
184  typedef typename Eigen::Matrix<T, 3, 3> Matrix;
185  typedef typename Eigen::Matrix<T, 3, 1> Vector;
186 
187  // Scale the matrix so its entries are in [-1,1]. The scaling is applied
188  // only when at least one matrix entry has magnitude larger than 1.
189  T scale = matrix.cwiseAbs().maxCoeff();
190  if( scale <= std::numeric_limits<T>::min() )
191  scale = static_cast<T>( 1.0 );
192 
193  Matrix scaledMat = matrix / scale;
194 
195  // Compute the eigenvalues
196  Vector evals = computeRoots(scaledMat);
197 
198  // check if we should compute eigenvectors
199  if( eigenVectors )
200  {
201  Matrix evecs;
202 
203  if ((evals (2) - evals (0)) <= std::numeric_limits<T>::epsilon())
204  {
205  // all three equal
206  evecs.setIdentity ();
207  }
208  else if ((evals (1) - evals (0)) <= std::numeric_limits<T>::epsilon() )
209  {
210  // first and second equal
211  Matrix tmp;
212  tmp = scaledMat;
213  tmp.diagonal ().array () -= evals (2);
214 
215  Vector vec1 = tmp.row (0).cross (tmp.row (1));
216  Vector vec2 = tmp.row (0).cross (tmp.row (2));
217  Vector vec3 = tmp.row (1).cross (tmp.row (2));
218 
219  T len1 = vec1.squaredNorm ();
220  T len2 = vec2.squaredNorm ();
221  T len3 = vec3.squaredNorm ();
222 
223  if (len1 >= len2 && len1 >= len3)
224  evecs.col (2) = vec1 / std::sqrt (len1);
225  else if (len2 >= len1 && len2 >= len3)
226  evecs.col (2) = vec2 / std::sqrt (len2);
227  else
228  evecs.col (2) = vec3 / std::sqrt (len3);
229 
230  evecs.col (1) = evecs.col (2).unitOrthogonal ();
231  evecs.col (0) = evecs.col (1).cross (evecs.col (2));
232  }
233  else if ((evals (2) - evals (1)) <= std::numeric_limits<T>::epsilon() )
234  {
235  // second and third equal
236  Matrix tmp;
237  tmp = scaledMat;
238  tmp.diagonal ().array () -= evals (0);
239 
240  Vector vec1 = tmp.row (0).cross (tmp.row (1));
241  Vector vec2 = tmp.row (0).cross (tmp.row (2));
242  Vector vec3 = tmp.row (1).cross (tmp.row (2));
243 
244  T len1 = vec1.squaredNorm ();
245  T len2 = vec2.squaredNorm ();
246  T len3 = vec3.squaredNorm ();
247 
248  if (len1 >= len2 && len1 >= len3)
249  evecs.col (0) = vec1 / std::sqrt (len1);
250  else if (len2 >= len1 && len2 >= len3)
251  evecs.col (0) = vec2 / std::sqrt (len2);
252  else
253  evecs.col (0) = vec3 / std::sqrt (len3);
254 
255  evecs.col (1) = evecs.col (0).unitOrthogonal ();
256  evecs.col (2) = evecs.col (0).cross (evecs.col (1));
257  }
258  else
259  {
260  Matrix tmp;
261  tmp = scaledMat;
262  tmp.diagonal ().array () -= evals (2);
263 
264  Vector vec1 = tmp.row (0).cross (tmp.row (1));
265  Vector vec2 = tmp.row (0).cross (tmp.row (2));
266  Vector vec3 = tmp.row (1).cross (tmp.row (2));
267 
268  T len1 = vec1.squaredNorm ();
269  T len2 = vec2.squaredNorm ();
270  T len3 = vec3.squaredNorm ();
271  std::array<T, 3> mmax;
272 
273  unsigned int min_el = 2;
274  unsigned int max_el = 2;
275  if (len1 >= len2 && len1 >= len3)
276  {
277  mmax[2] = len1;
278  evecs.col (2) = vec1 / std::sqrt (len1);
279  }
280  else if (len2 >= len1 && len2 >= len3)
281  {
282  mmax[2] = len2;
283  evecs.col (2) = vec2 / std::sqrt (len2);
284  }
285  else
286  {
287  mmax[2] = len3;
288  evecs.col (2) = vec3 / std::sqrt (len3);
289  }
290 
291  tmp = scaledMat;
292  tmp.diagonal ().array () -= evals (1);
293 
294  vec1 = tmp.row (0).cross (tmp.row (1));
295  vec2 = tmp.row (0).cross (tmp.row (2));
296  vec3 = tmp.row (1).cross (tmp.row (2));
297 
298  len1 = vec1.squaredNorm ();
299  len2 = vec2.squaredNorm ();
300  len3 = vec3.squaredNorm ();
301  if (len1 >= len2 && len1 >= len3)
302  {
303  mmax[1] = len1;
304  evecs.col (1) = vec1 / std::sqrt (len1);
305  min_el = len1 <= mmax[min_el] ? 1 : min_el;
306  max_el = len1 > mmax[max_el] ? 1 : max_el;
307  }
308  else if (len2 >= len1 && len2 >= len3)
309  {
310  mmax[1] = len2;
311  evecs.col (1) = vec2 / std::sqrt (len2);
312  min_el = len2 <= mmax[min_el] ? 1 : min_el;
313  max_el = len2 > mmax[max_el] ? 1 : max_el;
314  }
315  else
316  {
317  mmax[1] = len3;
318  evecs.col (1) = vec3 / std::sqrt (len3);
319  min_el = len3 <= mmax[min_el] ? 1 : min_el;
320  max_el = len3 > mmax[max_el] ? 1 : max_el;
321  }
322 
323  tmp = scaledMat;
324  tmp.diagonal ().array () -= evals (0);
325 
326  vec1 = tmp.row (0).cross (tmp.row (1));
327  vec2 = tmp.row (0).cross (tmp.row (2));
328  vec3 = tmp.row (1).cross (tmp.row (2));
329 
330  len1 = vec1.squaredNorm ();
331  len2 = vec2.squaredNorm ();
332  len3 = vec3.squaredNorm ();
333  if (len1 >= len2 && len1 >= len3)
334  {
335  mmax[0] = len1;
336  evecs.col (0) = vec1 / std::sqrt (len1);
337  min_el = len3 <= mmax[min_el] ? 0 : min_el;
338  max_el = len3 > mmax[max_el] ? 0 : max_el;
339  }
340  else if (len2 >= len1 && len2 >= len3)
341  {
342  mmax[0] = len2;
343  evecs.col (0) = vec2 / std::sqrt (len2);
344  min_el = len3 <= mmax[min_el] ? 0 : min_el;
345  max_el = len3 > mmax[max_el] ? 0 : max_el;
346  }
347  else
348  {
349  mmax[0] = len3;
350  evecs.col (0) = vec3 / std::sqrt (len3);
351  min_el = len3 <= mmax[min_el] ? 0 : min_el;
352  max_el = len3 > mmax[max_el] ? 0 : max_el;
353  }
354 
355  unsigned mid_el = 3 - min_el - max_el;
356  evecs.col (min_el) = evecs.col ((min_el + 1) % 3).cross ( evecs.col ((min_el + 2) % 3) ).normalized ();
357  evecs.col (mid_el) = evecs.col ((mid_el + 1) % 3).cross ( evecs.col ((mid_el + 2) % 3) ).normalized ();
358  }
359 
360  *eigenVectors = evecs;
361  } // end check for eigenvectors
362 
363  // scale back eigenvalues if they are wanted
364  if( eigenValues )
365  *eigenValues = evals * scale;
366 } // end eigenDecomposition
367 
368 #endif // INCLUDE_NRT_EIGEN_EIGENDECOMPOSITION_IMPL_H_
369 #endif // NRT_HAVE_EIGEN3