28 using namespace Eigen;
38 NewPose2D(
const double &x,
const double &y,
const double& theta) :
39 rotation(Rotation2D<double>(theta).toRotationMatrix())
56 Rotation2D<double> angle(0.0);
57 angle.fromRotationMatrix(rotation);
58 std::cout <<
"[ " << translation[0] <<
"," << translation[1] <<
"," << angle.angle() <<
"]" << std::endl;
67 Pose2D(
const double &x_=0.0,
const double &y_=0.0,
const double&theta=0.0) :
68 x(x_), y(y_), rotation(theta)
70 Pose2D(
const Pose2D& pose) : x(pose.x), y(pose.y), rotation(pose.rotation) {}
72 double s = sin(rotation);
73 double c = cos(rotation);
74 return Pose2D(c*x-s*y, s*x+c*y, -rotation);
78 double s = sin(rotation);
79 double c = cos(rotation);
81 y+s*pose.
x + c*pose.
y,
89 std::cout <<
"[ " << x <<
"," << y <<
"," << rotation <<
"]" << std::endl;
103 return static_cast<double>(rand()%99)/100.0;
108 Ralpha << 1.0, 0.0, 0.0,
109 0.0, cos(alpha), -sin(alpha),
110 0.0, sin(alpha), cos(alpha);
111 Rbeta << cos(beta), 0.0, sin(beta),
113 -sin(beta), 0.0, cos(beta);
114 Rgamma << cos(gamma), -sin(gamma), 0.0,
115 sin(gamma), cos(gamma), 0.0,
117 return Ralpha*Rbeta*Rgamma;
121 template <
typename MatrixType>
127 return stopwatch.split();
130 template <
typename MatrixType>
136 return stopwatch.split();
139 template<
typename Pose>
143 for (
unsigned int i = 0; i <
REPEAT; ++i ) {
146 if ( inverse_test ) {
155 template <
typename MatrixType>
158 MatrixType t1 = MatrixType::Identity(), t2(t1), t3(t1);
159 for (
unsigned int i = 0; i <
REPEAT; ++i ) {
160 t1.linear() = Rotation2D<double>(3.14*i*
increment).toRotationMatrix();
162 t1.translation() = v;
163 t2.linear() = Rotation2D<double>(1.14*i*
increment).toRotationMatrix();
164 t2.translation() = 1.2*v;
165 if ( inverse_test ) {
174 template <
typename MatrixType>
177 MatrixType t1 = MatrixType::Identity(), t2(t1), t3(t1);
178 for (
unsigned int i = 0; i <
REPEAT; ++i ) {
181 t1.translation() = v;
183 t2.translation() = 1.2*v;
184 if ( inverse_test ) {
196 Matrix3d rot1 = Matrix3d::Identity(), rot2(rot1), rot3;
198 for (
unsigned int i = 0; i <
REPEAT; ++i ) {
203 if ( inverse_test ) {
206 trans3 = rot2*trans1 + trans2;
208 trans3 = rot1*trans1 + trans1;
210 trans3 = rot2*trans2 + trans2;
214 rot3 = rot1.transpose();
215 trans3 = rot3*trans1*-1;
216 rot3 = rot2.transpose();
217 trans3 = rot3*trans2*-1;
218 rot2 = rot3.transpose();
219 trans2 = rot2*trans3*-1;
228 Matrix4d t1 = Matrix4d::Identity(), t2(t1), t3(t1);
229 for (
unsigned int i = 0; i <
REPEAT; ++i ) {
232 t1.block<3,1>(0,3) = v;
234 t2.block<3,1>(0,3) = 1.2*v;
235 if ( inverse_test ) {
246 vector<Duration> times;
247 double angle1 = 0.0, angle2 = 1.0,
d, e;
248 const unsigned long repeats = 1000000000L;
249 double inc = 3.14/
static_cast<double>(repeats);
251 for (
unsigned long i = 0; i < repeats; ++i ) {
257 times.push_back(stopwatch.split());
259 for (
unsigned long i = 0; i < repeats; ++i ) {
265 times.push_back(stopwatch.split());
272 int main(
int argc,
char **argv) {
281 const bool inverse_test =
true;
283 std::cout << std::endl;
284 std::cout <<
"***********************************************************" << std::endl;
285 std::cout <<
" Sizes" << std::endl;
286 std::cout <<
"***********************************************************" << std::endl;
287 std::cout << std::endl;
289 std::cout <<
"Matrix4d : " <<
sizeof(
Matrix4d) << std::endl;
290 std::cout <<
"3x3+3x1 : " <<
sizeof(
Matrix3d) +
sizeof(
Vector3d) << std::endl;
291 std::cout <<
"Affine3D : " <<
sizeof(Transform<double,3,Affine>) << std::endl;
292 std::cout <<
"AffineCom3D : " <<
sizeof(Transform<double,3,AffineCompact>) << std::endl;
293 std::cout <<
"Isometr3D : " <<
sizeof(Transform<double,3,Isometry>) << std::endl;
294 std::cout <<
"Rotation2D : " <<
sizeof(Rotation2D<double>) << std::endl;
295 std::cout <<
"Translation3D: " <<
sizeof(Translation<double,3>) << std::endl;
296 std::cout <<
"Quaternion : " <<
sizeof(Quaternion<double>) << std::endl;
298 std::cout << std::endl;
299 std::cout <<
"***********************************************************" << std::endl;
300 std::cout <<
" Trig vs *" << std::endl;
301 std::cout <<
"***********************************************************" << std::endl;
302 std::cout << std::endl;
304 vector<Duration> trig_times =
trigTest();
305 std::cout <<
"Cos+Sin: " << trig_times[0] << std::endl;
306 std::cout <<
"Two *'s: " << trig_times[1] << std::endl;
308 std::cout << std::endl;
309 std::cout <<
"***********************************************************" << std::endl;
310 std::cout <<
" Products 2D" << std::endl;
311 std::cout <<
"***********************************************************" << std::endl;
312 std::cout << std::endl;
314 times.
clear(); times = pose2DTest<Pose2D>();
315 std::cout <<
"Pose2D : " << times.
average() <<
" " << times.
stdDev() << std::endl;
316 times.
clear(); times = pose2DTest<NewPose2D>();
317 std::cout <<
"NewPose2D : " << times.
average() <<
" " << times.
stdDev() << std::endl;
318 times.
clear(); times = transform2DTest< Transform<double,2,Affine> >();
319 std::cout <<
"Affine : " << times.
average() <<
" " << times.
stdDev() << std::endl;
320 times.
clear(); times = transform2DTest< Transform<double,2,AffineCompact> >();
321 std::cout <<
"CompactAffine: " << times.
average() <<
" " << times.
stdDev() << std::endl;
322 times.
clear(); times = transform2DTest< Transform<double,2,Isometry> >();
323 std::cout <<
"Isometry : " << times.
average() <<
" " << times.
stdDev() << std::endl;
324 std::cout <<
"CompactIso : N/A" << std::endl;
326 std::cout << std::endl;
327 std::cout <<
"***********************************************************" << std::endl;
328 std::cout <<
" Inverse 2D" << std::endl;
329 std::cout <<
"***********************************************************" << std::endl;
330 std::cout << std::endl;
332 times.
clear(); times = pose2DTest<Pose2D>(inverse_test);
333 std::cout <<
"Pose2D : " << times.
average() <<
" " << times.
stdDev() << std::endl;
334 times.
clear(); times = pose2DTest<NewPose2D>(inverse_test);
335 std::cout <<
"NewPose2D : " << times.
average() <<
" " << times.
stdDev() << std::endl;
336 times.
clear(); times = transform2DTest< Transform<double,2,Affine> >(inverse_test);
337 std::cout <<
"Affine : " << times.
average() <<
" " << times.
stdDev() << std::endl;
338 times.
clear(); times = transform2DTest< Transform<double,2,AffineCompact> >(inverse_test);
339 std::cout <<
"CompactAffine: " << times.
average() <<
" " << times.
stdDev() << std::endl;
340 times.
clear(); times = transform2DTest< Transform<double,2,Isometry> >(inverse_test);
341 std::cout <<
"Isometry : " << times.
average() <<
" " << times.
stdDev() << std::endl;
342 std::cout <<
"CompactIso : N/A" << std::endl;
344 std::cout << std::endl;
345 std::cout <<
"***********************************************************" << std::endl;
346 std::cout <<
" Products 3D" << std::endl;
347 std::cout <<
"***********************************************************" << std::endl;
348 std::cout << std::endl;
351 std::cout <<
"Rot+Tra : " << times.
average() <<
" " << times.
stdDev() << std::endl;
353 std::cout <<
"Matrix4d : " << times.
average() <<
" " << times.
stdDev() << std::endl;
354 times.
clear(); times = transform3DTest< Transform<double,3,Affine> >();
355 std::cout <<
"Affine : " << times.
average() <<
" " << times.
stdDev() << std::endl;
356 times.
clear(); times = transform3DTest< Transform<double,3,AffineCompact> >();
357 std::cout <<
"CompactAffine: " << times.
average() <<
" " << times.
stdDev() << std::endl;
358 times.
clear(); times = transform3DTest< Transform<double,3,Isometry> >();
359 std::cout <<
"Isometry : " << times.
average() <<
" " << times.
stdDev() << std::endl;
360 std::cout <<
"CompactIso : N/A" << std::endl;
362 std::cout << std::endl;
363 std::cout <<
"***********************************************************" << std::endl;
364 std::cout <<
" Inverse 3D" << std::endl;
365 std::cout <<
"***********************************************************" << std::endl;
366 std::cout << std::endl;
369 std::cout <<
"Rot+Tra : " << times.
average() <<
" " << times.
stdDev() << std::endl;
371 std::cout <<
"Matrix4d : " << times.
average() <<
" " << times.
stdDev() << std::endl;
372 times.
clear(); times = transform3DTest< Transform<double,3,Affine> >(inverse_test);
373 std::cout <<
"Affine : " << times.
average() <<
" " << times.
stdDev() << std::endl;
374 times.
clear(); times = transform3DTest< Transform<double,3,AffineCompact> >(inverse_test);
375 std::cout <<
"CompactAffine: " << times.
average() <<
" " << times.
stdDev() << std::endl;
376 times.
clear(); times = transform3DTest< Transform<double,3,Isometry> >(inverse_test);
377 std::cout <<
"Isometry : " << times.
average() <<
" " << times.
stdDev() << std::endl;
378 std::cout <<
"CompactIso : N/A" << std::endl;
380 std::cout << std::endl;