From 62ebff5a9ab0daa25c24c20195231fe0ed5930e0 Mon Sep 17 00:00:00 2001 From: redto0 Date: Fri, 30 May 2025 15:55:57 -0400 Subject: [PATCH 1/4] added ccma files Signed-off-by: redto0 --- include/polynomial_planner/backend.hpp | 21 +- include/polynomial_planner/ccma.hpp | 441 +++++++++++++++++++++++++ src/backend.cpp | 8 + 3 files changed, 462 insertions(+), 8 deletions(-) create mode 100644 include/polynomial_planner/ccma.hpp diff --git a/include/polynomial_planner/backend.hpp b/include/polynomial_planner/backend.hpp index 23fad5d..1e8b3af 100644 --- a/include/polynomial_planner/backend.hpp +++ b/include/polynomial_planner/backend.hpp @@ -8,6 +8,7 @@ #include "image_geometry/pinhole_camera_model.h" #include "nav_msgs/msg/path.hpp" +#include "ccma.hpp" class Polynomial { public: @@ -42,12 +43,16 @@ class Polynomial { }; namespace backend { -std::vector cameraPixelToGroundPos(std::vector& pixels, - image_geometry::PinholeCameraModel& rgb_info_sub); -nav_msgs::msg::Path cameraPixelToGroundPath(std::vector& pixels, - const image_geometry::PinholeCameraModel& rgb_info_sub, float camera_height, - std::string frame_id); -std::optional create_path(std::vector& left_contours, - std::vector& right_contours, - image_geometry::PinholeCameraModel& camera_info, std::string frame_id); + std::vector cameraPixelToGroundPos(std::vector& pixels, + image_geometry::PinholeCameraModel& rgb_info_sub); + nav_msgs::msg::Path cameraPixelToGroundPath(std::vector& pixels, + const image_geometry::PinholeCameraModel& rgb_info_sub, float camera_height, + std::string frame_id); + std::optional create_path(std::vector& left_contours, + std::vector& right_contours, + image_geometry::PinholeCameraModel& camera_info, std::string frame_id); + + ccma::CCMA ccma_obj; + std::vector ccma_points(const std::vector& points); + } // namespace backend diff --git a/include/polynomial_planner/ccma.hpp b/include/polynomial_planner/ccma.hpp new file mode 100644 index 0000000..642fd48 --- /dev/null +++ b/include/polynomial_planner/ccma.hpp @@ -0,0 +1,441 @@ +/*********************************************************************************************** + * CURVATURE-CORRECTED MOVING-AVERAGE ( C C M A ) + * + * A **model-free** path-smoothing algorithm for 2-D / 3-D data that avoids the typical + * inward-bending artefact of a plain moving average by *explicit* curvature compensation. + * + * This header is the literal C++17 translation of the official Python reference + * implementation (Steinecker & Wuensche, IV 2023) – **but** every relevant line is + * annotated with two kinds of comments: + * + * WHAT → explains *what* the statement does. + * WHY → explains *why* that design/implementation decision was taken. + * + * Build test-program (with Eigen in include-path): + * g++ -std=c++17 -O3 example.cpp -I /usr/include/eigen3 + * + **********************************************************************************************/ +#ifndef CCMA_HPP +#define CCMA_HPP + +/*───────────────────────────────────────────────────────────────────────────────* + * WHAT: Standard library & Eigen headers. * + * WHY : • Eigen supplies vector/matrix math without external libs. * + * • : math functions (sin, cos, log, etc.). * + * • : std::accumulate for kernel normalisation. * + *───────────────────────────────────────────────────────────────────────────────*/ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace ccma { +/*=============================================================================* + * Helper utilities * + *============================================================================*/ +namespace detail { +/* WHAT: √2 constant – used in the error-function based normal CDF. */ +constexpr double kSQRT2 = 1.41421356237309504880168872420969808; + +/*---------------------------------------------------------------------------*/ +/* Φ(x) – Standard-normal cumulative distribution function. */ +/* WHAT: wrapper around std::erfc so we avoid heavyweight. */ +/* WHY : fastest portable implementation in . */ +inline double normal_cdf(double x) noexcept { return 0.5 * std::erfc(-x / kSQRT2); } + +/*---------------------------------------------------------------------------*/ +/* unit(v) – returns v/‖v‖ (or v if ‖v‖==0). * + * WHY: frequent normalisations; inline avoids function-call overhead. */ +inline Eigen::Vector3d unit(const Eigen::Vector3d& v) noexcept { + double n = v.norm(); + return n == 0.0 ? v : v / n; +} + +/*---------------------------------------------------------------------------*/ +/* Manual *valid-length* 1-D convolution (kernel shorter than data). * + * WHAT: reproduces numpy.convolve(..., mode="valid") exactly. * + * WHY : Eigen lacks an easy 1-D convolution; writing loop is trivial & * + * avoids another dependency. */ +inline std::vector convolve_valid(const std::vector& data, const std::vector& kernel) { + std::size_t n = data.size(), k = kernel.size(); + if (n < k) throw std::runtime_error("convolve_valid: kernel longer than data"); + + std::vector res(n - k + 1, 0.0); + for (std::size_t i = 0; i <= n - k; ++i) + for (std::size_t j = 0; j < k; ++j) res[i] += data[i + j] * kernel[j]; + return res; +} + +} // namespace detail + +/*=============================================================================* + * CCMA class * + *============================================================================*/ +class CCMA { +public: + /*-------------------------------------------------------------------------*/ + /* Constructor – stores hyper-parameters *and* pre-computes kernels. * + * WHY: weights are independent of data → compute once, reuse often. */ + CCMA(int w_ma = 5, /* half-window for moving avg */ + int w_cc = 3, /* half-window for curvature */ + std::string distrib = "pascal", /* default kernel family */ + std::string distrib_ma = "", /* override for MA (opt) */ + std::string distrib_cc = "", /* override for CC (opt) */ + double rho_ma = 0.95, /* truncation area (normal) */ + double rho_cc = 0.95) + : w_ma_(w_ma), + w_cc_(w_cc), + w_ccma_(w_ma + w_cc + 1) /* larger composite window */ + , + distrib_ma_(distrib_ma.empty() ? distrib : distrib_ma), + distrib_cc_(distrib_cc.empty() ? distrib : distrib_cc), + rho_ma_(rho_ma), + rho_cc_(rho_cc) { + /* WHAT: generate lookup tables of kernels for all window sizes ≤ w. */ + weights_ma_ = generate_weights(w_ma_, distrib_ma_, rho_ma_); + weights_cc_ = generate_weights(w_cc_, distrib_cc_, rho_cc_); + } + + /*-------------------------------------------------------------------------*/ + /* Public API: filter() */ + /* WHAT: accepts raw points, returns smoothed points. * + * WHY : hides boundary handling + 2-D/3-D up-conversion details. */ + std::vector filter(const std::vector& input, const std::string& mode = "padding", + bool cc_mode = true) const { + /*———— Sanity checks ————*/ + if (mode != "none" && mode != "padding" && mode != "wrapping" && mode != "fill_boundary") + throw std::invalid_argument("CCMA::filter: invalid mode"); + + if (input.size() < 3) throw std::runtime_error("CCMA::filter: at least 3 points required"); + + /* copy because we may extend/modify pts for boundary treatment */ + Eigen::MatrixXd pts(3, input.size()); + + for (int i = 0; i < input.size(); i++) { + pts(0,i) = input.at(i).x; + pts(1,i) = input.at(i).y; + pts(2,i) = 0; + } + + /*======================== 1. Boundary pre-processing =================*/ + // if (mode == "padding") { + // /* WHAT: replicate first/last sample to keep length unchanged. * + // * WHY : simplest length-preserving strategy when data are *open*.*/ + // int n_pad = cc_mode ? w_ccma_ : w_ma_; + // pts.conservativeResize(pts.rows() + 2 * n_pad, pts.cols()); + // pts.block(0, 0, n_pad, pts.cols()) = pts.row(n_pad).replicate(n_pad, 1); + // pts.block(pts.rows() - n_pad, 0, n_pad, pts.cols()) = pts.row(pts.rows() - n_pad - 1).replicate(n_pad, 1); + // } else if (mode == "wrapping") { + // /* WHAT: treat data as cyclic. * + // * WHY : useful for closed curves (e.g. circle, track loop). */ + // int n_pad = cc_mode ? w_ccma_ : w_ma_; + // Eigen::MatrixXd tmp(pts.rows() + 2 * n_pad, pts.cols()); + // tmp.block(0, 0, n_pad, pts.cols()) = pts.bottomRows(n_pad); + // tmp.block(n_pad, 0, pts.rows(), pts.cols()) = pts; + // tmp.block(n_pad + pts.rows(), 0, n_pad, pts.cols()) = pts.topRows(n_pad); + // pts.swap(tmp); + // } + + /* ensure enough points remain after trimming (for “none” mode) */ + if (pts.cols() < (cc_mode ? w_ccma_ * 2 + 1 : w_ma_ * 2 + 1)) + throw std::runtime_error("CCMA::filter: not enough points for given widths"); + + /*======================== 2. 2-D → 3-D up-conversion =================*/ // should never run + // bool is2d = (pts.cols() == 2); + // if (is2d) { + // /* WHY: core math implemented in 3-D; simply append z=0 column. */ + // Eigen::MatrixXd tmp(pts.rows(), 3); + // tmp.block(0, 0, pts.rows(), 2) = pts; + // tmp.col(2).setZero(); + // pts.swap(tmp); + // } + + /*======================== 3. Filter core =============================*/ + Eigen::MatrixXd out; + + if (mode != "fill_boundary") { + /* full-size kernel everywhere */ + out = _filter(pts, w_ma_, w_cc_, cc_mode); + } else { + /* cascading smaller kernels near edges */ + out = fill_boundary_mode(pts, cc_mode); + } + + std::vector ret; + for (int i = 0; i < out.cols(); i++) { + cv::Point2d toPush(out(0,i), out(1,i)); + ret.push_back(toPush); + } + + /*======================== 4. Return shape as input ===================*/ + return ret; + } + + /*============================================================================* + * Internals * + *============================================================================*/ +private: + /*-------------------------------------------------------------------------*/ + /* generate_weights() – produces *all* kernels up to window size w. * + * WHY: pre-computing dramatically speeds repeated filtering. */ + static std::vector> generate_weights(int w, const std::string& distrib, double crit_z) { + std::vector> weight_list; + weight_list.reserve(w + 1); + + /*================ Normal (truncated) kernel ==========================*/ + if (distrib == "normal") { + /* WHAT: boundaries that enclose ‘rho’ area under N(0,1). */ + crit_z = std::fabs(crit_z); + double x_start = -crit_z; + double x_end = crit_z; + + for (int wi = 0; wi <= w; ++wi) { + int k = 2 * wi + 1; /* kernel length*/ + std::vector weights(k, 0.0); + + /* integrate PDF over slices → exactly matches Python impl. */ + std::vector x(k + 1); + for (int i = 0; i <= k; ++i) x[i] = x_start + (x_end - x_start) * static_cast(i) / k; + + for (int i = 0; i < k; ++i) weights[i] = detail::normal_cdf(x[i + 1]) - detail::normal_cdf(x[i]); + + /* renormalise to sum≈1 (truncation removed mass ‘1-rho’) */ + for (double& v : weights) v /= (1 - detail::normal_cdf(crit_z) * 2); + weight_list.emplace_back(std::move(weights)); + } + } + /*================ Uniform kernel ====================================*/ + else if (distrib == "uniform") { + for (int wi = 0; wi <= w; ++wi) { + int k = 2 * wi + 1; + weight_list.emplace_back(k, 1.0 / static_cast(k)); + } + } + /*================ Pascal (binomial) kernel ==========================*/ + else if (distrib == "pascal") { + auto pascal_row = [](int n) { + /* WHAT: computes row ‘n’ of Pascal’s triangle iteratively. */ + std::vector row{1.0}; + for (int r = 1; r <= n; ++r) row.push_back(row.back() * static_cast(n - r + 1) / r); + return row; + }; + + for (int wi = 0; wi <= w; ++wi) { + auto row = pascal_row(2 * wi); /* even row length */ + double sum = std::accumulate(row.begin(), row.end(), 0.0); + for (double& v : row) v /= sum; /* normalise */ + weight_list.emplace_back(std::move(row)); + } + } + /*================ Hanning (raised-cosine) kernel ====================*/ + else if (distrib == "hanning") { + for (int wi = 0; wi <= w; ++wi) { + int k = 2 * wi + 1; + int window_size = k + 2; /* match Python impl*/ + std::vector kernel; + kernel.reserve(k); + + for (int n = 1; n <= k; ++n) { + double v = 0.5 * (1.0 - std::cos(2.0 * M_PI * n / (window_size - 1))); + kernel.push_back(v); + } + double sum = std::accumulate(kernel.begin(), kernel.end(), 0.0); + for (double& v : kernel) v /= sum; + weight_list.emplace_back(std::move(kernel)); + } + } else { + throw std::invalid_argument("unknown kernel distribution '" + distrib + "'"); + } + return weight_list; + } + + /*-------------------------------------------------------------------------*/ + /* ma_points() – convolution along each axis with chosen MA kernel. * + * WHAT: returns *valid* samples only → easy index bookkeeping later. * + * WHY : explicit loops avoid Eigen’s heavy convolution workaround. */ + inline static Eigen::MatrixXd ma_points(const Eigen::MatrixXd& points, const std::vector& weights) { + int k = (weights.size()); + int n_out = points.rows() - k ; + Eigen::MatrixXd out(n_out, 3); + + Eigen::VectorXd w = Eigen::Map(weights.data(), k); + + for (int i = 0; i < n_out; ++i) { // IF CRASHES PLEASE LOOK HERE + out(0,i) = points.row(0).segment(i, k).dot(w); + out(1,i) = points.row(1).segment(i, k).dot(w); + out(2,i) = points.row(2).segment(i, k).dot(w); + } + return out; + } + + /*-------------------------------------------------------------------------*/ + /* curvature_vectors() – approximates signed curvature normal for each pt.*/ + /* WHAT: uses circumcircle of triplets (i-1,i,i+1). * + * WHY : local curvature needed to correct systematic MA shrinkage. */ + inline static Eigen::MatrixXd curvature_vectors(const Eigen::MatrixXd& pts) { + Eigen::MatrixXd cv = Eigen::MatrixXd::Zero(pts.rows(), 3); + + for (int i = 1; i < pts.rows() - 1; ++i) { + Eigen::Vector3d v1 = pts.row(i) - pts.row(i - 1); + Eigen::Vector3d v2 = pts.row(i + 1) - pts.row(i); + Eigen::Vector3d cross = v1.cross(v2); + double cross_norm = cross.norm(); + + double curvature = 0.0; + if (cross_norm != 0.0) { + double radius = v1.norm() * v2.norm() * (pts.row(i + 1) - pts.row(i - 1)).norm() / (2.0 * cross_norm); + curvature = 1.0 / radius; + } + cv.row(i) = curvature * detail::unit(cross); + } + return cv; + } + + /*-------------------------------------------------------------------------*/ + /* alphas() – angle between consecutive chord vectors under constant curv.*/ + inline static std::vector alphas(const Eigen::MatrixXd& pts, const std::vector& curv) { + int n = pts.rows(); + std::vector a(n, 0.0); + + for (int i = 1; i < n - 1; ++i) { + if (curv[i] != 0.0) { + double R = 1.0 / curv[i]; + double d = (pts.row(i + 1) - pts.row(i - 1)).norm(); + a[i] = std::asin(0.5 * d / R); + } + } + return a; + } + + /*-------------------------------------------------------------------------*/ + /* normalized_ma_radii() – Eq. (7) in the paper (figure-2 intuition). * + * WHY: converts angles to *radius shrink-factor* for curvature fix. */ + inline static std::vector normalized_ma_radii(const std::vector& alphas, int w_ma, + const std::vector& weights) { + int n = static_cast(alphas.size()); + std::vector radii(n, 0.0); + + for (int i = 1; i < n - 1; ++i) { + double radius = weights[w_ma]; /* central weight (k=0) */ + for (int k = 1; k <= w_ma; ++k) radius += 2.0 * std::cos(alphas[i] * k) * weights[w_ma + k]; + radii[i] = std::max(0.35, radius); /* lower-bound to avoid overshoot */ + } + return radii; + } + + /*-------------------------------------------------------------------------*/ + /* _filter() – workhorse for *one* window size (possibly with CC). */ + Eigen::MatrixXd _filter(const Eigen::MatrixXd& pts, int w_ma, int w_cc, bool cc_mode) const { + int w_ccma = w_ma + w_cc + 1; + + /*——- Step 1: plain moving-average over pts ————————————————*/ + Eigen::MatrixXd pts_ma = ma_points(pts, weights_ma_[w_ma]); + + if (!cc_mode) return pts_ma; /* user disabled CC */ + + /*——- Step 2: curvature + correction vectors ———————————————*/ + Eigen::MatrixXd cv = curvature_vectors(pts_ma); + std::vector curv(cv.rows()); + for (int i = 0; i < cv.rows(); ++i) curv[i] = cv.row(i).norm(); + + std::vector a = alphas(pts_ma, curv); + std::vector radii_ma = normalized_ma_radii(a, w_ma, weights_ma_[w_ma]); + + /*——- Step 3: reconstruct curvature-corrected points ————————*/ + int n_out = pts.rows() - 2 * w_ccma; + Eigen::MatrixXd out(n_out, 3); + + for (int i = 0; i < n_out; ++i) { + /* tangent vector (for cross-product reconstruction) */ + Eigen::Vector3d unit_tangent = detail::unit(pts_ma.row(w_cc + i + 2) - pts_ma.row(w_cc + i)); + + Eigen::Vector3d shift = Eigen::Vector3d::Zero(); + + for (int j = 0; j < 2 * w_cc + 1; ++j) { + int idx = i + j + 1; /* corresponding MA index */ + if (curv[idx] == 0.0) continue; /* straight line → no shift */ + + Eigen::Vector3d u_vec = detail::unit(cv.row(idx)); + double weight = weights_cc_[w_cc][j]; + double shift_mag = (1.0 / curv[idx]) * (1.0 / radii_ma[idx] - 1.0); + shift += u_vec * weight * shift_mag; + } + /* reconstruct by rotating shift into tangent plane */ + out.col(i) = pts_ma.col(i + w_cc + 1) + unit_tangent.cross(shift); + } + return out; + } + + /*-------------------------------------------------------------------------*/ + /* fill_boundary_mode() – gradually shrinking window near ends. * + * WHY: keeps *length* unchanged while avoiding heavy extrapolation. */ + Eigen::MatrixXd fill_boundary_mode(const Eigen::MatrixXd& pts, bool cc_active) const { + int dim = 3; + Eigen::MatrixXd out = Eigen::MatrixXd::Zero(pts.rows(), dim); + + if (cc_active) /* CCMA with shrinking windows */ + { + /* Build descending sequence of (w_ma,w_cc) pairs, largest first. */ + std::vector> widths; + int w_ma_cur = w_ma_, w_cc_cur = w_cc_; + while (w_ma_cur > 0 || w_cc_cur > 0) { + (w_cc_cur >= w_ma_cur) ? --w_cc_cur : --w_ma_cur; + widths.emplace_back(w_ma_cur, w_cc_cur); + } + std::reverse(widths.begin(), widths.end()); + + /* Unchanged first/last original points */ + out.row(0) = pts.row(0); + out.row(pts.rows() - 1) = pts.row(pts.rows() - 1); + + /* Fully filtered *interior* */ + out.block(w_ccma_, 0, pts.rows() - 2 * w_ccma_, dim) = _filter(pts, w_ma_, w_cc_, true); + + /* Ascending/descending partial windows */ + for (std::size_t w_idx = 0; w_idx < widths.size(); ++w_idx) { + int wma = widths[w_idx].first; + int wcc = widths[w_idx].second; + int wccma = wma + wcc + 1; + + /* Design choice: if w_ma==0 use MA=1 but no curvature corr. */ + bool use_ma1 = (wma == 0 && w_ma_ != 0); + + /* Start side */ + out.row(w_idx + 1) = + _filter(pts.topRows(w_idx + 1 + wccma + 1), use_ma1 ? 1 : wma, wcc, use_ma1 ? false : true).row(0); + + /* End side */ + out.row(out.rows() - w_idx - 2) = + _filter(pts.bottomRows(w_idx + 1 + wccma + 1), use_ma1 ? 1 : wma, wcc, use_ma1 ? false : true) + .row(0); + } + } else /* MA-only variant with width-cascade */ + { + out.block(w_ma_, 0, pts.rows() - 2 * w_ma_, dim) = _filter(pts, w_ma_, 0, false); + + for (int w = 0; w < w_ma_; ++w) { + out.row(w) = _filter(pts.topRows(2 * w + 1), w, 0, false).row(0); + + out.row(out.rows() - w - 1) = _filter(pts.bottomRows(2 * w + 1), w, 0, false).row(0); + } + } + return out; + } + + /*============================================================================* + * Data members * + *============================================================================*/ + int w_ma_, w_cc_, w_ccma_; /* window params */ + std::string distrib_ma_, distrib_cc_; /* kernel families */ + double rho_ma_, rho_cc_; /* truncation area */ + std::vector> weights_ma_, weights_cc_; /* kernel LUTs */ +}; + +} // namespace ccma +#endif /* CCMA_HPP */ diff --git a/src/backend.cpp b/src/backend.cpp index bb287ad..5b61e2b 100644 --- a/src/backend.cpp +++ b/src/backend.cpp @@ -47,6 +47,8 @@ std::optional backend::create_path(std::vector cam_path.push_back(cv::Point2d(x, y)); } + cam_path = ccma_points(cam_path); + if (cam_path.empty()) { return std::nullopt; } else { @@ -165,4 +167,10 @@ nav_msgs::msg::Path backend::cameraPixelToGroundPath(std::vector& p rwpoints.header.frame_id = frame_id; return rwpoints; +} + +std::vector backend::ccma_points(const std::vector& ground_points) { + if (ground_points.size() > 3) + return ccma_obj.filter(ground_points, "none"); + return ground_points; } \ No newline at end of file From f2d96e58bf8cbaac3f2cdf62539632c7b94d0f13 Mon Sep 17 00:00:00 2001 From: redto0 Date: Mon, 2 Jun 2025 00:34:48 -0400 Subject: [PATCH 2/4] ccma working, currently very expensive to run but we will see if its worth it! Signed-off-by: redto0 --- include/polynomial_planner/CCMA.hpp | 132 ++++++++ include/polynomial_planner/backend.hpp | 26 +- include/polynomial_planner/ccma.hpp | 441 ------------------------- src/CCMA.cpp | 271 +++++++++++++++ src/backend.cpp | 19 +- 5 files changed, 427 insertions(+), 462 deletions(-) create mode 100644 include/polynomial_planner/CCMA.hpp delete mode 100644 include/polynomial_planner/ccma.hpp create mode 100644 src/CCMA.cpp diff --git a/include/polynomial_planner/CCMA.hpp b/include/polynomial_planner/CCMA.hpp new file mode 100644 index 0000000..a3d5ba8 --- /dev/null +++ b/include/polynomial_planner/CCMA.hpp @@ -0,0 +1,132 @@ + +#ifndef CCMA_HPP +#define CCMA_HPP + +#include +#include +#include +#include +#include + +namespace ccma { +namespace helpers { + +constexpr double kSQRT2 = 1.41421356237309504880168872420969807856967187537694; +inline double normal_cdf(double z) { return 0.5 * std::erfc(-z / kSQRT2); } + +inline Eigen::VectorXd unit(const Eigen::VectorXd& v) { + double n = v.norm(); + return n == 0 ? v : v / n; +} + +inline void printMatrixXd(const Eigen::MatrixXd& matrix) { + for (int row = 0; row < matrix.rows(); row++) { + for (int col = 0; col < matrix.cols(); col++) { + std::cout << matrix(row, col) << ", "; + } + std::cout << std::endl; + } +} +} // namespace helpers +class CCMA { +public: + inline CCMA(int w_ma = 5, int w_cc = 3, std::string distrib = "pascal", std::string distrib_ma = "", + std::string distrib_cc = "", double crit_z_ma = 2, double crit_z_cc = 2) + : w_ma_(w_ma), + w_cc_(w_cc), + w_ccma_(w_ma + w_cc + 1), + distrib_ma_(distrib_ma.empty() ? distrib : distrib_ma), // unholy initializer list + distrib_cc_(distrib_cc.empty() ? distrib : distrib_cc), + crit_z_ma_(fabs(crit_z_ma)), + crit_z_cc_(fabs(crit_z_cc)) { + weights_ma_ = generate_weights(w_ma_, distrib_ma_, crit_z_ma_); // generates the weights algorithmically + weights_cc_ = generate_weights(w_cc_, distrib_cc_, crit_z_cc_); + }; + + Eigen::MatrixXd filter(const Eigen::MatrixXd& input, const std::string& mode = "none", + bool cc_mode = true); // the only public funciton + + Eigen::MatrixXd points_to_MatrixXd(const std::vector& points); // lies, deception ^ + std::vector matrixXd_to_Points2d(const Eigen::MatrixXd& matrix); + +private: + /// + /// Generates every kernel (vector of weights) up to the given width. Should likely use w_ma/cc when grabbing list of weights. + /// GPT says it's faster on runtime instead of recalculating every run. Maybe we should only save the one we want since we aren't changing widths? + /// + /// : The width of the desired weight list + /// : The type of distribution. May be: normal uniform pascal AND NOT hanning i got bored + /// + /// : Desired z-val of the normal distribution. Only works if normal is picked, unused otherwise. + /// Vector of every kernel up to and including width, which will have 2(width) +-? 1 values, and will be stored at [width - 1]. + static std::vector> generate_weights(int width, const std::string& distribution, + double crit_z_val); + + /// + /// Moving average points. Convolution along all 3 axes with the provided weights. + /// + /// : The matrix of points. + /// : The vector of weights. + /// The convoluted matrix. This will be a differently-sized matrix! + Eigen::MatrixXd ma_points(const Eigen::MatrixXd& points, const std::vector& weights) const; + + /// + /// + /// Curvatures of the circle inscribed by each set of 3 points. Visual in comments + /// look at function for proper structuring this is so frustratingly awful to get to linebreak + /// + /// + /// + /// + static Eigen::MatrixXd curvature_vectors(const Eigen::MatrixXd& points); + + /* [ 0, 0, 0, ... 0 + 0, 0, 0, ... 0 + 0, k1, k2, ... 0 ]*/ + + /// + /// Angles between adjacent points for each point. Starts with the second point and ends with the second to last point. + /// + /// + /// , could probably just use a function call natively, not sure why it doesn't + /// angles between adjacent points + static std::vector alphas(const Eigen::MatrixXd& points, const std::vector& curvatures); + + /// + /// weights for the radii in a similar way that we do for moving average. slightly weirder though. + /// + /// angles between points + /// + /// + /// + static std::vector normalized_ma_radii(const std::vector& alphas, int w_ma, + const std::vector& weights); + + /// + /// one window-size filter. why not use this? + /// + /// + /// + /// + /// : whether we are using curvature correction + /// filtered matrix + Eigen::MatrixXd _filter(const Eigen::MatrixXd& points, int w_ma, int w_cc, bool cc_mode) const; + + /// + /// readjusts to regular size, with extrapolation reduction algorithms + /// + /// + /// + /// + // Eigen::MatrixXd fill_boundary_mode(const Eigen::MatrixXd& points, bool cc_active) const; + + // members + int w_ma_, w_cc_, w_ccma_; // width of the weights + std::string distrib_ma_, distrib_cc_; // type of distribution + double crit_z_ma_, + crit_z_cc_; // instead of using crit p vals, we use crit z vals to get around needing prob functions + std::vector> weights_ma_, weights_cc_; // a vector of vectors of doubles for weights! +}; +} // namespace ccma + +#endif diff --git a/include/polynomial_planner/backend.hpp b/include/polynomial_planner/backend.hpp index 1e8b3af..04b157c 100644 --- a/include/polynomial_planner/backend.hpp +++ b/include/polynomial_planner/backend.hpp @@ -6,9 +6,9 @@ #include #include +#include "CCMA.hpp" #include "image_geometry/pinhole_camera_model.h" #include "nav_msgs/msg/path.hpp" -#include "ccma.hpp" class Polynomial { public: @@ -43,16 +43,16 @@ class Polynomial { }; namespace backend { - std::vector cameraPixelToGroundPos(std::vector& pixels, - image_geometry::PinholeCameraModel& rgb_info_sub); - nav_msgs::msg::Path cameraPixelToGroundPath(std::vector& pixels, - const image_geometry::PinholeCameraModel& rgb_info_sub, float camera_height, - std::string frame_id); - std::optional create_path(std::vector& left_contours, - std::vector& right_contours, - image_geometry::PinholeCameraModel& camera_info, std::string frame_id); - - ccma::CCMA ccma_obj; - std::vector ccma_points(const std::vector& points); - +std::vector cameraPixelToGroundPos(std::vector& pixels, + image_geometry::PinholeCameraModel& rgb_info_sub); +nav_msgs::msg::Path cameraPixelToGroundPath(std::vector& pixels, + const image_geometry::PinholeCameraModel& rgb_info_sub, float camera_height, + std::string frame_id); +std::optional create_path(std::vector& left_contours, + std::vector& right_contours, + image_geometry::PinholeCameraModel& camera_info, std::string frame_id); + +ccma::CCMA ccma_obj; +std::vector ccma_points(const std::vector& points); + } // namespace backend diff --git a/include/polynomial_planner/ccma.hpp b/include/polynomial_planner/ccma.hpp deleted file mode 100644 index 642fd48..0000000 --- a/include/polynomial_planner/ccma.hpp +++ /dev/null @@ -1,441 +0,0 @@ -/*********************************************************************************************** - * CURVATURE-CORRECTED MOVING-AVERAGE ( C C M A ) - * - * A **model-free** path-smoothing algorithm for 2-D / 3-D data that avoids the typical - * inward-bending artefact of a plain moving average by *explicit* curvature compensation. - * - * This header is the literal C++17 translation of the official Python reference - * implementation (Steinecker & Wuensche, IV 2023) – **but** every relevant line is - * annotated with two kinds of comments: - * - * WHAT → explains *what* the statement does. - * WHY → explains *why* that design/implementation decision was taken. - * - * Build test-program (with Eigen in include-path): - * g++ -std=c++17 -O3 example.cpp -I /usr/include/eigen3 - * - **********************************************************************************************/ -#ifndef CCMA_HPP -#define CCMA_HPP - -/*───────────────────────────────────────────────────────────────────────────────* - * WHAT: Standard library & Eigen headers. * - * WHY : • Eigen supplies vector/matrix math without external libs. * - * • : math functions (sin, cos, log, etc.). * - * • : std::accumulate for kernel normalisation. * - *───────────────────────────────────────────────────────────────────────────────*/ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -namespace ccma { -/*=============================================================================* - * Helper utilities * - *============================================================================*/ -namespace detail { -/* WHAT: √2 constant – used in the error-function based normal CDF. */ -constexpr double kSQRT2 = 1.41421356237309504880168872420969808; - -/*---------------------------------------------------------------------------*/ -/* Φ(x) – Standard-normal cumulative distribution function. */ -/* WHAT: wrapper around std::erfc so we avoid heavyweight. */ -/* WHY : fastest portable implementation in . */ -inline double normal_cdf(double x) noexcept { return 0.5 * std::erfc(-x / kSQRT2); } - -/*---------------------------------------------------------------------------*/ -/* unit(v) – returns v/‖v‖ (or v if ‖v‖==0). * - * WHY: frequent normalisations; inline avoids function-call overhead. */ -inline Eigen::Vector3d unit(const Eigen::Vector3d& v) noexcept { - double n = v.norm(); - return n == 0.0 ? v : v / n; -} - -/*---------------------------------------------------------------------------*/ -/* Manual *valid-length* 1-D convolution (kernel shorter than data). * - * WHAT: reproduces numpy.convolve(..., mode="valid") exactly. * - * WHY : Eigen lacks an easy 1-D convolution; writing loop is trivial & * - * avoids another dependency. */ -inline std::vector convolve_valid(const std::vector& data, const std::vector& kernel) { - std::size_t n = data.size(), k = kernel.size(); - if (n < k) throw std::runtime_error("convolve_valid: kernel longer than data"); - - std::vector res(n - k + 1, 0.0); - for (std::size_t i = 0; i <= n - k; ++i) - for (std::size_t j = 0; j < k; ++j) res[i] += data[i + j] * kernel[j]; - return res; -} - -} // namespace detail - -/*=============================================================================* - * CCMA class * - *============================================================================*/ -class CCMA { -public: - /*-------------------------------------------------------------------------*/ - /* Constructor – stores hyper-parameters *and* pre-computes kernels. * - * WHY: weights are independent of data → compute once, reuse often. */ - CCMA(int w_ma = 5, /* half-window for moving avg */ - int w_cc = 3, /* half-window for curvature */ - std::string distrib = "pascal", /* default kernel family */ - std::string distrib_ma = "", /* override for MA (opt) */ - std::string distrib_cc = "", /* override for CC (opt) */ - double rho_ma = 0.95, /* truncation area (normal) */ - double rho_cc = 0.95) - : w_ma_(w_ma), - w_cc_(w_cc), - w_ccma_(w_ma + w_cc + 1) /* larger composite window */ - , - distrib_ma_(distrib_ma.empty() ? distrib : distrib_ma), - distrib_cc_(distrib_cc.empty() ? distrib : distrib_cc), - rho_ma_(rho_ma), - rho_cc_(rho_cc) { - /* WHAT: generate lookup tables of kernels for all window sizes ≤ w. */ - weights_ma_ = generate_weights(w_ma_, distrib_ma_, rho_ma_); - weights_cc_ = generate_weights(w_cc_, distrib_cc_, rho_cc_); - } - - /*-------------------------------------------------------------------------*/ - /* Public API: filter() */ - /* WHAT: accepts raw points, returns smoothed points. * - * WHY : hides boundary handling + 2-D/3-D up-conversion details. */ - std::vector filter(const std::vector& input, const std::string& mode = "padding", - bool cc_mode = true) const { - /*———— Sanity checks ————*/ - if (mode != "none" && mode != "padding" && mode != "wrapping" && mode != "fill_boundary") - throw std::invalid_argument("CCMA::filter: invalid mode"); - - if (input.size() < 3) throw std::runtime_error("CCMA::filter: at least 3 points required"); - - /* copy because we may extend/modify pts for boundary treatment */ - Eigen::MatrixXd pts(3, input.size()); - - for (int i = 0; i < input.size(); i++) { - pts(0,i) = input.at(i).x; - pts(1,i) = input.at(i).y; - pts(2,i) = 0; - } - - /*======================== 1. Boundary pre-processing =================*/ - // if (mode == "padding") { - // /* WHAT: replicate first/last sample to keep length unchanged. * - // * WHY : simplest length-preserving strategy when data are *open*.*/ - // int n_pad = cc_mode ? w_ccma_ : w_ma_; - // pts.conservativeResize(pts.rows() + 2 * n_pad, pts.cols()); - // pts.block(0, 0, n_pad, pts.cols()) = pts.row(n_pad).replicate(n_pad, 1); - // pts.block(pts.rows() - n_pad, 0, n_pad, pts.cols()) = pts.row(pts.rows() - n_pad - 1).replicate(n_pad, 1); - // } else if (mode == "wrapping") { - // /* WHAT: treat data as cyclic. * - // * WHY : useful for closed curves (e.g. circle, track loop). */ - // int n_pad = cc_mode ? w_ccma_ : w_ma_; - // Eigen::MatrixXd tmp(pts.rows() + 2 * n_pad, pts.cols()); - // tmp.block(0, 0, n_pad, pts.cols()) = pts.bottomRows(n_pad); - // tmp.block(n_pad, 0, pts.rows(), pts.cols()) = pts; - // tmp.block(n_pad + pts.rows(), 0, n_pad, pts.cols()) = pts.topRows(n_pad); - // pts.swap(tmp); - // } - - /* ensure enough points remain after trimming (for “none” mode) */ - if (pts.cols() < (cc_mode ? w_ccma_ * 2 + 1 : w_ma_ * 2 + 1)) - throw std::runtime_error("CCMA::filter: not enough points for given widths"); - - /*======================== 2. 2-D → 3-D up-conversion =================*/ // should never run - // bool is2d = (pts.cols() == 2); - // if (is2d) { - // /* WHY: core math implemented in 3-D; simply append z=0 column. */ - // Eigen::MatrixXd tmp(pts.rows(), 3); - // tmp.block(0, 0, pts.rows(), 2) = pts; - // tmp.col(2).setZero(); - // pts.swap(tmp); - // } - - /*======================== 3. Filter core =============================*/ - Eigen::MatrixXd out; - - if (mode != "fill_boundary") { - /* full-size kernel everywhere */ - out = _filter(pts, w_ma_, w_cc_, cc_mode); - } else { - /* cascading smaller kernels near edges */ - out = fill_boundary_mode(pts, cc_mode); - } - - std::vector ret; - for (int i = 0; i < out.cols(); i++) { - cv::Point2d toPush(out(0,i), out(1,i)); - ret.push_back(toPush); - } - - /*======================== 4. Return shape as input ===================*/ - return ret; - } - - /*============================================================================* - * Internals * - *============================================================================*/ -private: - /*-------------------------------------------------------------------------*/ - /* generate_weights() – produces *all* kernels up to window size w. * - * WHY: pre-computing dramatically speeds repeated filtering. */ - static std::vector> generate_weights(int w, const std::string& distrib, double crit_z) { - std::vector> weight_list; - weight_list.reserve(w + 1); - - /*================ Normal (truncated) kernel ==========================*/ - if (distrib == "normal") { - /* WHAT: boundaries that enclose ‘rho’ area under N(0,1). */ - crit_z = std::fabs(crit_z); - double x_start = -crit_z; - double x_end = crit_z; - - for (int wi = 0; wi <= w; ++wi) { - int k = 2 * wi + 1; /* kernel length*/ - std::vector weights(k, 0.0); - - /* integrate PDF over slices → exactly matches Python impl. */ - std::vector x(k + 1); - for (int i = 0; i <= k; ++i) x[i] = x_start + (x_end - x_start) * static_cast(i) / k; - - for (int i = 0; i < k; ++i) weights[i] = detail::normal_cdf(x[i + 1]) - detail::normal_cdf(x[i]); - - /* renormalise to sum≈1 (truncation removed mass ‘1-rho’) */ - for (double& v : weights) v /= (1 - detail::normal_cdf(crit_z) * 2); - weight_list.emplace_back(std::move(weights)); - } - } - /*================ Uniform kernel ====================================*/ - else if (distrib == "uniform") { - for (int wi = 0; wi <= w; ++wi) { - int k = 2 * wi + 1; - weight_list.emplace_back(k, 1.0 / static_cast(k)); - } - } - /*================ Pascal (binomial) kernel ==========================*/ - else if (distrib == "pascal") { - auto pascal_row = [](int n) { - /* WHAT: computes row ‘n’ of Pascal’s triangle iteratively. */ - std::vector row{1.0}; - for (int r = 1; r <= n; ++r) row.push_back(row.back() * static_cast(n - r + 1) / r); - return row; - }; - - for (int wi = 0; wi <= w; ++wi) { - auto row = pascal_row(2 * wi); /* even row length */ - double sum = std::accumulate(row.begin(), row.end(), 0.0); - for (double& v : row) v /= sum; /* normalise */ - weight_list.emplace_back(std::move(row)); - } - } - /*================ Hanning (raised-cosine) kernel ====================*/ - else if (distrib == "hanning") { - for (int wi = 0; wi <= w; ++wi) { - int k = 2 * wi + 1; - int window_size = k + 2; /* match Python impl*/ - std::vector kernel; - kernel.reserve(k); - - for (int n = 1; n <= k; ++n) { - double v = 0.5 * (1.0 - std::cos(2.0 * M_PI * n / (window_size - 1))); - kernel.push_back(v); - } - double sum = std::accumulate(kernel.begin(), kernel.end(), 0.0); - for (double& v : kernel) v /= sum; - weight_list.emplace_back(std::move(kernel)); - } - } else { - throw std::invalid_argument("unknown kernel distribution '" + distrib + "'"); - } - return weight_list; - } - - /*-------------------------------------------------------------------------*/ - /* ma_points() – convolution along each axis with chosen MA kernel. * - * WHAT: returns *valid* samples only → easy index bookkeeping later. * - * WHY : explicit loops avoid Eigen’s heavy convolution workaround. */ - inline static Eigen::MatrixXd ma_points(const Eigen::MatrixXd& points, const std::vector& weights) { - int k = (weights.size()); - int n_out = points.rows() - k ; - Eigen::MatrixXd out(n_out, 3); - - Eigen::VectorXd w = Eigen::Map(weights.data(), k); - - for (int i = 0; i < n_out; ++i) { // IF CRASHES PLEASE LOOK HERE - out(0,i) = points.row(0).segment(i, k).dot(w); - out(1,i) = points.row(1).segment(i, k).dot(w); - out(2,i) = points.row(2).segment(i, k).dot(w); - } - return out; - } - - /*-------------------------------------------------------------------------*/ - /* curvature_vectors() – approximates signed curvature normal for each pt.*/ - /* WHAT: uses circumcircle of triplets (i-1,i,i+1). * - * WHY : local curvature needed to correct systematic MA shrinkage. */ - inline static Eigen::MatrixXd curvature_vectors(const Eigen::MatrixXd& pts) { - Eigen::MatrixXd cv = Eigen::MatrixXd::Zero(pts.rows(), 3); - - for (int i = 1; i < pts.rows() - 1; ++i) { - Eigen::Vector3d v1 = pts.row(i) - pts.row(i - 1); - Eigen::Vector3d v2 = pts.row(i + 1) - pts.row(i); - Eigen::Vector3d cross = v1.cross(v2); - double cross_norm = cross.norm(); - - double curvature = 0.0; - if (cross_norm != 0.0) { - double radius = v1.norm() * v2.norm() * (pts.row(i + 1) - pts.row(i - 1)).norm() / (2.0 * cross_norm); - curvature = 1.0 / radius; - } - cv.row(i) = curvature * detail::unit(cross); - } - return cv; - } - - /*-------------------------------------------------------------------------*/ - /* alphas() – angle between consecutive chord vectors under constant curv.*/ - inline static std::vector alphas(const Eigen::MatrixXd& pts, const std::vector& curv) { - int n = pts.rows(); - std::vector a(n, 0.0); - - for (int i = 1; i < n - 1; ++i) { - if (curv[i] != 0.0) { - double R = 1.0 / curv[i]; - double d = (pts.row(i + 1) - pts.row(i - 1)).norm(); - a[i] = std::asin(0.5 * d / R); - } - } - return a; - } - - /*-------------------------------------------------------------------------*/ - /* normalized_ma_radii() – Eq. (7) in the paper (figure-2 intuition). * - * WHY: converts angles to *radius shrink-factor* for curvature fix. */ - inline static std::vector normalized_ma_radii(const std::vector& alphas, int w_ma, - const std::vector& weights) { - int n = static_cast(alphas.size()); - std::vector radii(n, 0.0); - - for (int i = 1; i < n - 1; ++i) { - double radius = weights[w_ma]; /* central weight (k=0) */ - for (int k = 1; k <= w_ma; ++k) radius += 2.0 * std::cos(alphas[i] * k) * weights[w_ma + k]; - radii[i] = std::max(0.35, radius); /* lower-bound to avoid overshoot */ - } - return radii; - } - - /*-------------------------------------------------------------------------*/ - /* _filter() – workhorse for *one* window size (possibly with CC). */ - Eigen::MatrixXd _filter(const Eigen::MatrixXd& pts, int w_ma, int w_cc, bool cc_mode) const { - int w_ccma = w_ma + w_cc + 1; - - /*——- Step 1: plain moving-average over pts ————————————————*/ - Eigen::MatrixXd pts_ma = ma_points(pts, weights_ma_[w_ma]); - - if (!cc_mode) return pts_ma; /* user disabled CC */ - - /*——- Step 2: curvature + correction vectors ———————————————*/ - Eigen::MatrixXd cv = curvature_vectors(pts_ma); - std::vector curv(cv.rows()); - for (int i = 0; i < cv.rows(); ++i) curv[i] = cv.row(i).norm(); - - std::vector a = alphas(pts_ma, curv); - std::vector radii_ma = normalized_ma_radii(a, w_ma, weights_ma_[w_ma]); - - /*——- Step 3: reconstruct curvature-corrected points ————————*/ - int n_out = pts.rows() - 2 * w_ccma; - Eigen::MatrixXd out(n_out, 3); - - for (int i = 0; i < n_out; ++i) { - /* tangent vector (for cross-product reconstruction) */ - Eigen::Vector3d unit_tangent = detail::unit(pts_ma.row(w_cc + i + 2) - pts_ma.row(w_cc + i)); - - Eigen::Vector3d shift = Eigen::Vector3d::Zero(); - - for (int j = 0; j < 2 * w_cc + 1; ++j) { - int idx = i + j + 1; /* corresponding MA index */ - if (curv[idx] == 0.0) continue; /* straight line → no shift */ - - Eigen::Vector3d u_vec = detail::unit(cv.row(idx)); - double weight = weights_cc_[w_cc][j]; - double shift_mag = (1.0 / curv[idx]) * (1.0 / radii_ma[idx] - 1.0); - shift += u_vec * weight * shift_mag; - } - /* reconstruct by rotating shift into tangent plane */ - out.col(i) = pts_ma.col(i + w_cc + 1) + unit_tangent.cross(shift); - } - return out; - } - - /*-------------------------------------------------------------------------*/ - /* fill_boundary_mode() – gradually shrinking window near ends. * - * WHY: keeps *length* unchanged while avoiding heavy extrapolation. */ - Eigen::MatrixXd fill_boundary_mode(const Eigen::MatrixXd& pts, bool cc_active) const { - int dim = 3; - Eigen::MatrixXd out = Eigen::MatrixXd::Zero(pts.rows(), dim); - - if (cc_active) /* CCMA with shrinking windows */ - { - /* Build descending sequence of (w_ma,w_cc) pairs, largest first. */ - std::vector> widths; - int w_ma_cur = w_ma_, w_cc_cur = w_cc_; - while (w_ma_cur > 0 || w_cc_cur > 0) { - (w_cc_cur >= w_ma_cur) ? --w_cc_cur : --w_ma_cur; - widths.emplace_back(w_ma_cur, w_cc_cur); - } - std::reverse(widths.begin(), widths.end()); - - /* Unchanged first/last original points */ - out.row(0) = pts.row(0); - out.row(pts.rows() - 1) = pts.row(pts.rows() - 1); - - /* Fully filtered *interior* */ - out.block(w_ccma_, 0, pts.rows() - 2 * w_ccma_, dim) = _filter(pts, w_ma_, w_cc_, true); - - /* Ascending/descending partial windows */ - for (std::size_t w_idx = 0; w_idx < widths.size(); ++w_idx) { - int wma = widths[w_idx].first; - int wcc = widths[w_idx].second; - int wccma = wma + wcc + 1; - - /* Design choice: if w_ma==0 use MA=1 but no curvature corr. */ - bool use_ma1 = (wma == 0 && w_ma_ != 0); - - /* Start side */ - out.row(w_idx + 1) = - _filter(pts.topRows(w_idx + 1 + wccma + 1), use_ma1 ? 1 : wma, wcc, use_ma1 ? false : true).row(0); - - /* End side */ - out.row(out.rows() - w_idx - 2) = - _filter(pts.bottomRows(w_idx + 1 + wccma + 1), use_ma1 ? 1 : wma, wcc, use_ma1 ? false : true) - .row(0); - } - } else /* MA-only variant with width-cascade */ - { - out.block(w_ma_, 0, pts.rows() - 2 * w_ma_, dim) = _filter(pts, w_ma_, 0, false); - - for (int w = 0; w < w_ma_; ++w) { - out.row(w) = _filter(pts.topRows(2 * w + 1), w, 0, false).row(0); - - out.row(out.rows() - w - 1) = _filter(pts.bottomRows(2 * w + 1), w, 0, false).row(0); - } - } - return out; - } - - /*============================================================================* - * Data members * - *============================================================================*/ - int w_ma_, w_cc_, w_ccma_; /* window params */ - std::string distrib_ma_, distrib_cc_; /* kernel families */ - double rho_ma_, rho_cc_; /* truncation area */ - std::vector> weights_ma_, weights_cc_; /* kernel LUTs */ -}; - -} // namespace ccma -#endif /* CCMA_HPP */ diff --git a/src/CCMA.cpp b/src/CCMA.cpp new file mode 100644 index 0000000..030bd96 --- /dev/null +++ b/src/CCMA.cpp @@ -0,0 +1,271 @@ + +#pragma once + +#include "polynomial_planner/CCMA.hpp" + +#include +using namespace ccma; +using std::vector, std::string, Eigen::MatrixXd, Eigen::VectorXd; + +/* POINTS MATRIX LOOKS LIKE THIS: CHATGPT FORGETS THIS SOMETIMES AND MAKES IT VERTICAL BUT NO! IT IS LIKE THIS! I SWEAR! +* Z is along for the ride fyi +* [ x, x, x, x, x, x, x, x, x, x, x, x, x, ... +* y, y, y, y, y, y, y, y, y, y, y, y, y, ... +* z, z, z, z, z, z, z, z, z, z, z, z, z, ... ] +* +* WHEN WE WANT TO DO THE CONVOLUTION, WE TAKE SLICES THAT ARE THE WIDTH OF THE THING WE ARE DOTTING (AKA THE WEIGHTS) +* +* [ x1, x2, x3, x4 [ w1 +* y1, y2, y3, y4 * w2 +* z1, z2, z3, z4 ] w3 +* w4 ] +* +* SO WE GET +* +* [ xv*wv +* yv*wv +* zv*wv ] +* +* +* +* Type naming convention: +* +* MatrixXd << X dimension aka dynamic, d for double +* Matrix3d << 3x3 of doubles +* VectorXd << dynamic dimensions, doubles. one column. +* There is a templated constructor needed +* Transposes exist but there's a comment against using them +* +* +* +*/ + +// Convert vector to Eigen::MatrixXd +Eigen::MatrixXd CCMA::points_to_MatrixXd(const std::vector& points) { + const size_t n = points.size(); + Eigen::MatrixXd mat(3, n); // n rows, 3 columns (x, y, z) + + for (size_t i = 0; i < n; ++i) { + const cv::Point2d& pt = points[i]; + mat(0, i) = pt.x; // x-coordinate + mat(1, i) = pt.y; // y-coordinate + mat(2, i) = 0.0; // z-coordinate (always 0 for 2D points) + } + + return mat; +} + +// Convert Eigen::MatrixXd to vector +std::vector CCMA::matrixXd_to_Points2d(const Eigen::MatrixXd& matrix) { + std::vector points; + points.reserve(matrix.cols()); // Number of columns = number of points + + for (int i = 0; i < matrix.cols(); ++i) { + points.emplace_back(matrix(0, i), matrix(1, i)); + } + return points; +} + +vector> CCMA::generate_weights(int width, const string& distribution, double crit_z_val) { + vector> weight_list; + weight_list.reserve(width + 1); + if (distribution == "normal") { + crit_z_val = fabs(crit_z_val); // assuring we don't get these backward by mistake + double x_start = -crit_z_val; + double x_end = crit_z_val; + + for (int wi = 0; wi <= width; + ++wi) { // loops through each width until width is reached, can probably be removed + int k = 2 * wi + 1; // kernel width + vector weights(k, 0.0); + vector x(k + 1); + for (int i = 0; i <= k; i++) { + x[i] = x_start + (x_end - x_start) * i / k; + } + + for (int i = 0; i < k; i++) { + weights[i] = helpers::normal_cdf(x[i + 1]) - helpers::normal_cdf(x[i]); + } + + double sum = 0; + for (int i = 0; i < weights.size(); i++) { + sum += weights[i]; + } + for (double& w : weights) w /= sum; + weight_list.emplace_back(weights); + } + } else if (distribution == "uniform") { + for (int wi = 0; wi <= width; wi++) { + int k = 2 * wi + 1; + weight_list.emplace_back(k, 1.0 / k); + } + } else if (distribution == "pascal") { + auto pascal_row = [](int n) { + vector row{1}; + + for (int r = 1; r <= n; r++) { + row.push_back(row.back() * static_cast(n - r + 1) / r); // neat algorithm + } + + return row; + }; + + for (int wi = 0; wi <= width; wi++) { + auto row = pascal_row(2 * wi); + double sum = std::accumulate(row.begin(), row.end(), 0.0); + + for (double& v : row) v /= sum; + + weight_list.emplace_back(std::move(row)); + } + } + + else + return (generate_weights(width, "pascal", crit_z_val)); + + return weight_list; +}; + +MatrixXd CCMA::ma_points(const MatrixXd& points, const vector& weights) const { + int kernel_width = weights.size(); + int last_index = points.cols() - kernel_width; + + // e.g. if we have a 3x3 matrix and we have 2 weights (for some reason), + // we do one ma at col 1 (index 0, covers col 1 and 2) + // and one at col 2 (index 1, covers col 2 and 3). + + MatrixXd out(3, last_index + 1); + VectorXd w = Eigen::Map(weights.data(), kernel_width); + + for (int column = 0; column < last_index + 1; column++) { // v not off by one? + for (int row = 0; row < points.rows(); row++) { + out(row, column) = points.row(row).segment(column, kernel_width).dot(w); + } + } // this had to be flipped from gpt implementation because dot products don't work the other way around and there's no transpose (without warning lmao) + + return out; +} + +MatrixXd CCMA::curvature_vectors( + const MatrixXd& points) { // this might not need to be MatrixXd, see if it can be replaced with a vector + // this might not need to be curvature, see if it can be replaced with radii. + MatrixXd kv = MatrixXd::Zero(3, points.cols()); + + for (int i = 1; i < points.rows() - 1; i++) { // don't index outermost points + const Eigen::Vector3d& previous_point = points.col(i - 1); + const Eigen::Vector3d& this_point = points.col(i); + const Eigen::Vector3d& next_point = + points.col(i + 1); // i have no idea if this is more or less readable than just using indexes lmao + + const Eigen::Vector3d vector_to_previous = previous_point - this_point; + const Eigen::Vector3d vector_to_next = next_point - this_point; + const Eigen::Vector3d vector_between_others = next_point - previous_point; + const Eigen::Vector3d prev_next_cross = + vector_to_previous.cross(vector_to_next); // hopefully makes vector math more obvious + const double prev_next_cross_mag = prev_next_cross.norm(); + + double k = 0.0; + if (prev_next_cross_mag != 0.0) { + double r = -vector_to_previous.norm() * vector_to_next.norm() * vector_between_others.norm() / + (2.0 * prev_next_cross_mag); + // ^ radius can be negative, because we want curvature to be negative and we want signs in various other places. if you want actual r take mag. + k = 1 / r; + } + kv(2, i) = k; + } + + return kv; +} + +vector CCMA::alphas(const MatrixXd& points, const vector& curvatures) { + int width = points.cols(); + vector angles(width, 0.0); + + for (int i = 1; i < width - 1; i++) { + if (curvatures[i] != 0) { + double r = 1 / curvatures[i]; + double d = (points.col(i - 1) - points.col(i + 1)).norm(); + angles[i] = std::asin(0.5 * d / r); + } + } + + return angles; +} + +vector CCMA::normalized_ma_radii(const vector& alphas, int w_ma, const vector& weights) { + int n = alphas.size(); + vector radii(n, 0.0); + + for (int i = 1; i < n - 1; i++) { + double r = weights[w_ma]; + + for (int k = 1; k <= w_ma; k++) { + r += 2 * std::cos(alphas[i] * k) * weights[w_ma + k]; // consult the magic paper for math + } + + radii[i] = std::max(0.35, r); // do we need this max function? + } + + return radii; +} + +MatrixXd CCMA::_filter(const MatrixXd& points, int w_ma, int w_cc, bool cc_mode) const { + int w_ccma = w_ma + w_cc + 1; + + MatrixXd points_ma = ma_points(points, weights_ma_[w_ma]); + + if (!cc_mode) return points_ma; + + MatrixXd cv = curvature_vectors(points_ma); + vector curvatures(cv.cols()); + + for (int i = 0; i < cv.cols(); i++) { + curvatures[i] = cv.col(i).norm(); // shouldn't this just get the z coordinate?? + } + + vector angles = alphas(points_ma, curvatures); + vector radii_ma = normalized_ma_radii(angles, w_ma, weights_ma_[w_ma]); + + int n_out = points.cols() - 2 * w_ccma; + MatrixXd out(3, n_out); + + for (int i = 0; i < n_out; i++) { + Eigen::Vector3d unit_tangent = helpers::unit( + points_ma.col(w_cc + i + 2) - points_ma.col(w_cc + i)); // adjacent columns to the w_cc+i th column + Eigen::Vector3d shift = Eigen::Vector3d::Zero(); + + for (int j = 0; j < 2 * w_cc + 1; j++) { // iterates through all of the cc weights + + int idx = i + j + 1; + + if (curvatures[idx] == 0) continue; // skip if straight line + + Eigen::Vector3d curvature_unit_vector = helpers::unit(cv.col(idx)); + double weight = weights_cc_[w_cc][j]; // grab the jth weight from the w_cc weights width + double shift_mag = (1 / curvatures[idx]) * (1 / radii_ma[idx] - 1); + shift += curvature_unit_vector * weight * shift_mag; + } + + out.col(i) = + points_ma.col(i + w_cc + 1) + + unit_tangent.cross( + shift); // gets the normal to the vector between the adjacent points and multiplies it by the shift and shifts the original point. + } + + return out; +} + +/*MatrixXd fill_boundary_mode(const MatrixXd& points, bool cc_active) const { + int dim = points.rows(); + MatrixXd out = MatrixXd::Zero(points.rows(), dim); // only needed if we want to use edge filling which keeps the same amount of points, without this we lose ~ w_ccma points from either end, but we have a ton of points so this shouldn't really matter. Can be implemented if needed. +}*/ + +MatrixXd CCMA::filter(const MatrixXd& points, const string& mode, bool cc_mode) { + // PADDING AND WRAPPING NOT IMPLEMENTED BECAUSE WE SHOULDN'T NEED THEM, PADDING MIGHT BE NICE TO HAVE BUT WE NEED TO MAKE SURE THIS WORKS + + if (points.cols() < (cc_mode ? w_ccma_ * 2 + 1 : w_ma_ * 2 + 1)) { + // since we need more than our width as points we just panic if we dont get them and return 0. + return MatrixXd::Zero(points.rows(), points.cols()); + } + return _filter(points, w_ma_, w_cc_, cc_mode); +} diff --git a/src/backend.cpp b/src/backend.cpp index 5b61e2b..67d2682 100644 --- a/src/backend.cpp +++ b/src/backend.cpp @@ -4,6 +4,7 @@ #include #include +#include "CCMA.cpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "image_geometry/pinhole_camera_model.h" @@ -23,11 +24,11 @@ std::optional backend::create_path(std::vector std::vector cam_path; // this is the vector of path plannign points in camera space ground_path.emplace_back(cv::Point2d(0, 0)); - int width = camera_info.fullResolution().width; // camera space sizes! - int height = camera_info.fullResolution().height; // Camera space sizes! + // int width = camera_info.fullResolution().width; // camera space sizes! + // int height = camera_info.fullResolution().height; // Camera space sizes! - bool is_right_valid = true; // stores if Polynomial was intizatized! - bool is_left_valid = true; // left and right respectively + // bool is_right_valid = true; // stores if Polynomial was intizatized! + // bool is_left_valid = true; // left and right respectively if (left_contours.empty()) { // for any and all checks regarding data cleaning! @@ -47,9 +48,9 @@ std::optional backend::create_path(std::vector cam_path.push_back(cv::Point2d(x, y)); } - cam_path = ccma_points(cam_path); + ground_path = backend::ccma_points(cam_path); - if (cam_path.empty()) { + if (ground_path.empty()) { return std::nullopt; } else { // Convert from cv types to nav::msg @@ -170,7 +171,9 @@ nav_msgs::msg::Path backend::cameraPixelToGroundPath(std::vector& p } std::vector backend::ccma_points(const std::vector& ground_points) { - if (ground_points.size() > 3) - return ccma_obj.filter(ground_points, "none"); + if (ground_points.size() > 3) { + const MatrixXd cam_matrix = ccma_obj.points_to_MatrixXd(ground_points); + return ccma_obj.matrixXd_to_Points2d(ccma_obj.filter(cam_matrix, "none")); + } return ground_points; } \ No newline at end of file From c75221efe09be85f1a8f0e485b5fc840afba712f Mon Sep 17 00:00:00 2001 From: redto0 Date: Mon, 2 Jun 2025 02:28:34 -0400 Subject: [PATCH 3/4] CCMA WORKS BABY!!!! we didnt publish the right array :sob: Signed-off-by: redto0 --- include/polynomial_planner/backend.hpp | 2 +- src/CCMA.cpp | 22 +++++++++++----------- src/backend.cpp | 7 ++++--- 3 files changed, 16 insertions(+), 15 deletions(-) diff --git a/include/polynomial_planner/backend.hpp b/include/polynomial_planner/backend.hpp index 04b157c..89142d5 100644 --- a/include/polynomial_planner/backend.hpp +++ b/include/polynomial_planner/backend.hpp @@ -6,7 +6,7 @@ #include #include -#include "CCMA.hpp" +#include "polynomial_planner/CCMA.hpp" #include "image_geometry/pinhole_camera_model.h" #include "nav_msgs/msg/path.hpp" diff --git a/src/CCMA.cpp b/src/CCMA.cpp index 030bd96..69c20d0 100644 --- a/src/CCMA.cpp +++ b/src/CCMA.cpp @@ -126,7 +126,7 @@ vector> CCMA::generate_weights(int width, const string& distribut return weight_list; }; -MatrixXd CCMA::ma_points(const MatrixXd& points, const vector& weights) const { +MatrixXd CCMA::ma_points(const Eigen::MatrixXd& points, const vector& weights) const { int kernel_width = weights.size(); int last_index = points.cols() - kernel_width; @@ -134,7 +134,7 @@ MatrixXd CCMA::ma_points(const MatrixXd& points, const vector& weights) // we do one ma at col 1 (index 0, covers col 1 and 2) // and one at col 2 (index 1, covers col 2 and 3). - MatrixXd out(3, last_index + 1); + Eigen::MatrixXd out(3, last_index + 1); VectorXd w = Eigen::Map(weights.data(), kernel_width); for (int column = 0; column < last_index + 1; column++) { // v not off by one? @@ -147,9 +147,9 @@ MatrixXd CCMA::ma_points(const MatrixXd& points, const vector& weights) } MatrixXd CCMA::curvature_vectors( - const MatrixXd& points) { // this might not need to be MatrixXd, see if it can be replaced with a vector + const Eigen::MatrixXd& points) { // this might not need to be MatrixXd, see if it can be replaced with a vector // this might not need to be curvature, see if it can be replaced with radii. - MatrixXd kv = MatrixXd::Zero(3, points.cols()); + Eigen::MatrixXd kv = Eigen::MatrixXd::Zero(3, points.cols()); for (int i = 1; i < points.rows() - 1; i++) { // don't index outermost points const Eigen::Vector3d& previous_point = points.col(i - 1); @@ -177,7 +177,7 @@ MatrixXd CCMA::curvature_vectors( return kv; } -vector CCMA::alphas(const MatrixXd& points, const vector& curvatures) { +vector CCMA::alphas(const Eigen::MatrixXd& points, const vector& curvatures) { int width = points.cols(); vector angles(width, 0.0); @@ -209,14 +209,14 @@ vector CCMA::normalized_ma_radii(const vector& alphas, int w_ma, return radii; } -MatrixXd CCMA::_filter(const MatrixXd& points, int w_ma, int w_cc, bool cc_mode) const { +Eigen::MatrixXd CCMA::_filter(const Eigen::MatrixXd& points, int w_ma, int w_cc, bool cc_mode) const { int w_ccma = w_ma + w_cc + 1; - MatrixXd points_ma = ma_points(points, weights_ma_[w_ma]); + Eigen::MatrixXd points_ma = ma_points(points, weights_ma_[w_ma]); if (!cc_mode) return points_ma; - MatrixXd cv = curvature_vectors(points_ma); + Eigen::MatrixXd cv = curvature_vectors(points_ma); vector curvatures(cv.cols()); for (int i = 0; i < cv.cols(); i++) { @@ -227,7 +227,7 @@ MatrixXd CCMA::_filter(const MatrixXd& points, int w_ma, int w_cc, bool cc_mode) vector radii_ma = normalized_ma_radii(angles, w_ma, weights_ma_[w_ma]); int n_out = points.cols() - 2 * w_ccma; - MatrixXd out(3, n_out); + Eigen::MatrixXd out(3, n_out); for (int i = 0; i < n_out; i++) { Eigen::Vector3d unit_tangent = helpers::unit( @@ -260,12 +260,12 @@ MatrixXd CCMA::_filter(const MatrixXd& points, int w_ma, int w_cc, bool cc_mode) MatrixXd out = MatrixXd::Zero(points.rows(), dim); // only needed if we want to use edge filling which keeps the same amount of points, without this we lose ~ w_ccma points from either end, but we have a ton of points so this shouldn't really matter. Can be implemented if needed. }*/ -MatrixXd CCMA::filter(const MatrixXd& points, const string& mode, bool cc_mode) { +Eigen::MatrixXd CCMA::filter(const Eigen::MatrixXd& points, const string& mode, bool cc_mode) { // PADDING AND WRAPPING NOT IMPLEMENTED BECAUSE WE SHOULDN'T NEED THEM, PADDING MIGHT BE NICE TO HAVE BUT WE NEED TO MAKE SURE THIS WORKS if (points.cols() < (cc_mode ? w_ccma_ * 2 + 1 : w_ma_ * 2 + 1)) { // since we need more than our width as points we just panic if we dont get them and return 0. - return MatrixXd::Zero(points.rows(), points.cols()); + return Eigen::MatrixXd::Zero(points.rows(), points.cols()); } return _filter(points, w_ma_, w_cc_, cc_mode); } diff --git a/src/backend.cpp b/src/backend.cpp index 67d2682..181c261 100644 --- a/src/backend.cpp +++ b/src/backend.cpp @@ -1,10 +1,11 @@ #include "polynomial_planner/backend.hpp" +#include "CCMA.cpp" #include #include #include -#include "CCMA.cpp" +#include "polynomial_planner/CCMA.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "image_geometry/pinhole_camera_model.h" @@ -58,7 +59,7 @@ std::optional backend::create_path(std::vector // TODO use tf2 to fnd the hieght // auto ground_points = backend::cameraPixelToGroundPos(cam_path, camera_info, 0.6, frame_id); nav_msgs::msg::Path msg{}; - std::transform(cam_path.begin(), cam_path.end(), std::back_inserter(msg.poses), + std::transform(ground_path.begin(), ground_path.end(), std::back_inserter(msg.poses), [&frame_id](const cv::Point2d& point) { geometry_msgs::msg::PoseStamped pose{}; // frame = "redto0 isn't sure if we use this"; @@ -172,7 +173,7 @@ nav_msgs::msg::Path backend::cameraPixelToGroundPath(std::vector& p std::vector backend::ccma_points(const std::vector& ground_points) { if (ground_points.size() > 3) { - const MatrixXd cam_matrix = ccma_obj.points_to_MatrixXd(ground_points); + const Eigen::MatrixXd cam_matrix = ccma_obj.points_to_MatrixXd(ground_points); return ccma_obj.matrixXd_to_Points2d(ccma_obj.filter(cam_matrix, "none")); } return ground_points; From ab390d6204245d75bc842ecf38f7e00579ae87b6 Mon Sep 17 00:00:00 2001 From: redto0 Date: Wed, 5 Nov 2025 20:40:50 -0500 Subject: [PATCH 4/4] added circle projection --- include/polynomial_planner/backend.hpp | 1 + src/backend.cpp | 71 +++++++++++++++++++++++++- 2 files changed, 70 insertions(+), 2 deletions(-) diff --git a/include/polynomial_planner/backend.hpp b/include/polynomial_planner/backend.hpp index 89142d5..a8aa1af 100644 --- a/include/polynomial_planner/backend.hpp +++ b/include/polynomial_planner/backend.hpp @@ -54,5 +54,6 @@ std::optional create_path(std::vector& left_co ccma::CCMA ccma_obj; std::vector ccma_points(const std::vector& points); +std::vector circle_project(const std::vector& ground_points, int kernel, float projection); } // namespace backend diff --git a/src/backend.cpp b/src/backend.cpp index 181c261..92cd130 100644 --- a/src/backend.cpp +++ b/src/backend.cpp @@ -46,10 +46,13 @@ std::optional backend::create_path(std::vector double x = bigger_array[i].x; double y = bigger_array[i].y - 1.80; + cam_path.push_back(cv::Point2d(x, y)); } - ground_path = backend::ccma_points(cam_path); + // backend::circle_project(const std::vector& ground_points, int kernel, float projection) + cam_path = circle_project(bigger_array, ) + ground_path = backend::ccma_points(cam_path, 5, 1.8); if (ground_path.empty()) { return std::nullopt; @@ -177,4 +180,68 @@ std::vector backend::ccma_points(const std::vector& gr return ccma_obj.matrixXd_to_Points2d(ccma_obj.filter(cam_matrix, "none")); } return ground_points; -} \ No newline at end of file +} + +std::vector backend::circle_project(const std::vector& ground_points, int kernel, float projection) { + // given k kernal + int k = kernel; + int j = (k - 1) / 2; + + std::vector moved_ground_points; + + for(int i = 0; i < ground_points.size(); i++){ + if ( j <= i || i <= j ){ + continue; + } + cv::Point2d left; + cv::Point2d right; + for (int m = i - j; m > j + i; j++){ + left.x += ground_points[m].x; + left.y += ground_points[m].y; + } + left.x = left.x / j; + left.y = left.y / j; + + for (int m = i + 1; m > j + i; j++){ + right.x += ground_points[m].x; + right.y += ground_points[m].y; + } + right.x = right.x / j; + right.y = right.y / j; + + cv::Point2d center_point = ground_points[i]; + + double x1 = left.x, y1 = left.y; + double x2 = center_point.x, y2 = center_point.y; + double x3 = right.x, y3 = right.y; + + // Calculate determinants + double A = x1 * (y2 - y3) - y1 * (x2 - x3) + (x2 * y3 - x3 * y2); + double B = (x1*x1 + y1*y1) * (y3 - y2) + + (x2*x2 + y2*y2) * (y1 - y3) + + (x3*x3 + y3*y3) * (y2 - y1); + double C = (x1*x1 + y1*y1) * (x2 - x3) + + (x2*x2 + y2*y2) * (x3 - x1) + + (x3*x3 + y3*y3) * (x1 - x2); + + cv::Point2d circle_center(-B / (2 * A), -C / (2 * A)); + double radius = std::sqrt((B*B + C*C) / (4*A*A) + (x1*x1 + y1*y1 - 2*circle_center.x*x1 - 2*circle_center.y*y1)); + + + // Calculate tangent vector at center_point + // The tangent is perpendicular to the radius vector from circle_center to center_point + cv::Point2d radius_vector = center_point - circle_center; + cv::Point2d tangent_vector(-radius_vector.y, radius_vector.x); // Rotate 90 degrees + + // Normalize tangent vector + double tangent_length = cv::norm(tangent_vector); + if (tangent_length > 0) { + tangent_vector /= tangent_length; + } + + // Project the point along the tangent direction + moved_ground_points.push_back( center_point + tangent_vector * projection); + } + + return ground_points; +}