ecl_geometry: ecl_geometry: pose2d.cpp Source File

00001 
00010 /*****************************************************************************
00011 ** Defines
00012 *****************************************************************************/
00013 
00014 #define ECL_USE_EIGEN3
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/angle.hpp"
00024 #include "../../include/ecl/geometry/pose2d.hpp"
00025 
00026 /*****************************************************************************
00027 ** Using
00028 *****************************************************************************/
00029 
00030 using ecl::Angle;
00031 using ecl::Pose2D;
00032 using ecl::RotationAngleStorage;
00033 using ecl::linear_algebra::Matrix2d;
00034 using ecl::linear_algebra::Vector2d;
00035 using ecl::linear_algebra::Vector3d;
00036 
00037 /*****************************************************************************
00038 ** Tests
00039 *****************************************************************************/
00040 
00041 TEST(Pose2DTests,RotationMatrixConstructors) {
00042         Pose2D<double> pose1;
00043         EXPECT_EQ(0.0,pose1.x());
00044         EXPECT_EQ(0.0,pose1.y());
00045         EXPECT_GT(1.01,pose1.rotation()(0,0));
00046         EXPECT_LT(0.99,pose1.rotation()(0,0));
00047         EXPECT_GT(0.01,pose1.rotation()(0,1));
00048         EXPECT_LT(-0.01,pose1.rotation()(0,1));
00049         EXPECT_GT(0.01,pose1.rotation()(1,0));
00050         EXPECT_LT(-0.01,pose1.rotation()(1,0));
00051         EXPECT_GT(1.01,pose1.rotation()(1,1));
00052         EXPECT_LT(0.99,pose1.rotation()(1,1));
00053         Pose2D<double> pose2(1.0, 2.0, 3.14);
00054         EXPECT_EQ(1.0,pose2.x());
00055         EXPECT_EQ(2.0,pose2.y());
00056         EXPECT_EQ(3.14,pose2.heading());
00057         Vector2d trans; trans << 1.0, 2.0;
00058         Matrix2d rot = Angle<double>(3.14).rotationMatrix();
00059         Pose2D<double> pose3(rot,trans);
00060         EXPECT_EQ(1.0,pose3.x());
00061         EXPECT_EQ(2.0,pose3.y());
00062         EXPECT_GT(3.15, pose3.heading());// Allow some roundoff error here
00063         EXPECT_LT(3.13, pose3.heading());
00064         Pose2D<double> pose4(3.14,trans);
00065         EXPECT_EQ(1.0,pose4.x());
00066         EXPECT_EQ(2.0,pose4.y());
00067         EXPECT_GT(3.15, pose4.heading());// Allow some roundoff error here
00068         EXPECT_LT(3.13, pose4.heading());
00069         Pose2D<double> pose5(pose4);
00070         EXPECT_EQ(1.0,pose5.x());
00071         EXPECT_EQ(2.0,pose5.y());
00072         EXPECT_GT(3.15, pose5.heading());// Allow some roundoff error here
00073         EXPECT_LT(3.13, pose5.heading());
00074         Pose2D<double,RotationAngleStorage> pose_other(3.14,trans);
00075         Pose2D<double> pose6(pose_other);
00076         EXPECT_EQ(1.0,pose6.x());
00077         EXPECT_EQ(2.0,pose6.y());
00078         EXPECT_GT(3.15, pose6.heading());// Allow some roundoff error here
00079         EXPECT_LT(3.13, pose6.heading());
00080 }
00081 
00082 TEST(Pose2DTests,RotationAngleConstructors) {
00083         Pose2D<double,RotationAngleStorage> pose1;
00084         EXPECT_EQ(0.0,pose1.x());
00085         EXPECT_EQ(0.0,pose1.y());
00086         EXPECT_GT(0.01,pose1.rotation());
00087         EXPECT_LT(-0.01,pose1.rotation());
00088         Pose2D<double,RotationAngleStorage> pose2(1.0, 2.0, 3.14);
00089         EXPECT_EQ(1.0,pose2.x());
00090         EXPECT_EQ(2.0,pose2.y());
00091         EXPECT_EQ(3.14,pose2.heading());
00092         Vector2d trans; trans << 1.0, 2.0;
00093         Matrix2d rot = Angle<double>(3.14).rotationMatrix();
00094         Pose2D<double,RotationAngleStorage> pose3(rot,trans);
00095         EXPECT_EQ(1.0,pose3.x());
00096         EXPECT_EQ(2.0,pose3.y());
00097         EXPECT_GT(3.15, pose3.heading());// Allow some roundoff error here
00098         EXPECT_LT(3.13, pose3.heading());
00099         Pose2D<double,RotationAngleStorage> pose4(3.14,trans);
00100         EXPECT_EQ(1.0,pose4.x());
00101         EXPECT_EQ(2.0,pose4.y());
00102         EXPECT_GT(3.15, pose4.heading());// Allow some roundoff error here
00103         EXPECT_LT(3.13, pose4.heading());
00104         Pose2D<double,RotationAngleStorage> pose5(pose4);
00105         EXPECT_EQ(1.0,pose5.x());
00106         EXPECT_EQ(2.0,pose5.y());
00107         EXPECT_GT(3.15, pose5.heading());// Allow some roundoff error here
00108         EXPECT_LT(3.13, pose5.heading());
00109         Pose2D<double> pose_other(1.0, 2.0, 3.14);
00110         Pose2D<double,RotationAngleStorage> pose6(pose_other);
00111         EXPECT_EQ(1.0,pose6.x());
00112         EXPECT_EQ(2.0,pose6.y());
00113         EXPECT_GT(3.15, pose6.heading());// Allow some roundoff error here
00114         EXPECT_LT(3.13, pose6.heading());
00115 }
00116 
00117 TEST(Pose2DTests,assignment) {
00118         Pose2D<double> pose_r1(2.0, 1.0, 1.14);
00119         Pose2D<double,RotationAngleStorage> pose_a1(1.0, 2.0, 3.14);
00120         Pose2D<double> pose_r2;
00121         Pose2D<double,RotationAngleStorage> pose_a2;
00122         pose_r2 = pose_r1;
00123         EXPECT_EQ(2.0,pose_r2.x());
00124         EXPECT_EQ(1.0,pose_r2.y());
00125         EXPECT_GT(1.15, pose_r2.heading());// Allow some roundoff error here
00126         EXPECT_LT(1.13, pose_r2.heading());
00127         pose_r2 = pose_a1;
00128         EXPECT_EQ(1.0,pose_r2.x());
00129         EXPECT_EQ(2.0,pose_r2.y());
00130         EXPECT_GT(3.15, pose_r2.heading());// Allow some roundoff error here
00131         EXPECT_LT(3.13, pose_r2.heading());
00132         pose_a2 = pose_r1;
00133         EXPECT_EQ(2.0,pose_a2.x());
00134         EXPECT_EQ(1.0,pose_a2.y());
00135         EXPECT_GT(1.15, pose_a2.heading());// Allow some roundoff error here
00136         EXPECT_LT(1.13, pose_a2.heading());
00137         pose_a2 = pose_a1;
00138         EXPECT_EQ(1.0,pose_a2.x());
00139         EXPECT_EQ(2.0,pose_a2.y());
00140         EXPECT_GT(3.15, pose_a2.heading());// Allow some roundoff error here
00141         EXPECT_LT(3.13, pose_a2.heading());
00142 }
00143 
00144 TEST(Pose2DTests,eigenStyle) {
00145         Vector2d trans; trans << 1.0, 2.0;
00146         Matrix2d rot = Angle<double>(3.14).rotationMatrix();
00147         Pose2D<double> rpose;
00148         rpose.rotation(rot);
00149         rpose.translation(trans);
00150         EXPECT_EQ(1.0,rpose.x());
00151         EXPECT_EQ(2.0,rpose.y());
00152         EXPECT_GT(3.15, rpose.heading());// Allow some roundoff error here
00153         EXPECT_LT(3.13, rpose.heading());
00154         rot = rpose.rotation();
00155         trans = rpose.translation();
00156         EXPECT_EQ(1.0,trans[0]);
00157         EXPECT_EQ(2.0,trans[1]);
00158         EXPECT_GT(3.15, Angle<double>(rot));// Allow some roundoff error here
00159         EXPECT_LT(3.13, Angle<double>(rot));
00160         Pose2D<double,RotationAngleStorage> apose;
00161         apose.rotation(3.14);
00162         apose.translation(trans);
00163         EXPECT_EQ(1.0,apose.x());
00164         EXPECT_EQ(2.0,apose.y());
00165         EXPECT_GT(3.15, apose.heading());// Allow some roundoff error here
00166         EXPECT_LT(3.13, apose.heading());
00167         double angle = apose.rotation();
00168         trans = apose.translation();
00169         EXPECT_EQ(1.0,trans[0]);
00170         EXPECT_EQ(2.0,trans[1]);
00171         EXPECT_GT(3.15, angle);// Allow some roundoff error here
00172         EXPECT_LT(3.13, angle);
00173 }
00174 TEST(Pose2DTests,convenienceStyle) {
00175         Vector2d trans; trans << 1.0, 2.0;
00176         Matrix2d rot;
00177         Pose2D<double> rpose;
00178         rpose.x(1.0); rpose.y(2.0); rpose.heading(3.14);
00179         EXPECT_EQ(1.0,rpose.x());
00180         EXPECT_EQ(2.0,rpose.y());
00181         EXPECT_GT(3.15, rpose.heading());// Allow some roundoff error here
00182         EXPECT_LT(3.13, rpose.heading());
00183         rpose.heading(0.0);
00184         rot = rpose.rotationMatrix();
00185         EXPECT_GT(1.01,rot(0,0));
00186         EXPECT_LT(0.99,rot(0,0));
00187         EXPECT_GT(0.01,rot(0,1));
00188         EXPECT_LT(-0.01,rot(0,1));
00189         EXPECT_GT(0.01,rot(1,0));
00190         EXPECT_LT(-0.01,rot(1,0));
00191         EXPECT_GT(1.01,rot(1,1));
00192         EXPECT_LT(0.99,rot(1,1));
00193         Pose2D<double,RotationAngleStorage> apose;
00194         apose.x(1.0); apose.y(2.0); apose.heading(3.14);
00195         EXPECT_EQ(1.0,apose.x());
00196         EXPECT_EQ(2.0,apose.y());
00197         EXPECT_GT(3.15, apose.heading());// Allow some roundoff error here
00198         EXPECT_LT(3.13, apose.heading());
00199         apose.heading(0.0);
00200         rot = apose.rotationMatrix();
00201         EXPECT_GT(1.01,rot(0,0));
00202         EXPECT_LT(0.99,rot(0,0));
00203         EXPECT_GT(0.01,rot(0,1));
00204         EXPECT_LT(-0.01,rot(0,1));
00205         EXPECT_GT(0.01,rot(1,0));
00206         EXPECT_LT(-0.01,rot(1,0));
00207         EXPECT_GT(1.01,rot(1,1));
00208         EXPECT_LT(0.99,rot(1,1));
00209 }
00210 TEST(Pose2DTests,inverse) {
00211         Pose2D<double> pose(1.0, 2.0, ecl::pi);
00212         Pose2D<double> inverse = pose.inverse();
00213         EXPECT_GT(1.01,inverse.x()); // Allow some roundoff error here
00214         EXPECT_LT(0.99,inverse.x());
00215         EXPECT_GT(2.01,inverse.y()); // Allow some roundoff error here
00216         EXPECT_LT(1.99,inverse.y());
00217         EXPECT_GT(-ecl::pi+0.01,inverse.heading()); // Allow some roundoff error here
00218         EXPECT_LT(-ecl::pi-0.01,inverse.heading());
00219         Pose2D<double,RotationAngleStorage> apose(1.0, 2.0, ecl::pi);
00220         Pose2D<double,RotationAngleStorage> ainverse = pose.inverse();
00221         EXPECT_GT(1.01,ainverse.x()); // Allow some roundoff error here
00222         EXPECT_LT(0.99,ainverse.x());
00223         EXPECT_GT(2.01,ainverse.y()); // Allow some roundoff error here
00224         EXPECT_LT(1.99,ainverse.y());
00225         EXPECT_GT(-ecl::pi+0.01,ainverse.heading()); // Allow some roundoff error here
00226         EXPECT_LT(-ecl::pi-0.01,ainverse.heading());
00227 }
00228 
00229 TEST(Pose2DTests,operators) {
00230         Pose2D<double> a(1.0, 2.0, 0.0), b(1.0, 3.0, ecl::pi);
00231         Pose2D<double> diff = a.inverse()*b; // diff = b - a;
00232         EXPECT_EQ(0.0,diff.x());
00233         EXPECT_EQ(1.0,diff.y());
00234         EXPECT_EQ(ecl::pi,diff.heading());
00235         Pose2D<double,RotationAngleStorage> a2(1.0, 2.0, 0.0), b2(1.0, 3.0, ecl::pi);
00236         Pose2D<double,RotationAngleStorage> diff2 = a2.inverse()*b2; // diff = b - a;
00237         EXPECT_EQ(0.0,diff2.x());
00238         EXPECT_EQ(1.0,diff2.y());
00239         EXPECT_EQ(ecl::pi,diff2.heading());
00240 }
00241 TEST(Pose2DTests,relative) {
00242         Pose2D<double> a(1.0, 1.0, 1.57), b(1.0, 2.0, 3.14);
00243         Pose2D<double> brela = b.relative(a);
00244         EXPECT_GT(1.01,brela.x());
00245         EXPECT_LT(0.99,brela.x());
00246         EXPECT_GT(0.01,brela.y());
00247         EXPECT_LT(-0.01,brela.y());
00248         EXPECT_GT(1.58,brela.heading());
00249         EXPECT_LT(1.56,brela.heading());
00250         Pose2D<double,RotationAngleStorage> a_(1.0, 1.0, 1.57), b_(1.0, 2.0, 3.14);
00251         Pose2D<double,RotationAngleStorage> brela_ = b.relative(a);
00252         EXPECT_GT(1.01,brela_.x());
00253         EXPECT_LT(0.99,brela_.x());
00254         EXPECT_GT(0.01,brela_.y());
00255         EXPECT_LT(-0.01,brela_.y());
00256         EXPECT_GT(1.58,brela_.heading());
00257         EXPECT_LT(1.56,brela_.heading());
00258 }
00259 
00260 /*****************************************************************************
00261 ** Main program
00262 *****************************************************************************/
00263 
00264 int main(int argc, char **argv) {
00265         testing::InitGoogleTest(&argc,argv);
00266     return RUN_ALL_TESTS();
00267 }