$search
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 }