pose3d.cpp
Go to the documentation of this file.
```00001
00010 /*****************************************************************************
00011 ** Preprocessor
00012 *****************************************************************************/
00013
00014 #define ECL_USE_EIGEN3 1
00015
00016 /*****************************************************************************
00017 ** Includes
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 ** Using
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 ** Tests
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 ** Main program
00123 *****************************************************************************/
00124
00125 int main(int argc, char **argv) {