orocos_kdl
svd_eigen_Macie.cpp
Go to the documentation of this file.
1 // Copyright (C) 2007 Ruben Smits <ruben dot smits at intermodalics dot eu>
2 
3 // Version: 1.0
4 // Author: Ruben Smits <ruben dot smits at intermodalics dot eu>
5 // Maintainer: Ruben Smits <ruben dot smits at intermodalics dot eu>
6 // URL: http://www.orocos.org/kdl
7 
8 // This library is free software; you can redistribute it and/or
9 // modify it under the terms of the GNU Lesser General Public
10 // License as published by the Free Software Foundation; either
11 // version 2.1 of the License, or (at your option) any later version.
12 
13 // This library is distributed in the hope that it will be useful,
14 // but WITHOUT ANY WARRANTY; without even the implied warranty of
15 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
16 // Lesser General Public License for more details.
17 
18 // You should have received a copy of the GNU Lesser General Public
19 // License along with this library; if not, write to the Free Software
20 // Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
21 
22 #include "svd_eigen_Macie.hpp"
23 
24 namespace KDL{
25  int svd_eigen_Macie(const Eigen::MatrixXd& A,Eigen::MatrixXd& U,Eigen::VectorXd& S, Eigen::MatrixXd& V,
26  Eigen::MatrixXd& B, Eigen::VectorXd& tempi,
27  double threshold,bool toggle)
28  {
29  bool rotate = true;
30  unsigned int sweeps=0;
31  unsigned int rotations=0;
32  if(toggle){
33  //Calculate B from new A and previous V
34  B=A.lazyProduct(V);
35  while(rotate){
36  rotate=false;
37  rotations=0;
38  //Perform rotations between columns of B
39  for(unsigned int i=0;i<B.cols();i++){
40  for(unsigned int j=i+1;j<B.cols();j++){
41  //calculate plane rotation
42  double p = B.col(i).dot(B.col(j));
43  double qi =B.col(i).dot(B.col(i));
44  double qj = B.col(j).dot(B.col(j));
45  double q=qi-qj;
46  double alpha = pow(p,2.0)/(qi*qj);
47  //if columns are orthogonal with precision
48  //threshold, don't perform rotation and continue
49  if(alpha<threshold)
50  continue;
51  rotations++;
52  double c = sqrt(4*pow(p,2.0)+pow(q,2.0));
53  double cos,sin;
54  if(q>=0){
55  cos=sqrt((c+q)/(2*c));
56  sin=p/(c*cos);
57  }else{
58  if(p<0)
59  sin=-sqrt((c-q)/(2*c));
60  else
61  sin=sqrt((c-q)/(2*c));
62  cos=p/(c*sin);
63  }
64 
65  //Apply plane rotation to columns of B
66  tempi = cos*B.col(i) + sin*B.col(j);
67  B.col(j) = - sin*B.col(i) + cos*B.col(j);
68  B.col(i) = tempi;
69 
70  //Apply plane rotation to columns of V
71  tempi.head(V.rows()) = cos*V.col(i) + sin*V.col(j);
72  V.col(j) = - sin*V.col(i) + cos*V.col(j);
73  V.col(i) = tempi.head(V.rows());
74 
75  rotate=true;
76  }
77  }
78  //Only calculate new U and S if there were any rotations
79  if(rotations!=0){
80  for(unsigned int i=0;i<U.rows();i++) {
81  if(i<B.cols()){
82  double si=sqrt(B.col(i).dot(B.col(i)));
83  if(si==0)
84  U.col(i) = B.col(i);
85  else
86  U.col(i) = B.col(i)/si;
87  S(i)=si;
88  }
89  else
90  U.col(i) = 0*tempi;
91  }
92  sweeps++;
93  }
94  }
95  return sweeps;
96  }else{
97  //Calculate B from new A and previous U'
98  B = U.transpose().lazyProduct(A);
99  while(rotate){
100  rotate=false;
101  rotations=0;
102  //Perform rotations between rows of B
103  for(unsigned int i=0;i<B.cols();i++){
104  for(unsigned int j=i+1;j<B.cols();j++){
105  //calculate plane rotation
106  double p = B.row(i).dot(B.row(j));
107  double qi = B.row(i).dot(B.row(i));
108  double qj = B.row(j).dot(B.row(j));
109 
110  double q=qi-qj;
111  double alpha = pow(p,2.0)/(qi*qj);
112  //if columns are orthogonal with precision
113  //threshold, don't perform rotation and
114  //continue
115  if(alpha<threshold)
116  continue;
117  rotations++;
118  double c = sqrt(4*pow(p,2.0)+pow(q,2.0));
119  double cos,sin;
120  if(q>=0){
121  cos=sqrt((c+q)/(2*c));
122  sin=p/(c*cos);
123  }else{
124  if(p<0)
125  sin=-sqrt((c-q)/(2*c));
126  else
127  sin=sqrt((c-q)/(2*c));
128  cos=p/(c*sin);
129  }
130 
131  //Apply plane rotation to rows of B
132  tempi.head(B.cols()) = cos*B.row(i) + sin*B.row(j);
133  B.row(j) = - sin*B.row(i) + cos*B.row(j);
134  B.row(i) = tempi.head(B.cols());
135 
136 
137  //Apply plane rotation to rows of U
138  tempi.head(U.rows()) = cos*U.col(i) + sin*U.col(j);
139  U.col(j) = - sin*U.col(i) + cos*U.col(j);
140  U.col(i) = tempi.head(U.rows());
141 
142  rotate=true;
143  }
144  }
145 
146  //Only calculate new U and S if there were any rotations
147  if(rotations!=0){
148  for(unsigned int i=0;i<V.rows();i++) {
149  double si=sqrt(B.row(i).dot(B.row(i)));
150  if(si==0)
151  V.col(i) = B.row(i);
152  else
153  V.col(i) = B.row(i)/si;
154  S(i)=si;
155  }
156  sweeps++;
157  }
158  }
159  return sweeps;
160  }
161  }
162 } // namespace KDL
KDL::pow
INLINE Rall1d< T, V, S > pow(const Rall1d< T, V, S > &arg, double m)
Definition: rall1d.h:383
KDL::sqrt
INLINE Rall1d< T, V, S > sqrt(const Rall1d< T, V, S > &arg)
Definition: rall1d.h:391
KDL::sin
INLINE Rall1d< T, V, S > sin(const Rall1d< T, V, S > &arg)
Definition: rall1d.h:335
KDL
Definition: kukaLWR_DHnew.cpp:25
KDL::cos
INLINE Rall1d< T, V, S > cos(const Rall1d< T, V, S > &arg)
Definition: rall1d.h:343
KDL::svd_eigen_Macie
int svd_eigen_Macie(const Eigen::MatrixXd &A, Eigen::MatrixXd &U, Eigen::VectorXd &S, Eigen::MatrixXd &V, Eigen::MatrixXd &B, Eigen::VectorXd &tempi, double threshold, bool toggle)
Definition: svd_eigen_Macie.cpp:25
svd_eigen_Macie.hpp
Generated on Tue Nov 26 2024 04:35:25 for orocos_kdl by   doxygen 1.8.17