add library for 2D polyfitting

this uses the [ceres solver] to implement polyfitting using least
squares for a given set of 2D points to find a two-dimensional
polynomial ($y = a0 + a1 * x + a2 * x^2$).

this can then be used in a next step to fit the line through the banana
contour (will be done in a follow-up commit).

note that the `polyfit` header-only library is not specific to the
current project and could easily be factored out (it has been built in a
way to not rely on the rest of the project). it could potentially also
be made more abstract to support generic polyfitting for n-dimensional
polynomials and/or n-dimensional data points.

the API accepts a `range` to accept any form of iterable container. this
requires at least C++20, most likely C++23 (where ranges have been
updated significantly). it has only been tested against C++23. see the
[`std::ranges` documentation] for further details.

note that this is using ceres solver v2.1.0 while v2.2.0 has already
been released. the reason for this is that v2.1.0 is the latest
available version in vcpkg, see microsoft/vcpkg#34483.

to better show what is happening a jupyter notebook is provided,
visualising the solution approach using python. due to this `.gitignore`
had to be updated as jupyterlab stores additional cache files.

[ceres solver]: http://ceres-solver.org/
[`std::ranges` documentation]: https://en.cppreference.com/w/cpp/ranges

Author: rursprung

.gitignore

@@ -1,2 +1,3 @@
-.idea
-cmake-build*
+.idea/
+cmake-build*/
+.ipynb_checkpoints/

docs/banana-center-line-fitting.ipynb

@@ -0,0 +1,85 @@
+/**\file
+ * \brief Header-only library which provides a simple API to fit a 2-dimensional polynomial (i.e. 3 coefficients) to a set of two-dimensional points (x, y coordinates).
+ */
+
+#ifndef BANANA_PROJECT_POLYNOMIAL2DFIT_HPP
+#define BANANA_PROJECT_POLYNOMIAL2DFIT_HPP
+
+#include <expected>
+#include <tuple>
+#include <utility>
+#include <vector>
+
+// TODO: we currently leak the ceres dependency to everyone which includes this header-only library.
+//  when changing this we'll also need to change the return type of `Fit2DPolynomial` which currently returns
+//  the ceres termination type in case of errors.
+#include <ceres/ceres.h>
+
+/**
+ * \brief Header-only library which provides a simple API to fit a 2-dimensional polynomial (i.e. 3 coefficients) to a set of two-dimensional points (x, y coordinates).
+ */
+namespace polyfit {
+
+    /** Internal helpers, do not use from the outside! */
+    namespace internal {
+        /**
+         * Calculates the residual (= error = distance) of the estimate for a specified point.
+         */
+        struct Polynomial2DResidual {
+        public:
+            Polynomial2DResidual(double const x, double const y) : x_(x), y_(y) {}
+
+            template<typename T>
+            bool operator()(const T *const a0, const T *const a1, const T *const a2, T *residual) const {
+                auto const y_estimate = a0[0] + a1[0] * x_ + a2[0] * x_ * x_;
+                residual[0] = y_ - y_estimate;
+                return true;
+            }
+
+        private:
+            double const x_;
+            double const y_;
+        };
+    }
+
+    /**
+     * Calculate the coefficients for a two-dimensional polynomial which fits the points as well as possible.
+     *
+     * @tparam R the underlying container supporting ranges. must contain std::tuple<double, double>
+     * @tparam print_report if enabled the underlying solver will print a detailed report to STDOUT. helpful for debugging.
+     * @param points the set of points (x & y coordinates) for which the polynomial should be fitted.
+     * @return either the set of coefficients or the termination type indicating why the fitting failed.
+     */
+    template<std::ranges::range R, bool print_report = false>
+    [[nodiscard]]
+    auto Fit2DPolynomial(
+            R&& points) -> std::expected<std::tuple<double, double, double>, ceres::TerminationType> {
+        double a0 = 1, a1 = 1, a2 = 1;
+
+        ceres::Problem problem;
+        for (auto const& point: points) {
+            ceres::CostFunction *cost_function =
+                    new ceres::AutoDiffCostFunction<internal::Polynomial2DResidual, 1, 1, 1, 1>(
+                            new internal::Polynomial2DResidual(point.first, point.second));
+            problem.AddResidualBlock(cost_function, nullptr, &a0, &a1, &a2);
+        }
+
+        ceres::Solver::Options options{
+                .logging_type = print_report ? ceres::PER_MINIMIZER_ITERATION : ceres::SILENT,
+                .minimizer_progress_to_stdout = print_report,
+        };
+        ceres::Solver::Summary summary;
+        ceres::Solve(options, &problem, &summary);
+        if (print_report) {
+            std::cout << summary.FullReport() << std::endl;
+        }
+        if (summary.termination_type == ceres::CONVERGENCE) {
+            return {{a0, a1, a2}};
+        } else {
+            return std::unexpected{summary.termination_type};
+        }
+    };
+
+}
+
+#endif //BANANA_PROJECT_POLYNOMIAL2DFIT_HPP

include/polyfit/Polynomial2DFit.hpp

@@ -1,6 +1,7 @@
 set(BANANA_HEADER_LIST "${PROJECT_SOURCE_DIR}/include/banana-lib/lib.hpp")
 
 find_package(OpenCV CONFIG REQUIRED)
+find_package(Ceres CONFIG REQUIRED)
 
 add_library(banana-lib lib.cpp ${BANANA_HEADER_LIST})
 
@@ -12,4 +13,5 @@ target_include_directories(
 
 target_link_libraries(banana-lib
         PUBLIC ${OpenCV_LIBS}
+        PRIVATE Ceres::ceres
 )

src/CMakeLists.txt

@@ -1,6 +1,13 @@
 find_package(GTest CONFIG REQUIRED)
 include(GoogleTest)
 
+find_package(Ceres CONFIG REQUIRED)
+
+add_executable(polyfit-test polyfit-test.cpp)
+target_include_directories(polyfit-test PRIVATE "${PROJECT_SOURCE_DIR}/include")
+target_link_libraries(polyfit-test Ceres::ceres GTest::gtest_main)
+gtest_discover_tests(polyfit-test)
+
 add_executable(banana-lib-test banana-lib-test.cpp)
 target_link_libraries(banana-lib-test banana-lib GTest::gtest_main)
 gtest_discover_tests(banana-lib-test)

test/CMakeLists.txt

@@ -0,0 +1,47 @@
+#include <gtest/gtest.h>
+
+#include <polyfit/Polynomial2DFit.hpp>
+
+#define ASSERT_COEFFS_NEAR(coeff_expected_0, coeff_expected_1, coeff_expected_2, coeffs) \
+    do {                                                                                 \
+        ASSERT_NEAR(coeff_expected_0, std::get<0>(coeffs), 1e-6);                        \
+        ASSERT_NEAR(coeff_expected_1, std::get<1>(coeffs), 1e-6);                        \
+        ASSERT_NEAR(coeff_expected_2, std::get<2>(coeffs), 1e-6);                        \
+    } while (false)
+
+/** y = 1 + x */
+TEST(Polynomial2DFitTestSuite, FitSimpleLine) {
+    std::vector<std::pair<double, double>> points = {
+            {0,1},
+            {1,2},
+            {2,3},
+            {3,4},
+    };
+    auto const result = polyfit::Fit2DPolynomial(points);
+    ASSERT_TRUE(result);
+    ASSERT_COEFFS_NEAR(1, 1, 0, *result);
+}
+
+/** y = -1 + x^2 */
+TEST(Polynomial2DFitTestSuite, FitSimpleCurve) {
+    std::vector<std::pair<double, double>> points = {
+            {-1,0},
+            {0,-1},
+            {1,0},
+    };
+    auto const result = polyfit::Fit2DPolynomial(points);
+    ASSERT_TRUE(result);
+    ASSERT_COEFFS_NEAR(-1, 0, 1, *result);
+}
+
+/** y = -1 + 3*x + 2*x^2 */
+TEST(Polynomial2DFitTestSuite, FitSimpleCurve2) {
+    std::vector<std::pair<double, double>> points = {
+            {-1,-2},
+            {0,-1},
+            {1,4},
+    };
+    auto const result = polyfit::Fit2DPolynomial(points);
+    ASSERT_TRUE(result);
+    ASSERT_COEFFS_NEAR(-1, 3, 2, *result);
+}

test/polyfit-test.cpp

@@ -8,5 +8,8 @@
   }, {
     "name" : "opencv",
     "version>=" : "4.8.0#1"
+  }, {
+    "name" : "ceres",
+    "version>=" : "2.1.0#5"
   } ]
 }