impleentation of SE2::between and gtest

include/core/math/SE2.h

@@ -408,7 +408,47 @@ inline VectorComposite plus(const VectorComposite& x, const VectorComposite& tau
     return res;
 }
 
+template<typename D1, typename D2, typename D3>
+inline void between(const MatrixBase<D1>& p1, const typename D1::Scalar& o1,
+                    const MatrixBase<D2>& p2, const typename D2::Scalar& o2,
+                    MatrixBase<D3>& p12, typename D3::Scalar& o12)
+{
+        MatrixSizeCheck<2, 1>::check(p1);
+        MatrixSizeCheck<2, 1>::check(p2);
+        MatrixSizeCheck<2, 1>::check(p12);
+
+        p12 = SO2::exp(-o1) * ( p2 - p1 );
+        o12 = o2 - o1;
+}
 
+template<typename D1, typename D2, typename D3>
+inline void between(const MatrixBase<D1>& d1,
+                    const MatrixBase<D2>& d2,
+                    MatrixBase<D3>& d12)
+{
+    MatrixSizeCheck<3, 1>::check(d1);
+    MatrixSizeCheck<3, 1>::check(d2);
+    MatrixSizeCheck<3, 1>::check(d12);
+
+    typedef typename D1::Scalar T;
+
+    Map<const Matrix<T, 2, 1> >   p1  ( & d1(0)  );
+    Map<const Matrix<T, 2, 1> >   p2  ( & d2(0)  );
+    Map<Matrix<T, 2, 1> >         p12 ( & d12(0) );
+
+    between(p1, d1(2), p2, d2(2), p12, d12(2));
+}
+
+template<typename D1, typename D2>
+inline Matrix<typename D1::Scalar, 3, 1> between(const MatrixBase<D1>& d1,
+                                                 const MatrixBase<D2>& d2 )
+{
+    MatrixSizeCheck<3, 1>::check(d1);
+    MatrixSizeCheck<3, 1>::check(d2);
+    Matrix<typename D1::Scalar, 3, 1> d12;
+    between(d1, d2, d12);
+    return d12;
+}
 
 } // namespace SE2
 } // namespacs wolf

test/CMakeLists.txt

@@ -126,11 +126,16 @@ target_link_libraries(gtest_processor_motion ${PLUGIN_NAME})
   
 # Rotation test
 wolf_add_gtest(gtest_rotation gtest_rotation.cpp)
+target_link_libraries(gtest_rotation ${PLUGIN_NAME})
 
 # SE3 test
 wolf_add_gtest(gtest_SE3 gtest_SE3.cpp)
 target_link_libraries(gtest_SE3 ${PLUGIN_NAME})
 
+# SE3 test
+wolf_add_gtest(gtest_SE2 gtest_SE2.cpp)
+target_link_libraries(gtest_SE2 ${PLUGIN_NAME})
+
 # SensorBase test
 wolf_add_gtest(gtest_sensor_base gtest_sensor_base.cpp)
 target_link_libraries(gtest_sensor_base ${PLUGIN_NAME})

test/gtest_SE2.cpp

+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program.  If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+
+
+#include "core/math/SE2.h"
+#include "core/utils/utils_gtest.h"
+
+using namespace Eigen;
+using namespace wolf;
+using namespace SE2;
+
+TEST(SE2, betweenBlocks)
+{
+    Vector2d p1, p2, pd;
+    double o1, o2, od;
+
+    // both origin: relative pose zero
+    p1 = Vector2d::Zero();
+    p2 = p1;
+    o1 = 0;
+    o2 = o1;
+    between(p1, o1, p2, o2, pd, od);
+
+    ASSERT_MATRIX_APPROX(pd, Eigen::Vector2d::Zero(), 1e-8);
+    ASSERT_NEAR(od, 0, 1e-8);
+
+    // both same random pose: relative pose zero
+    for (auto i = 0; i < 10; i++)
+    {
+        p1 = Vector2d::Random();
+        p2 = p1;
+        o1 = Vector1d::Random()(0) * M_PI;
+        o2 = o1;
+
+        between(p1, o1, p2, o2, pd, od);
+
+        ASSERT_MATRIX_APPROX(pd, Eigen::Vector2d::Zero(), 1e-8);
+        ASSERT_NEAR(od, 0, 1e-8);
+    }
+
+    // random relative pose
+    for (auto i = 0; i < 10; i++)
+    {
+        p1 = Vector2d::Random();
+        o1 = Vector1d::Random()(0) * M_PI;
+        Vector2d pd_gt = Vector2d::Random();
+        double od_gt = Vector1d::Random()(0) * M_PI;
+        
+        p2 = p1 + Rotation2Dd(o1) * pd_gt;
+        o2 = o1 + od_gt;
+
+        between(p1, o1, p2, o2, pd, od);
+
+        ASSERT_MATRIX_APPROX(pd, pd_gt, 1e-8);
+        ASSERT_NEAR(od, od_gt, 1e-8);
+    }
+}
+
+TEST(SE2, betweenVectors)
+{
+    Vector3d p1, p2, pd;
+
+    // both origin: relative pose zero
+    p1 = Vector3d::Zero();
+    p2 = p1;
+    between(p1, p2, pd);
+
+    ASSERT_MATRIX_APPROX(pd, Eigen::Vector3d::Zero(), 1e-8);
+
+    // both same random pose: relative pose zero
+    for (auto i = 0; i < 10; i++)
+    {
+        p1 = Vector3d::Random();
+        p1(2) *= M_PI;
+        p2 = p1;
+
+        between(p1, p2, pd);
+
+        ASSERT_MATRIX_APPROX(pd, Eigen::Vector3d::Zero(), 1e-8);
+    }
+
+    // random relative pose
+    for (auto i = 0; i < 10; i++)
+    {
+        p1 = Vector3d::Random();
+        p1(2) *= M_PI;
+        Vector3d pd_gt = Vector3d::Random();
+        pd_gt(2) *= M_PI;
+        
+        p2.head<2>() = p1.head<2>() + Rotation2Dd(p1(2)) * pd_gt.head<2>();
+        p2(2) = p1(2) + pd_gt(2);
+
+        between(p1, p2, pd);
+
+        ASSERT_MATRIX_APPROX(pd, pd_gt, 1e-8);
+    }
+}
+
+TEST(SE2, betweenVectorsReturn)
+{
+    Vector3d p1, p2, pd;
+
+    // both origin: relative pose zero
+    p1 = Vector3d::Zero();
+    p2 = p1;
+    pd = between(p1, p2);
+
+    ASSERT_MATRIX_APPROX(pd, Eigen::Vector3d::Zero(), 1e-8);
+
+    // both same random pose: relative pose zero
+    for (auto i = 0; i < 10; i++)
+    {
+        p1 = Vector3d::Random();
+        p1(2) *= M_PI;
+        p2 = p1;
+
+        pd = between(p1, p2);
+
+        ASSERT_MATRIX_APPROX(pd, Eigen::Vector3d::Zero(), 1e-8);
+    }
+
+    // random relative pose
+    for (auto i = 0; i < 10; i++)
+    {
+        p1 = Vector3d::Random();
+        p1(2) *= M_PI;
+        Vector3d pd_gt = Vector3d::Random();
+        pd_gt(2) *= M_PI;
+        
+        p2.head<2>() = p1.head<2>() + Rotation2Dd(p1(2)) * pd_gt.head<2>();
+        p2(2) = p1(2) + pd_gt(2);
+
+        pd = between(p1, p2);
+
+        ASSERT_MATRIX_APPROX(pd, pd_gt, 1e-8);
+    }
+}
+
+int main(int argc, char **argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}
+