diff --git a/include/core/math/SE2.h b/include/core/math/SE2.h
index 4ce5ab465d6e87ec8e7cacf5af05c06f2a6db32c..34ff6727a74a5e364348538b921b6f32094b665c 100644
--- a/include/core/math/SE2.h
+++ b/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
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index 743787f40026b6449169470e1d3eb66c14bddc38..17738bba01efaf24b170c0e25e9be7feb7e363d7 100644
--- a/test/CMakeLists.txt
+++ b/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})
diff --git a/test/gtest_SE2.cpp b/test/gtest_SE2.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..751637ed33b9ef406c11e20356f13f0e624c61fc
--- /dev/null
+++ b/test/gtest_SE2.cpp
@@ -0,0 +1,163 @@
+//--------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();
+}
+