diff --git a/CMakeLists.txt b/CMakeLists.txt
index 25e2b850ce1593711b470bcb2e9262e97ac2e155..b0008c84528c43c7b8c854cbdf23377efb1c6537 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.18)
+cmake_minimum_required(VERSION 3.18 FATAL_ERROR)
# default settings
set(ARMARX_ENABLE_DEPENDENCY_VERSION_CHECK_DEFAULT FALSE)
@@ -25,18 +25,20 @@ armarx_find_package(PUBLIC VisionX QUIET)
# - optional
armarx_find_package(PUBLIC OpenMP QUIET)
-armarx_find_package(PUBLIC Ceres QUIET)
+armarx_find_package(PUBLIC Ceres 2.0.0 EXACT QUIET)
armarx_find_package(PUBLIC VTK QUIET)
armarx_find_package(PUBLIC SemanticObjectRelations QUIET)
armarx_find_package(PUBLIC OpenCV QUIET) # Required as RobotAPI is a legacy project.
+armarx_find_package(PUBLIC range-v3 CONFIG REQUIRED)
add_subdirectory(etc)
-set(RANGES_VERBOSE_BUILD OFF)
-set(RANGES_RELEASE_BUILD ON)
-set(RANGES_CXX_STD 17)
-add_subdirectory(external)
+
+# set(RANGES_VERBOSE_BUILD OFF)
+# set(RANGES_RELEASE_BUILD ON)
+# set(RANGES_CXX_STD 17)
+# add_subdirectory(external)
#include(FetchContent)
diff --git a/source/armarx/navigation/algorithms/CMakeLists.txt b/source/armarx/navigation/algorithms/CMakeLists.txt
index c93bbffc732430e1462164322d70bdb8512bb461..bff2cb2ae4768543bebcaf17f3170674631c1284 100644
--- a/source/armarx/navigation/algorithms/CMakeLists.txt
+++ b/source/armarx/navigation/algorithms/CMakeLists.txt
@@ -1,10 +1,14 @@
armarx_add_library(algorithms
DEPENDENCIES
- ArmarXCoreInterfaces
- ArmarXCore
- armarx_navigation::core
- armarx_navigation::conversions
- OpenMP::OpenMP_CXX
+ PUBLIC
+ ArmarXCoreInterfaces
+ ArmarXCore
+ armarx_navigation::core
+ armarx_navigation::conversions
+ PRIVATE
+ OpenMP::OpenMP_CXX
+ range-v3::range-v3
+ Ceres::ceres
DEPENDENCIES_LEGACY
OpenCV
SOURCES
@@ -23,6 +27,7 @@ armarx_add_library(algorithms
# smoothing
./smoothing/ChainApproximation.cpp
./smoothing/CircularPathSmoothing.cpp
+ ./math/ellipse.cpp
HEADERS
./algorithms.h
./Costmap.h
@@ -39,8 +44,12 @@ armarx_add_library(algorithms
# smoothing
./smoothing/ChainApproximation.h
./smoothing/CircularPathSmoothing.h
+ ./math/ellipse.h
)
+#
+# Tests
+#
armarx_add_test(algorithms_spfa_test
TEST_FILES
@@ -50,3 +59,19 @@ armarx_add_test(algorithms_spfa_test
DEPENDENCIES_LEGACY
OpenCV
)
+
+
+armarx_add_test(algorithms_costmap_test
+ TEST_FILES
+ test/algorithms_costmap_test.cpp
+ DEPENDENCIES
+ armarx_navigation::algorithms
+)
+
+
+armarx_add_test(algorithms_math_ellipse_test
+ TEST_FILES
+ test/algorithms_math_ellipse_test.cpp
+ DEPENDENCIES
+ armarx_navigation::algorithms
+)
diff --git a/source/armarx/navigation/algorithms/math/ellipse.cpp b/source/armarx/navigation/algorithms/math/ellipse.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..9c13587051c5679b4ae9c2e4935e52262dae92da
--- /dev/null
+++ b/source/armarx/navigation/algorithms/math/ellipse.cpp
@@ -0,0 +1,137 @@
+/**
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX 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 General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "ellipse.h"
+
+#include <cmath>
+
+#include <ceres/autodiff_cost_function.h>
+#include <ceres/ceres.h>
+#include <ceres/types.h>
+
+#include <Eigen/Core>
+
+#include <SimoxUtility/math/periodic/periodic_clamp.h>
+
+namespace armarx::navigation::algorithms::math
+{
+
+ template <typename T>
+ T
+ distance(const Ellipse& a, const Ellipse& b, const T thetaA, const T thetaB)
+ {
+ const auto pA = a.getPosition(thetaA);
+ const auto pB = b.getPosition(thetaB);
+
+ const auto dX = pB.at(0) - pA.at(0);
+ const auto dY = pB.at(1) - pB.at(1);
+
+ return ceres::sqrt(dX * dX + dY * dY);
+ }
+
+ class DistanceBetweenEllipsesCostFunctor
+ {
+ public:
+ DistanceBetweenEllipsesCostFunctor(const Ellipse& ellipseA, const Ellipse& ellipseB) :
+ ellipseA(ellipseA), ellipseB(ellipseB)
+ {
+ }
+
+ template <typename T>
+ bool
+ operator()(const T* const x, const T* const y, T* e) const
+ {
+ const auto& thetaA = x[0];
+ const auto& thetaB = y[0];
+
+ e[0] = distance(ellipseA, ellipseB, thetaA, thetaB);
+ return true;
+ }
+
+ private:
+ const Ellipse ellipseA;
+ const Ellipse ellipseB;
+ };
+
+
+ std::array<double, 2>
+ guessInitialThetas(const Ellipse& ellipseA, const Ellipse& ellipseB)
+ {
+ const auto toEigen = [](const std::array<double, 2>& position) -> Eigen::Vector2f {
+ return {position.at(0), position.at(1)};
+ };
+
+ // vector from A's center to B's center ...
+ const Eigen::Vector2f a_V_b = toEigen(ellipseB.position) - toEigen(ellipseA.position);
+ // and its orientation
+ const auto yaw = std::atan2(a_V_b.y(), a_V_b.x());
+
+ // both initial thetas represent the intersection between the ellipse and the line connecting the centers
+ const double thetaA = yaw - ellipseA.yaw;
+ const double thetaB = yaw + M_PI - ellipseB.yaw;
+
+ return {simox::math::periodic_clamp(thetaA, -M_PI, M_PI),
+ simox::math::periodic_clamp(thetaB, -M_PI, M_PI)};
+ }
+
+
+ double
+ distance(const Ellipse& ellipseA, const Ellipse& ellipseB, const bool verbose)
+ {
+ ceres::Problem problem;
+
+ ceres::CostFunction* costFunction =
+ new ceres::AutoDiffCostFunction<DistanceBetweenEllipsesCostFunctor, 1, 1, 1>(
+ new DistanceBetweenEllipsesCostFunctor(ellipseA, ellipseB));
+
+ std::array<double, 2> thetas = guessInitialThetas(ellipseA, ellipseB);
+ if (verbose)
+ {
+ std::cout << "Initial thetas: " << thetas.at(0) << ", " << thetas.at(1);
+ }
+
+ problem.AddResidualBlock(costFunction, nullptr, {&thetas.at(0), &thetas.at(1)});
+
+ // Run the solver!
+ ceres::Solver::Options options;
+ options.linear_solver_type = ceres::DENSE_QR;
+ options.minimizer_progress_to_stdout = verbose;
+ options.max_num_iterations = 100;
+ options.function_tolerance = 0.001;
+
+ ceres::Solver::Summary summary;
+ Solve(options, &problem, &summary);
+
+
+ thetas.at(0) = simox::math::periodic_clamp(thetas.at(0), -M_PI, M_PI);
+ thetas.at(1) = simox::math::periodic_clamp(thetas.at(1), -M_PI, M_PI);
+
+ if (verbose)
+ {
+ std::cout << summary.FullReport() << "\n";
+ std::cout << "Optimal parameters: " << thetas.at(0) << ", " << thetas.at(1)
+ << std::endl;
+ }
+
+ return distance(ellipseA, ellipseB, thetas.at(0), thetas.at(1));
+ }
+
+} // namespace armarx::navigation::algorithms::math
diff --git a/source/armarx/navigation/algorithms/math/ellipse.h b/source/armarx/navigation/algorithms/math/ellipse.h
new file mode 100644
index 0000000000000000000000000000000000000000..51e1c3ccc6ea0fdc1a1fc5b34c757cd2c3cf6689
--- /dev/null
+++ b/source/armarx/navigation/algorithms/math/ellipse.h
@@ -0,0 +1,66 @@
+#pragma once
+
+#include <array>
+
+#include <ceres/jet.h>
+
+namespace armarx::navigation::algorithms::math
+{
+
+ class Ellipse
+ {
+
+ public:
+
+ Ellipse(const std::array<double, 2>& position,
+ const double yaw,
+ const double a,
+ const double b) :
+ position(position),
+ ceta{std::cos(yaw), std::sin(yaw)},
+ eta{-std::sin(yaw), std::cos(yaw)},
+ yaw(yaw),
+ a(a),
+ b(b)
+ {
+ }
+
+ template <typename T>
+ std::array<T, 2>
+ getPosition(const T theta) const
+ {
+ const auto cosTheta = ceres::cos(theta);
+ const auto sinTheta = ceres::sin(theta);
+
+ std::array<T, 2> pos;
+ pos.at(0) = position.at(0) + a * cosTheta * ceta.at(0) + b * sinTheta * eta.at(0);
+ pos.at(1) = position.at(1) + a * cosTheta * ceta.at(1) + b * sinTheta * eta.at(1);
+
+ return pos;
+ }
+
+ // center pose of the ellipse defined via
+ // ... its position
+ const std::array<double, 2> position;
+ // ... and two orthogonal vectors
+ const std::array<double, 2> ceta;
+ const std::array<double, 2> eta;
+ // ... and for simple access
+ const double yaw;
+
+ // these axis lengths correspond to ceta (-> a) and eta (-> b)
+ const double a;
+ const double b;
+ };
+
+
+ /**
+ * @brief numerically approximates the distance between two ellipses
+ *
+ * @param ellipseA, ellipseB: the ellipses
+ * @param verbose Will print optimization related things to std::cout if enabled
+ * @return double the distance
+ */
+ double distance(const Ellipse& ellipseA, const Ellipse& ellipseB, bool verbose = false);
+
+} // namespace armarx::navigation::algorithms::math
diff --git a/source/armarx/navigation/algorithms/test/algorithms_math_ellipse_test.cpp b/source/armarx/navigation/algorithms/test/algorithms_math_ellipse_test.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..af7d017079658e1101ed003b447e6a31a997dd2d
--- /dev/null
+++ b/source/armarx/navigation/algorithms/test/algorithms_math_ellipse_test.cpp
@@ -0,0 +1,45 @@
+/**
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX 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 General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package Navigation::ArmarXObjects::algorithms
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+
+#define BOOST_TEST_MODULE Navigation::ArmarXLibraries::algorithms
+
+#define ARMARX_BOOST_TEST
+
+#include <armarx/navigation/Test.h>
+#include <armarx/navigation/algorithms/math/ellipse.h>
+
+
+BOOST_AUTO_TEST_CASE(testOptimizeDistance)
+{
+ // e1 = Ellipse(position=np.asarray([0, 0]), orientation=0.3, a=0.1, b=0.3)
+ armarx::navigation::algorithms::math::Ellipse ellipseA({0, 0}, 0.3, 0.1, 0.3);
+
+ // e2 = Ellipse(position=np.asarray([1, 0]), orientation=-0.1, a=0.1, b=0.3)
+ armarx::navigation::algorithms::math::Ellipse ellipseB({1, 0}, 0.1, 0.1, 0.3);
+
+ const double distance = armarx::navigation::algorithms::math::distance(ellipseA, ellipseB, true);
+ std::cout << "Distance is " << distance << std::endl;
+
+ BOOST_CHECK(distance > 0);
+ BOOST_CHECK(std::abs(distance - 0.766) < 0.01);
+}