00001
00009
00010
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
00025
00026
00027 using namespace ecl;
00028 using namespace Eigen;
00029 using std::vector;
00030
00031
00032
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
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 {
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
00270
00271
00272 int main(int argc, char **argv) {
00273
00274 try {
00275 ecl::set_priority(ecl::RealTimePriority4);
00276 } catch ( ecl::StandardException &e ) {
00277
00278 }
00279
00280 TimeData times;
00281 const bool inverse_test = true;
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
00383
00384
00385
00386
00387
00388
00389
00390
00391
00392
00393
00394
00395
00396
00397
00398
00399
00400
00401
00402 return 0;
00403 }