Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
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
00068 B=A.lazyProduct(V);
00069 while(rotate){
00070 rotate=false;
00071 rotations=0;
00072
00073 for(unsigned int i=0;i<B.cols();i++){
00074 for(unsigned int j=i+1;j<B.cols();j++){
00075
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
00082
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
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
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
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
00132 B = U.transpose().lazyProduct(A);
00133 while(rotate){
00134 rotate=false;
00135 rotations=0;
00136
00137 for(unsigned int i=0;i<B.cols();i++){
00138 for(unsigned int j=i+1;j<B.cols();j++){
00139
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
00147
00148
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
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
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
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