00001
00010
00011
00012
00013
00014 #define ECL_USE_EIGEN3 1
00015
00016
00017
00018
00019
00020 #include <gtest/gtest.h>
00021 #include <ecl/linear_algebra.hpp>
00022 #include <ecl/math/constants.hpp>
00023 #include "../../include/ecl/geometry/pose2d.hpp"
00024 #include "../../include/ecl/geometry/pose3d.hpp"
00025
00026
00027
00028
00029
00030 using ecl::Pose2D;
00031 using ecl::Pose3D;
00032 using ecl::linear_algebra::AngleAxis;
00033 using ecl::linear_algebra::Matrix3d;
00034 using ecl::linear_algebra::Vector3d;
00035 using ecl::linear_algebra::Quaternion;
00036 using ecl::linear_algebra::Translation3d;
00037
00038
00039
00040
00041
00042 TEST(Pose3DTests,constructors) {
00043 bool result;
00044 Vector3d v; v << 1.0,2.0,3.0;
00045 Matrix3d zero_rotation = Matrix3d::Identity();
00046 Vector3d zero_translation; zero_translation << 0.0,0.0,0.0;
00047 Pose2D<double> pose_2d(0.0,1.0,ecl::pi);
00048 Pose3D<double> pose1(zero_rotation,v);
00049 EXPECT_TRUE(pose1.rotation().isApprox(zero_rotation,0.1));
00050 EXPECT_TRUE(pose1.translation().isApprox(v,0.1));
00051 Pose3D<double> pose2(pose_2d);
00052 result = pose2.rotation().block<2,2>(0,0).isApprox(pose_2d.rotationMatrix(),0.1);
00053 EXPECT_TRUE(result);
00054 result = pose2.translation().segment<2>(0).isApprox(pose_2d.translation(),0.1);
00055 EXPECT_TRUE(result);
00056 Pose3D<double> pose3(Quaternion<double>(pose2.rotation()),v);
00057 EXPECT_TRUE(pose3.rotation().isApprox(pose2.rotation(),0.1));
00058 EXPECT_TRUE(pose3.translation().isApprox(v,0.1));
00059 Pose3D<double> pose4(pose3);
00060 EXPECT_TRUE(pose4.rotation().isApprox(pose3.rotation(),0.1));
00061 EXPECT_TRUE(pose4.translation().isApprox(pose3.translation(),0.1));
00062 }
00063
00064
00065 TEST(Pose3DTests,assignment) {
00066 Vector3d v; v << 0.0,1.0,0.0;
00067 Matrix3d rot; rot << -1.0, 0.0, 0.0,
00068 0.0, -1.0, 0.0,
00069 0.0, 0.0, 1.0;;
00070 Pose2D<double> pose_2d(0.0,1.0,ecl::pi);
00071 Pose3D<double> pose1, pose2;
00072 pose1 = pose_2d;
00073 EXPECT_TRUE(pose1.rotation().isApprox(rot,0.1));
00074 EXPECT_TRUE(pose1.translation().isApprox(v,0.1));
00075 pose2 = pose1;
00076 EXPECT_TRUE(pose2.rotation().isApprox(rot,0.1));
00077 EXPECT_TRUE(pose2.translation().isApprox(v,0.1));
00078 }
00079
00080 TEST(Pose3DTests,eigenStyle) {
00081 Vector3d v; v << 0.0,1.0,0.0;
00082 Matrix3d rot; rot << -1.0, 0.0, 0.0,
00083 0.0, -1.0, 0.0,
00084 0.0, 0.0, 1.0;;
00085 Pose3D<double> pose1;
00086 pose1.rotation(rot);
00087 pose1.translation(v);
00088 EXPECT_TRUE(pose1.rotation().isApprox(rot,0.1));
00089 EXPECT_TRUE(pose1.translation().isApprox(v,0.1));
00090 }
00091 TEST(Pose3DTests,inverse) {
00092 Vector3d z_axis; z_axis << 0.0, 0.0, 1.0;
00093 Vector3d zero = Vector3d::Zero();
00094 Matrix3d rot = AngleAxis<double>(ecl::pi/2.0,z_axis).toRotationMatrix();
00095 Vector3d trans; trans << 1.0, 2.0, 3.0;
00096 Pose3D<double> pose(rot, trans);
00097 Pose3D<double> inverse = pose.inverse();
00098 Pose3D<double> repose = pose*inverse;
00099 EXPECT_TRUE(Matrix3d::Identity().isApprox(repose.rotation(),0.1));
00100 for ( unsigned int i = 0; i < 3; ++i ) {
00101 EXPECT_LT(zero[i],repose.translation()[i]+0.01);
00102 EXPECT_GT(zero[i]+0.01,repose.translation()[i]);
00103 }
00104 }
00105
00106 TEST(Pose3DTests,operators) {
00107 Vector3d z_axis; z_axis << 0.0, 0.0, 1.0;
00108 Matrix3d rot = AngleAxis<double>(ecl::pi/2.0,z_axis).toRotationMatrix();
00109 Matrix3d rot_pi = AngleAxis<double>(ecl::pi,z_axis).toRotationMatrix();
00110 Vector3d trans; trans << 1.0, 2.0, 3.0;
00111 Vector3d expected_final_trans; expected_final_trans << -1.0, 3.0, 6.0;
00112 Pose3D<double> pose(rot, trans);
00113 Pose3D<double> final_pose = pose*pose;
00114 EXPECT_TRUE(rot_pi.isApprox(final_pose.rotation()));
00115 EXPECT_TRUE(expected_final_trans.isApprox(final_pose.translation()));
00116 pose *= pose;
00117 EXPECT_TRUE(rot_pi.isApprox(pose.rotation()));
00118 EXPECT_TRUE(expected_final_trans.isApprox(pose.translation()));
00119 }
00120
00121
00122
00123
00124
00125 int main(int argc, char **argv) {
00126 testing::InitGoogleTest(&argc,argv);
00127 return RUN_ALL_TESTS();
00128 }