added SE2 inverse and tests

include/core/math/SE2.h

@@ -102,6 +102,38 @@ inline VectorComposite identity()
     return v;
 }
 
+template<typename D1, typename D2>
+inline void inverse(const MatrixBase<D1>& p, const typename D1::Scalar& o,
+                    MatrixBase<D2>& ip, typename D2::Scalar& io)
+{
+    MatrixSizeCheck<2, 1>::check(p);
+    MatrixSizeCheck<2, 1>::check(ip);
+
+    ip = SO2::exp(-o) * -p;
+    io = -o;
+}
+
+template<typename D1, typename D2>
+inline void inverse(const MatrixBase<D1>& d,
+                    MatrixBase<D2>& id)
+{
+    MatrixSizeCheck<3, 1>::check(d);
+    MatrixSizeCheck<3, 1>::check(id);
+
+    Map<const Matrix<typename D1::Scalar, 2, 1> >   p   ( & d( 0 ) );
+    Map<Matrix<typename D2::Scalar, 2, 1> >         ip  ( & id( 0 ) );
+
+    inverse(p, d(2), ip, id(2));
+}
+
+template<typename D>
+inline Matrix<typename D::Scalar, 3, 1> inverse(const MatrixBase<D>& d)
+{
+    Matrix<typename D::Scalar, 3, 1> id;
+    inverse(d, id);
+    return id;
+}
+
 template<class D1, class D2>
 inline void exp(const Eigen::MatrixBase<D1>& _tau, Eigen::MatrixBase<D2>& _delta)
 {

test/gtest_SE2.cpp

@@ -28,6 +28,102 @@ using namespace Eigen;
 using namespace wolf;
 using namespace SE2;
 
+TEST(SE2, exp)
+{
+    for (auto i = 1; i<10; i++)
+    {
+        double o = Vector1d::Random()(0) * M_PI;
+        ASSERT_MATRIX_APPROX(SO2::exp(o), Rotation2Dd(o).matrix(), 1e-8);
+    }
+}
+
+TEST(SE2, invBlocks)
+{
+    Vector2d p, ip;
+    double o, io;
+
+    // zero
+    p = Vector2d::Zero();
+    o = 0;
+
+    SE2::inverse(p,o,ip,io);
+    
+    ASSERT_MATRIX_APPROX(ip, Vector2d::Zero(), 1e-8);
+    ASSERT_NEAR(io, 0, 1e-8);
+
+    // zero angle
+    p = Vector2d::Random();
+    o = 0;
+
+    SE2::inverse(p,o,ip,io);
+    
+    ASSERT_MATRIX_APPROX(ip, -p, 1e-8);
+    ASSERT_NEAR(io, 0, 1e-8);
+
+    // zero translation
+    p = Vector2d::Zero();
+    o = Vector1d::Random()(0) * M_PI;
+
+    SE2::inverse(p,o,ip,io);
+    
+    ASSERT_MATRIX_APPROX(ip, Vector2d::Zero(), 1e-8);
+    ASSERT_NEAR(io, -o, 1e-8);
+
+    // random
+    for (auto i = 0; i < 10; i++)
+    {
+        p = Vector2d::Random();
+        o = Vector1d::Random()(0) * M_PI;
+
+        SE2::inverse(p,o,ip,io);
+        
+        ASSERT_MATRIX_APPROX(ip, Rotation2Dd(-o).matrix() * -p, 1e-8);
+        ASSERT_NEAR(io, -o, 1e-8);
+    }
+}
+
+TEST(SE2, invVector)
+{
+    Vector3d d, id;
+
+    // zero
+    d = Vector3d::Zero();
+
+    SE2::inverse(d,id);
+    
+    ASSERT_MATRIX_APPROX(id, Vector3d::Zero(), 1e-8);
+
+    // zero angle
+    d = Vector3d::Random();
+    d(2) = 0;
+
+    SE2::inverse(d,id);
+    
+    ASSERT_MATRIX_APPROX(id.head<2>(), -d.head<2>(), 1e-8);
+    ASSERT_NEAR(id(2), 0, 1e-8);
+
+    // zero translation
+    d = Vector3d::Zero();
+    d(2) = Vector1d::Random()(0) * M_PI;
+
+    SE2::inverse(d,id);
+    
+    ASSERT_MATRIX_APPROX(id.head<2>(), Vector2d::Zero(), 1e-8);
+    ASSERT_NEAR(id(2), -d(2), 1e-8);
+
+    // random
+    for (auto i = 0; i < 10; i++)
+    {
+        d = Vector3d::Random();
+        d(2) *= M_PI;
+
+        SE2::inverse(d,id);
+        
+        ASSERT_MATRIX_APPROX(id.head<2>(), Rotation2Dd(-d(2)) * -d.head<2>(), 1e-8);
+        ASSERT_NEAR(id(2), -d(2), 1e-8);
+    }
+}
+
 TEST(SE2, betweenBlocks)
 {
     Vector2d p1, p2, pd;
@@ -40,7 +136,7 @@ TEST(SE2, betweenBlocks)
     o2 = o1;
     between(p1, o1, p2, o2, pd, od);
 
-    ASSERT_MATRIX_APPROX(pd, Eigen::Vector2d::Zero(), 1e-8);
+    ASSERT_MATRIX_APPROX(pd, Vector2d::Zero(), 1e-8);
     ASSERT_NEAR(od, 0, 1e-8);
 
     // both same random pose: relative pose zero
@@ -53,7 +149,7 @@ TEST(SE2, betweenBlocks)
 
         between(p1, o1, p2, o2, pd, od);
 
-        ASSERT_MATRIX_APPROX(pd, Eigen::Vector2d::Zero(), 1e-8);
+        ASSERT_MATRIX_APPROX(pd, Vector2d::Zero(), 1e-8);
         ASSERT_NEAR(od, 0, 1e-8);
     }
 
@@ -84,7 +180,7 @@ TEST(SE2, betweenVectors)
     p2 = p1;
     between(p1, p2, pd);
 
-    ASSERT_MATRIX_APPROX(pd, Eigen::Vector3d::Zero(), 1e-8);
+    ASSERT_MATRIX_APPROX(pd, Vector3d::Zero(), 1e-8);
 
     // both same random pose: relative pose zero
     for (auto i = 0; i < 10; i++)
@@ -95,7 +191,7 @@ TEST(SE2, betweenVectors)
 
         between(p1, p2, pd);
 
-        ASSERT_MATRIX_APPROX(pd, Eigen::Vector3d::Zero(), 1e-8);
+        ASSERT_MATRIX_APPROX(pd, Vector3d::Zero(), 1e-8);
     }
 
     // random relative pose
@@ -124,7 +220,7 @@ TEST(SE2, betweenVectorsReturn)
     p2 = p1;
     pd = between(p1, p2);
 
-    ASSERT_MATRIX_APPROX(pd, Eigen::Vector3d::Zero(), 1e-8);
+    ASSERT_MATRIX_APPROX(pd, Vector3d::Zero(), 1e-8);
 
     // both same random pose: relative pose zero
     for (auto i = 0; i < 10; i++)
@@ -135,7 +231,7 @@ TEST(SE2, betweenVectorsReturn)
 
         pd = between(p1, p2);
 
-        ASSERT_MATRIX_APPROX(pd, Eigen::Vector3d::Zero(), 1e-8);
+        ASSERT_MATRIX_APPROX(pd, Vector3d::Zero(), 1e-8);
     }
 
     // random relative pose