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 23fad5d..a8aa1af 100644 --- a/include/polynomial_planner/backend.hpp +++ b/include/polynomial_planner/backend.hpp @@ -6,6 +6,7 @@ #include #include +#include "polynomial_planner/CCMA.hpp" #include "image_geometry/pinhole_camera_model.h" #include "nav_msgs/msg/path.hpp" @@ -50,4 +51,9 @@ nav_msgs::msg::Path cameraPixelToGroundPath(std::vector& pixels, 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 circle_project(const std::vector& ground_points, int kernel, float projection); + } // namespace backend diff --git a/src/CCMA.cpp b/src/CCMA.cpp new file mode 100644 index 0000000..69c20d0 --- /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 Eigen::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). + + 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? + 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 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. + 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); + 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 Eigen::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; +} + +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; + + Eigen::MatrixXd points_ma = ma_points(points, weights_ma_[w_ma]); + + if (!cc_mode) return points_ma; + + Eigen::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; + Eigen::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. +}*/ + +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 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 bb287ad..92cd130 100644 --- a/src/backend.cpp +++ b/src/backend.cpp @@ -1,9 +1,11 @@ #include "polynomial_planner/backend.hpp" +#include "CCMA.cpp" #include #include #include +#include "polynomial_planner/CCMA.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "image_geometry/pinhole_camera_model.h" @@ -23,11 +25,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! @@ -44,10 +46,15 @@ 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)); } - if (cam_path.empty()) { + // 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; } else { // Convert from cv types to nav::msg @@ -55,7 +62,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"; @@ -165,4 +172,76 @@ nav_msgs::msg::Path backend::cameraPixelToGroundPath(std::vector& p rwpoints.header.frame_id = frame_id; return rwpoints; -} \ No newline at end of file +} + +std::vector backend::ccma_points(const std::vector& ground_points) { + if (ground_points.size() > 3) { + 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; +} + +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; +}