eigen_utils.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2013, Willow Garage, Inc.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  *
00029  * Author: Mario Prats
00030  * Much of this code has been adapted from the ViSP library (http://www.irisa.fr/lagadic/visp)
00031  */
00032 
00033 #include <eigen_utils/eigen_utils.h>
00034 #include <tf_conversions/tf_eigen.h>
00035 #include <Eigen/SVD>
00036 
00037 namespace eigen_utils {
00038 
00039 void pseudoinverse(const Eigen::MatrixXd &M, Eigen::MatrixXd &Minv, double tolerance)
00040 {
00041   Eigen::JacobiSVD<Eigen::MatrixXd> svdOfM(M, Eigen::ComputeThinU | Eigen::ComputeThinV);
00042   const Eigen::MatrixXd U = svdOfM.matrixU();
00043   const Eigen::MatrixXd V = svdOfM.matrixV();
00044   const Eigen::VectorXd S = svdOfM.singularValues();
00045 
00046   Eigen::VectorXd Sinv = S;
00047   double maxsv = 0 ;
00048   for (std::size_t i = 0; i < S.rows(); ++i)
00049     if (fabs(S(i)) > maxsv) maxsv = fabs(S(i));
00050   for (std::size_t i = 0; i < S.rows(); ++i)
00051   {
00052     //Those singular values smaller than a percentage of the maximum singular value are removed
00053     if ( fabs(S(i)) > maxsv * tolerance )
00054       Sinv(i) = 1.0 / S(i);
00055     else Sinv(i) = 0;
00056   }
00057 
00058   Minv = V * Sinv.asDiagonal() * U.transpose();
00059 }
00060 
00061 Eigen::MatrixXd pseudoinverse(const Eigen::MatrixXd &M, double tolerance)
00062 {
00063   Eigen::MatrixXd Minv;
00064   pseudoinverse(M, Minv, tolerance);
00065   return Minv;
00066 }
00067 
00068 void transformToPoseVector(const Eigen::Affine3d &M, Eigen::VectorXd &pose)
00069 {
00070   pose.resize(6);
00071 
00072   //fill translation
00073   pose.matrix().block(0,0,3,1) = M.matrix().block(0,3,3,1);
00074 
00075   //Compute and fill rotation (theta-u convention)
00076   //Most of this code is adapted from ViSP vpThetaUVector::build_from(vpRotationMatrix)
00077   const Eigen::Matrix3d R = M.matrix().block(0, 0, 3, 3);
00078 
00079   double s,c,theta,sinc;
00080   s = (R(1,0) - R(0,1)) * (R(1,0) - R(0,1))
00081     + (R(2,0) - R(0,2)) * (R(2,0) - R(0,2))
00082     + (R(2,1) - R(1,2)) * (R(2,1) - R(1,2));
00083   s = sqrt(s) / 2.0;
00084   c = (R(0,0) + R(1,1) + R(2,2) - 1.0) / 2.0;
00085   theta = atan2(s,c);  /* theta in [0, PI] since s > 0 */
00086 
00087   // General case when theta != pi. If theta=pi, c=-1
00088   static const double minimum = 0.0001;
00089   if ( (1 + c) > minimum) // Since -1 <= c <= 1, no fabs(1+c) is required
00090   {
00091     static const double threshold = 1.0e-8;
00092     if (fabs(theta) < threshold) sinc = 1.0 ;
00093     else  sinc = (s / theta) ;
00094 
00095     pose(3) = (R(2,1) - R(1,2)) / (2*sinc);
00096     pose(4) = (R(0,2) - R(2,0)) / (2*sinc);
00097     pose(5) = (R(1,0) - R(0,1)) / (2*sinc);
00098   }
00099   else /* theta near PI */
00100   {
00101     if ( (R(0,0) - c) < std::numeric_limits<double>::epsilon() )
00102       pose(3) = 0.;
00103     else
00104       pose(3) = theta * (sqrt((R(0,0) - c) / (1 - c)));
00105     if ((R(2,1) - R(1,2)) < 0) pose(3) = -pose(3);
00106 
00107     if ( (R(1,1) - c) < std::numeric_limits<double>::epsilon() )
00108       pose(4) = 0.;
00109     else
00110       pose(4) = theta * (sqrt((R(1,1) - c) / (1 - c)));
00111 
00112     if ((R(0,2) - R(2,0)) < 0) pose(4) = -pose(4);
00113 
00114     if ( (R(2,2) - c) < std::numeric_limits<double>::epsilon() )
00115       pose(5) = 0.;
00116     else
00117       pose(5) = theta * (sqrt((R(2,2) - c) / (1 - c)));
00118 
00119     if ((R(1,0) - R(0,1)) < 0) pose(5) = -pose(5);
00120   }
00121 }
00122 
00123 Eigen::VectorXd transformToPoseVector(const Eigen::Affine3d &M) 
00124 {
00125   Eigen::VectorXd twist;
00126   transformToPoseVector(M, twist);
00127   return twist;
00128 }
00129 
00130 
00131 static const double ang_min_sinc = 1.0e-8;
00132 static const double ang_min_mc = 2.5e-4;
00133 
00134 double f_sinc(double sinx, double x)
00135 {
00136   if (fabs(x) < ang_min_sinc) return 1.0 ;
00137   else  return (sinx / x) ;
00138 }
00139 
00140 double f_mcosc(double cosx, double x)
00141 {
00142   if (fabs(x) < ang_min_mc) return 0.5 ;
00143   else  return ((1.0 - cosx) / x / x) ;
00144 }
00145 
00146 double f_msinc(double sinx, double x)
00147 {
00148   if (fabs(x) < ang_min_mc) return (1. / 6.0) ;
00149   else  return ((1.0 - sinx / x) / x / x) ;
00150 }
00151 
00152 Eigen::Affine3d UThetaToAffine3d(const Eigen::Vector3d &u)
00153 {
00154   Eigen::Affine3d rd;
00155   double theta, si, co, sinc, mcosc;
00156 
00157   theta = sqrt(u[0]*u[0] + u[1]*u[1] + u[2]*u[2]);
00158   si = sin(theta);
00159   co = cos(theta);
00160   sinc = f_sinc(si,theta);
00161   mcosc = f_mcosc(co,theta);
00162 
00163   rd(0,0) = co + mcosc*u[0]*u[0];
00164   rd(0,1) = -sinc*u[2] + mcosc*u[0]*u[1];
00165   rd(0,2) = sinc*u[1] + mcosc*u[0]*u[2];
00166   rd(1,0) = sinc*u[2] + mcosc*u[1]*u[0];
00167   rd(1,1) = co + mcosc*u[1]*u[1];
00168   rd(1,2) = -sinc*u[0] + mcosc*u[1]*u[2];
00169   rd(2,0) = -sinc*u[1] + mcosc*u[2]*u[0];
00170   rd(2,1) = sinc*u[0] + mcosc*u[2]*u[1];
00171   rd(2,2) = co + mcosc*u[2]*u[2];
00172 
00173   return rd;
00174 }
00175 
00176 Eigen::Affine3d direct_exponential_map(const Eigen::VectorXd &v, double delta_t)
00177 {
00178   double theta,si,co,sinc,mcosc,msinc;
00179   Eigen::Vector3d u;
00180   Eigen::Affine3d rd;
00181   rd.setIdentity();
00182   Eigen::Vector3d dt;
00183 
00184   Eigen::VectorXd v_dt = v * delta_t;
00185 
00186   u[0] = v_dt[3];
00187   u[1] = v_dt[4];
00188   u[2] = v_dt[5];
00189 
00190   rd = UThetaToAffine3d(u);
00191 
00192   theta = sqrt(u[0]*u[0] + u[1]*u[1] + u[2]*u[2]);
00193   si = sin(theta);
00194   co = cos(theta);
00195   sinc = f_sinc(si,theta);
00196   mcosc = f_mcosc(co,theta);
00197   msinc = f_msinc(si,theta);
00198 
00199   dt[0] = v_dt[0] * (sinc + u[0]*u[0]*msinc)
00200                       + v_dt[1]*(u[0]*u[1]*msinc - u[2]*mcosc)
00201                       + v_dt[2]*(u[0]*u[2]*msinc + u[1]*mcosc);
00202 
00203   dt[1] = v_dt[0] * (u[0]*u[1]*msinc + u[2]*mcosc)
00204                       + v_dt[1]*(sinc + u[1]*u[1]*msinc)
00205                       + v_dt[2]*(u[1]*u[2]*msinc - u[0]*mcosc);
00206 
00207   dt[2] = v_dt[0] * (u[0]*u[2]*msinc - u[1]*mcosc)
00208                       + v_dt[1]*(u[1]*u[2]*msinc + u[0]*mcosc)
00209                       + v_dt[2]*(sinc + u[2]*u[2]*msinc);
00210 
00211   Eigen::Affine3d Delta;
00212   Delta.setIdentity();
00213   Delta = rd;
00214   Delta(0,3) = dt[0];
00215   Delta(1,3) = dt[1];
00216   Delta(2,3) = dt[2];
00217 
00218   return Delta;
00219 }
00220 
00221 bool getTransform(const tf::TransformListener &listener, const std::string &target, const std::string source, Eigen::Affine3d &tMs, const ros::Time &timestamp, const ros::Duration &timeout)
00222 {
00223   try {
00224     tf::StampedTransform tMs_stamped;
00225     if (listener.waitForTransform(target, source, timestamp, timeout))
00226     {
00227       listener.lookupTransform(target, source, timestamp, tMs_stamped);
00228       tf::poseTFToEigen(tMs_stamped, tMs);
00229       return true;
00230     }
00231   } catch (tf::TransformException &ex) {
00232     ROS_ERROR("%s",ex.what());
00233   }
00234   return false;
00235 }
00236 
00237 } // namespace


eigen_utils
Author(s): Mario Prats
autogenerated on Sun Oct 5 2014 23:42:02