eigen3_transforms.cpp
Go to the documentation of this file.
00001 
00009 /*****************************************************************************
00010 ** Includes
00011 *****************************************************************************/
00012 
00013 #include <iostream>
00014 #include <vector>
00015 #include <ecl/config/macros.hpp>
00016 #include <ecl/threads/priority.hpp>
00017 #include <ecl/time/duration.hpp>
00018 #include <ecl/time/cpuwatch.hpp>
00019 #include <ecl/time/stopwatch.hpp>
00020 #include <ecl/time/time_data.hpp>
00021 #include <ecl/linear_algebra.hpp>
00022 
00023 /*****************************************************************************
00024 ** Using
00025 *****************************************************************************/
00026 
00027 using namespace ecl;
00028 using namespace Eigen;
00029 using std::vector;
00030 
00031 /*****************************************************************************
00032 ** Pose2D
00033 *****************************************************************************/
00034 
00035 class NewPose2D {
00036 public:
00037         NewPose2D(){}
00038         NewPose2D(const double &x, const double &y, const double& theta) :
00039                 rotation(Rotation2D<double>(theta).toRotationMatrix())
00040         {
00041                 translation << x, y;
00042         }
00043         NewPose2D operator*(const NewPose2D& pose) const {
00044                 NewPose2D new_pose;
00045                 new_pose.translation = translation+rotation*pose.translation;
00046                 new_pose.rotation = rotation*pose.rotation;
00047                 return new_pose;
00048         }
00049         NewPose2D inverse() const {
00050                 NewPose2D p;
00051                 p.translation = rotation*translation;
00052                 p.rotation = -1*rotation;
00053                 return p;
00054         }
00055         void print() {
00056                 Rotation2D<double> angle(0.0);
00057                 angle.fromRotationMatrix(rotation);
00058                 std::cout << "[ " << translation[0] << "," << translation[1] << "," << angle.angle() << "]" << std::endl;
00059         }
00060 
00061         Matrix2d rotation;
00062         Vector2d translation;
00063 };
00064 
00065 class Pose2D {
00066 public:
00067         Pose2D(const double &x_=0.0, const double &y_=0.0, const double&theta=0.0) :
00068                 x(x_), y(y_), rotation(theta)
00069         {}
00070         Pose2D(const Pose2D& pose) : x(pose.x), y(pose.y), rotation(pose.rotation) {}
00071         Pose2D inverse() const {
00072                 double s = sin(rotation);
00073                 double c = cos(rotation);
00074                 return Pose2D(c*x-s*y, s*x+c*y, -rotation);
00075         }
00076 
00077         Pose2D operator*(const Pose2D& pose) const {
00078                 double s = sin(rotation);
00079                 double c = cos(rotation);
00080                 Pose2D new_pose(x+c*pose.x-s*pose.y,
00081                                         y+s*pose.x + c*pose.y,
00082                                         rotation+pose.rotation);
00083                 while (new_pose.rotation > +3.14) { new_pose.rotation -= 2*3.14; }
00084                 while (new_pose.rotation < -3.14) { new_pose.rotation += 2*3.14; }
00085                 return new_pose;
00086         }
00087 
00088         void print() {
00089                 std::cout << "[ " << x << "," << y << "," << rotation << "]" << std::endl;
00090         }
00091         double x, y;
00092         double rotation;
00093 };
00094 
00095 /*****************************************************************************
00096 ** Globals
00097 *****************************************************************************/
00098 
00099 const unsigned int REPEAT = 1000;
00100 const double increment = 1.0/static_cast<double>(REPEAT);
00101 
00102 double random_float() {
00103         return static_cast<double>(rand()%99)/100.0;
00104 }
00105 
00106 Matrix3d rotation3D(const double &alpha, const double &beta, const double &gamma) {
00107         Matrix3d Ralpha, Rbeta, Rgamma;
00108         Ralpha << 1.0, 0.0, 0.0,
00109                           0.0, cos(alpha), -sin(alpha),
00110                           0.0, sin(alpha), cos(alpha);
00111         Rbeta <<  cos(beta), 0.0, sin(beta),
00112                           0.0, 1.0, 0.0,
00113                           -sin(beta), 0.0, cos(beta);
00114         Rgamma << cos(gamma), -sin(gamma), 0.0,
00115                           sin(gamma), cos(gamma), 0.0,
00116                           0.0, 0.0, 1.0;
00117         return Ralpha*Rbeta*Rgamma;
00118 }
00119 
00120 
00121 template <typename MatrixType>
00122 ECL_DONT_INLINE Duration product(const MatrixType &a, const MatrixType &b, MatrixType &c) {
00123         StopWatch stopwatch;
00124         c = a*a;
00125         c = a*b;
00126         c = b*b;
00127         return stopwatch.split();
00128 }
00129 
00130 template <typename MatrixType>
00131 ECL_DONT_INLINE  Duration inverse(const MatrixType &a, const MatrixType &b, MatrixType &c) {
00132         StopWatch stopwatch;
00133         c = a.inverse();
00134         c = b.inverse();
00135         c = a.inverse();
00136         return stopwatch.split();
00137 }
00138 
00139 template<typename Pose>
00140 ECL_DONT_INLINE TimeData pose2DTest(bool inverse_test = false) {
00141         TimeData times;
00142         Pose p1, p2, p3;
00143         for ( unsigned int i = 0; i < REPEAT; ++i ) {
00144                 p1 = Pose(i*increment, 2*i*increment, 3.14*i*increment);
00145                 p2 = Pose(1.2*i*increment, 1.2*2*i*increment, 1.14*i*increment);
00146                 if ( inverse_test ) {
00147                         times.push_back(inverse(p1,p2,p3));
00148                 } else {
00149                         times.push_back(product(p1,p2,p3));
00150                 }
00151         }
00152         return times;
00153 }
00154 
00155 template <typename MatrixType>
00156 ECL_DONT_INLINE TimeData transform2DTest(bool inverse_test = false) {
00157         TimeData times;
00158         MatrixType t1 = MatrixType::Identity(), t2(t1), t3(t1);
00159         for ( unsigned int i = 0; i < REPEAT; ++i ) {
00160                 t1.linear() = Rotation2D<double>(3.14*i*increment).toRotationMatrix();
00161                 Vector2d v; v << i*increment, 2*i*increment;
00162                 t1.translation() = v;
00163                 t2.linear() = Rotation2D<double>(1.14*i*increment).toRotationMatrix();
00164                 t2.translation() = 1.2*v;
00165                 if ( inverse_test ) {
00166                         times.push_back(inverse(t1,t2,t3));
00167                 } else {
00168                         times.push_back(product(t1,t2,t3));
00169                 }
00170         }
00171         return times;
00172 }
00173 
00174 template <typename MatrixType>
00175 ECL_DONT_INLINE TimeData transform3DTest(bool inverse_test = false) {
00176         TimeData times;
00177         MatrixType t1 = MatrixType::Identity(), t2(t1), t3(t1);
00178         for ( unsigned int i = 0; i < REPEAT; ++i ) {
00179                 t1.linear() = rotation3D(3.14*i*increment,-3.14*i*increment, 1.0+i*increment );
00180                 Vector3d v; v << i*increment, 2*i*increment, 3*i*increment;
00181                 t1.translation() = v;
00182                 t2.linear() = rotation3D(3.14*i*increment,-3.14*i*increment, 1.0+i*increment );
00183                 t2.translation() = 1.2*v;
00184                 if ( inverse_test ) {
00185                         times.push_back(inverse(t1,t2,t3));
00186                 } else {
00187                         times.push_back(product(t1,t2,t3));
00188                 }
00189         }
00190         return times;
00191 }
00192 
00193 ECL_DONT_INLINE TimeData rotTrans3DTest(bool inverse_test = false) {
00194         StopWatch stopwatch;
00195         TimeData times;
00196         Matrix3d rot1 = Matrix3d::Identity(), rot2(rot1), rot3;
00197         Vector3d trans1, trans2, trans3;
00198         for ( unsigned int i = 0; i < REPEAT; ++i ) {
00199                 rot1 = rotation3D(3.14*i*increment,-3.14*i*increment, 1.0+i*increment );
00200                 rot1 = rotation3D(1.14*i*increment,-1.14*i*increment, 1.2+i*increment );
00201                 trans1 << i*increment, 2*i*increment, 3*i*increment;
00202                 trans1 << 2*i*increment, 1*i*increment, 3*i*increment;
00203                 if ( inverse_test ) {
00204                         stopwatch.restart();
00205                         rot3 = rot2*rot1;
00206                         trans3 = rot2*trans1 + trans2;
00207                         rot3 = rot1*rot1;
00208                         trans3 = rot1*trans1 + trans1;
00209                         rot3 = rot2*rot2;
00210                         trans3 = rot2*trans2 + trans2;
00211                         times.push_back(stopwatch.split());
00212                 } else { // inverse
00213                         stopwatch.restart();
00214                     rot3 = rot1.transpose();
00215                     trans3 = rot3*trans1*-1;
00216                     rot3 = rot2.transpose();
00217                     trans3 = rot3*trans2*-1;
00218                     rot2 = rot3.transpose();
00219                     trans2 = rot2*trans3*-1;
00220                         times.push_back(stopwatch.split());
00221                 }
00222         }
00223         return times;
00224 }
00225 
00226 ECL_DONT_INLINE TimeData matrix3DTest(bool inverse_test = false) {
00227         TimeData times;
00228         Matrix4d t1 = Matrix4d::Identity(), t2(t1), t3(t1);
00229         for ( unsigned int i = 0; i < REPEAT; ++i ) {
00230                 t1.block<3,3>(0,0) = rotation3D(3.14*i*increment,-3.14*i*increment, 1.0+i*increment );
00231                 Vector3d v; v << i*increment, 2*i*increment, 3*i*increment;
00232                 t1.block<3,1>(0,3) = v;
00233                 t2.block<3,3>(0,0) = rotation3D(3.14*i*increment,-3.14*i*increment, 1.0+i*increment );
00234                 t2.block<3,1>(0,3) = 1.2*v;
00235                 if ( inverse_test ) {
00236                         times.push_back(inverse(t1,t2,t3));
00237                 } else {
00238                         times.push_back(product(t1,t2,t3));
00239                 }
00240         }
00241         return times;
00242 }
00243 
00244 ECL_DONT_INLINE vector<Duration> trigTest() {
00245         StopWatch stopwatch;
00246         vector<Duration> times;
00247         double angle1 = 0.0, angle2 = 1.0, d, e;
00248         const unsigned long repeats = 1000000000L;
00249         double inc = 3.14/static_cast<double>(repeats);
00250         stopwatch.restart();
00251         for ( unsigned long i = 0; i < repeats; ++i ) {
00252                 angle1 += inc;
00253                 angle2 -= inc;
00254                 d = sin(angle1);
00255                 e = cos(angle2);
00256         }
00257         times.push_back(stopwatch.split());
00258         stopwatch.restart();
00259         for ( unsigned long i = 0; i < repeats; ++i ) {
00260                 angle1 += inc;
00261                 angle2 -= inc;
00262                 d = angle1*angle2;
00263                 e = 3.315*angle1;
00264         }
00265         times.push_back(stopwatch.split());
00266         return times;
00267 }
00268 /*****************************************************************************
00269 ** Main
00270 *****************************************************************************/
00271 
00272 int main(int argc, char **argv) {
00273 
00274         try {
00275                 ecl::set_priority(ecl::RealTimePriority4);
00276         } catch ( ecl::StandardException &e ) {
00277                 // dont worry about it.
00278         }
00279 
00280         TimeData times;
00281     const bool inverse_test = true; // Just a flag
00282 
00283     std::cout << std::endl;
00284     std::cout << "***********************************************************" << std::endl;
00285     std::cout << "                      Sizes" << std::endl;
00286     std::cout << "***********************************************************" << std::endl;
00287     std::cout << std::endl;
00288 
00289     std::cout << "Matrix4d     : " << sizeof(Matrix4d) << std::endl;
00290     std::cout << "3x3+3x1      : " << sizeof(Matrix3d) + sizeof(Vector3d) << std::endl;
00291     std::cout << "Affine3D     : " << sizeof(Transform<double,3,Affine>) << std::endl;
00292     std::cout << "AffineCom3D  : " << sizeof(Transform<double,3,AffineCompact>) << std::endl;
00293     std::cout << "Isometr3D    : " << sizeof(Transform<double,3,Isometry>) << std::endl;
00294     std::cout << "Rotation2D   : " << sizeof(Rotation2D<double>) << std::endl;
00295     std::cout << "Translation3D: " << sizeof(Translation<double,3>) << std::endl;
00296     std::cout << "Quaternion   : " << sizeof(Quaternion<double>) << std::endl;
00297 //
00298         std::cout << std::endl;
00299     std::cout << "***********************************************************" << std::endl;
00300     std::cout << "                     Trig vs *" << std::endl;
00301     std::cout << "***********************************************************" << std::endl;
00302     std::cout << std::endl;
00303 
00304     vector<Duration> trig_times = trigTest();
00305     std::cout << "Cos+Sin: " << trig_times[0] << std::endl;
00306     std::cout << "Two *'s: " << trig_times[1] << std::endl;
00307 
00308         std::cout << std::endl;
00309     std::cout << "***********************************************************" << std::endl;
00310     std::cout << "                    Products 2D" << std::endl;
00311     std::cout << "***********************************************************" << std::endl;
00312     std::cout << std::endl;
00313 
00314     times.clear(); times = pose2DTest<Pose2D>();
00315     std::cout << "Pose2D       : " << times.average() << " " << times.stdDev() <<  std::endl;
00316     times.clear(); times = pose2DTest<NewPose2D>();
00317     std::cout << "NewPose2D    : " << times.average() << " " << times.stdDev() <<  std::endl;
00318     times.clear(); times = transform2DTest< Transform<double,2,Affine> >();
00319     std::cout << "Affine       : " << times.average() << " " << times.stdDev() <<  std::endl;
00320     times.clear(); times = transform2DTest< Transform<double,2,AffineCompact> >();
00321     std::cout << "CompactAffine: " << times.average() << " " << times.stdDev() <<  std::endl;
00322     times.clear(); times = transform2DTest< Transform<double,2,Isometry> >();
00323     std::cout << "Isometry     : " << times.average() << " " << times.stdDev() <<  std::endl;
00324     std::cout << "CompactIso   : N/A" <<  std::endl;
00325 
00326         std::cout << std::endl;
00327     std::cout << "***********************************************************" << std::endl;
00328     std::cout << "                    Inverse 2D" << std::endl;
00329     std::cout << "***********************************************************" << std::endl;
00330     std::cout << std::endl;
00331 
00332     times.clear(); times = pose2DTest<Pose2D>(inverse_test);
00333     std::cout << "Pose2D       : " << times.average() << " " << times.stdDev() <<  std::endl;
00334     times.clear(); times = pose2DTest<NewPose2D>(inverse_test);
00335     std::cout << "NewPose2D    : " << times.average() << " " << times.stdDev() <<  std::endl;
00336     times.clear(); times = transform2DTest< Transform<double,2,Affine> >(inverse_test);
00337     std::cout << "Affine       : " << times.average() << " " << times.stdDev() <<  std::endl;
00338     times.clear(); times = transform2DTest< Transform<double,2,AffineCompact> >(inverse_test);
00339     std::cout << "CompactAffine: " << times.average() << " " << times.stdDev() <<  std::endl;
00340     times.clear(); times = transform2DTest< Transform<double,2,Isometry> >(inverse_test);
00341     std::cout << "Isometry     : " << times.average() << " " << times.stdDev() <<  std::endl;
00342     std::cout << "CompactIso   : N/A" <<  std::endl;
00343 
00344     std::cout << std::endl;
00345     std::cout << "***********************************************************" << std::endl;
00346     std::cout << "                    Products 3D" << std::endl;
00347     std::cout << "***********************************************************" << std::endl;
00348     std::cout << std::endl;
00349 
00350     times.clear(); times = rotTrans3DTest();
00351     std::cout << "Rot+Tra      : " << times.average() << " " << times.stdDev() <<  std::endl;
00352     times.clear(); times = matrix3DTest();
00353     std::cout << "Matrix4d     : " << times.average() << " " << times.stdDev() <<  std::endl;
00354     times.clear(); times = transform3DTest< Transform<double,3,Affine> >();
00355     std::cout << "Affine       : " << times.average() << " " << times.stdDev() <<  std::endl;
00356     times.clear(); times = transform3DTest< Transform<double,3,AffineCompact> >();
00357     std::cout << "CompactAffine: " << times.average() << " " << times.stdDev() <<  std::endl;
00358     times.clear(); times = transform3DTest< Transform<double,3,Isometry> >();
00359     std::cout << "Isometry     : " << times.average() << " " << times.stdDev() <<  std::endl;
00360     std::cout << "CompactIso   : N/A" <<  std::endl;
00361 
00362         std::cout << std::endl;
00363     std::cout << "***********************************************************" << std::endl;
00364     std::cout << "                    Inverse 3D" << std::endl;
00365     std::cout << "***********************************************************" << std::endl;
00366     std::cout << std::endl;
00367 
00368     times.clear(); times = rotTrans3DTest(inverse_test);
00369     std::cout << "Rot+Tra      : " << times.average() << " " << times.stdDev() <<  std::endl;
00370     times.clear(); times = matrix3DTest(inverse_test);
00371     std::cout << "Matrix4d     : " << times.average() << " " << times.stdDev() <<  std::endl;
00372     times.clear(); times = transform3DTest< Transform<double,3,Affine> >(inverse_test);
00373     std::cout << "Affine       : " << times.average() << " " << times.stdDev() <<  std::endl;
00374     times.clear(); times = transform3DTest< Transform<double,3,AffineCompact> >(inverse_test);
00375     std::cout << "CompactAffine: " << times.average() << " " << times.stdDev() <<  std::endl;
00376     times.clear(); times = transform3DTest< Transform<double,3,Isometry> >(inverse_test);
00377     std::cout << "Isometry     : " << times.average() << " " << times.stdDev() <<  std::endl;
00378     std::cout << "CompactIso   : N/A" <<  std::endl;
00379 
00380     std::cout << std::endl;
00381 
00382 //      for ( unsigned int i = 0; i < times.data().size(); ++i ) {
00383 //              std::cout << times.data()[i] << std::endl;
00384 //      }
00385 
00386 //      std::cout << std::endl;
00387 //    std::cout << "***********************************************************" << std::endl;
00388 //    std::cout << "                    Pose Unit Tests" << std::endl;
00389 //    std::cout << "***********************************************************" << std::endl;
00390 //    std::cout << std::endl;
00391 //
00392 //    Pose2D p1(1.0,0.0,1.57), p2(0.0,1.0,1.57), p3;
00393 //    NewPose2D o1(1.0,0.0,1.57), o2(0.0,1.0,1.57), o3;
00394 //    p3 = p2*p1;
00395 //    o3 = o2*o1;
00396 //    p3.print();
00397 //    o3.print();
00398 //    p3 = p2.inverse();
00399 //    o3 = o2.inverse();
00400 //    p3.print();
00401 //    o3.print();
00402         return 0;
00403 }


ecl_core_apps
Author(s): Daniel Stonier (d.stonier@gmail.com)
autogenerated on Thu Jan 2 2014 11:13:30