svd_eigen_Macie.hpp
Go to the documentation of this file.
00001 // Copyright  (C)  2008  Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
00002 
00003 // Version: 1.0
00004 // Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
00005 // Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
00006 // URL: http://www.orocos.org/kdl
00007 
00008 // This library is free software; you can redistribute it and/or
00009 // modify it under the terms of the GNU Lesser General Public
00010 // License as published by the Free Software Foundation; either
00011 // version 2.1 of the License, or (at your option) any later version.
00012 
00013 // This library is distributed in the hope that it will be useful,
00014 // but WITHOUT ANY WARRANTY; without even the implied warranty of
00015 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00016 // Lesser General Public License for more details.
00017 
00018 // You should have received a copy of the GNU Lesser General Public
00019 // License along with this library; if not, write to the Free Software
00020 // Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
00021 
00022 
00023 //implementation of svd according to (Maciejewski and Klein,1989)
00024 //and (Braun, Ulrey, Maciejewski and Siegel,2002)
00025 
00031 #ifndef SVD_BOOST_MACIE
00032 #define SVD_BOOST_MACIE
00033 
00034 #include <Eigen/Core>
00035 using namespace Eigen;
00036 
00037 
00038 namespace KDL
00039 {
00040 
00059     int svd_eigen_Macie(const MatrixXd& A,MatrixXd& U,VectorXd& S, MatrixXd& V,
00060                         MatrixXd& B, VectorXd& tempi,
00061                         double treshold,bool toggle)
00062     {
00063         bool rotate = true;
00064         unsigned int sweeps=0;
00065         unsigned int rotations=0;
00066         if(toggle){
00067             //Calculate B from new A and previous V
00068             B=A.lazyProduct(V);
00069             while(rotate){
00070                 rotate=false;
00071                 rotations=0;
00072                 //Perform rotations between columns of B
00073                 for(unsigned int i=0;i<B.cols();i++){
00074                     for(unsigned int j=i+1;j<B.cols();j++){
00075                         //calculate plane rotation
00076                         double p = B.col(i).dot(B.col(j));
00077                         double qi =B.col(i).dot(B.col(i));
00078                         double qj = B.col(j).dot(B.col(j));
00079                         double q=qi-qj;
00080                         double alpha = pow(p,2.0)/(qi*qj);
00081                         //if columns are orthogonal with precision
00082                         //treshold, don't perform rotation and continue
00083                         if(alpha<treshold)
00084                             continue;
00085                         rotations++;
00086                         double c = sqrt(4*pow(p,2.0)+pow(q,2.0));
00087                         double cos,sin;
00088                         if(q>=0){
00089                             cos=sqrt((c+q)/(2*c));
00090                             sin=p/(c*cos);
00091                         }else{
00092                             if(p<0)
00093                                 sin=-sqrt((c-q)/(2*c));
00094                             else
00095                                 sin=sqrt((c-q)/(2*c));
00096                             cos=p/(c*sin);
00097                         }
00098 
00099                         //Apply plane rotation to columns of B
00100                         tempi = cos*B.col(i) + sin*B.col(j);
00101                         B.col(j) = - sin*B.col(i) + cos*B.col(j);
00102                         B.col(i) = tempi;
00103                         
00104                         //Apply plane rotation to columns of V
00105                         tempi.head(V.rows()) = cos*V.col(i) + sin*V.col(j);
00106                         V.col(j) = - sin*V.col(i) + cos*V.col(j);
00107                         V.col(i) = tempi.head(V.rows());
00108 
00109                         rotate=true;
00110                     }
00111                 }
00112                 //Only calculate new U and S if there were any rotations
00113                 if(rotations!=0){
00114                     for(unsigned int i=0;i<U.rows();i++) {
00115                         if(i<B.cols()){
00116                             double si=sqrt(B.col(i).dot(B.col(i)));
00117                             if(si==0)
00118                                 U.col(i) = B.col(i);
00119                             else
00120                                 U.col(i) = B.col(i)/si;
00121                             S(i)=si;
00122                         }
00123                         else
00124                             U.col(i) = 0*tempi;
00125                     }
00126                     sweeps++;
00127                 }
00128             }
00129             return sweeps;
00130         }else{
00131             //Calculate B from new A and previous U'
00132             B = U.transpose().lazyProduct(A);
00133             while(rotate){
00134                 rotate=false;
00135                 rotations=0;
00136                 //Perform rotations between rows of B
00137                 for(unsigned int i=0;i<B.cols();i++){
00138                     for(unsigned int j=i+1;j<B.cols();j++){
00139                         //calculate plane rotation
00140                         double p = B.row(i).dot(B.row(j));
00141                         double qi = B.row(i).dot(B.row(i));
00142                         double qj = B.row(j).dot(B.row(j));
00143 
00144                         double q=qi-qj;
00145                         double alpha = pow(p,2.0)/(qi*qj);
00146                         //if columns are orthogonal with precision
00147                         //treshold, don't perform rotation and
00148                         //continue
00149                         if(alpha<treshold)
00150                             continue;
00151                         rotations++;
00152                         double c = sqrt(4*pow(p,2.0)+pow(q,2.0));
00153                         double cos,sin;
00154                         if(q>=0){
00155                             cos=sqrt((c+q)/(2*c));
00156                             sin=p/(c*cos);
00157                         }else{
00158                             if(p<0)
00159                                 sin=-sqrt((c-q)/(2*c));
00160                             else
00161                                 sin=sqrt((c-q)/(2*c));
00162                             cos=p/(c*sin);
00163                         }
00164 
00165                         //Apply plane rotation to rows of B
00166                         tempi.head(B.cols()) =  cos*B.row(i) + sin*B.row(j);
00167                         B.row(j) =  - sin*B.row(i) + cos*B.row(j);
00168                         B.row(i) =  tempi.head(B.cols());
00169 
00170 
00171                         //Apply plane rotation to rows of U
00172                         tempi.head(U.rows()) = cos*U.col(i) + sin*U.col(j);
00173                         U.col(j) = - sin*U.col(i) + cos*U.col(j);
00174                         U.col(i) = tempi.head(U.rows());
00175 
00176                         rotate=true;
00177                     }
00178                 }
00179 
00180                 //Only calculate new U and S if there were any rotations
00181                 if(rotations!=0){
00182                     for(unsigned int i=0;i<V.rows();i++) {
00183                         double si=sqrt(B.row(i).dot(B.row(i)));
00184                         if(si==0)
00185                             V.col(i) = B.row(i);
00186                         else
00187                             V.col(i) = B.row(i)/si;
00188                         S(i)=si;
00189                     }
00190                     sweeps++;
00191                 }
00192             }
00193             return sweeps;
00194         }
00195     }
00196 
00197 
00198 }
00199 #endif


orocos_kdl
Author(s):
autogenerated on Wed Aug 26 2015 15:14:14