$search
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) { 00126 testing::InitGoogleTest(&argc,argv); 00127 return RUN_ALL_TESTS(); 00128 }