From ca347a194b9b62c517c1a3a47e8745b6e48209cc Mon Sep 17 00:00:00 2001 From: Vegard Johansen Date: Sun, 2 Feb 2025 12:53:47 +0100 Subject: [PATCH] Bots engaged --- .../linedetectorPipe.hpp | 113 +++-- .../pipeline_line_fitting_ros.hpp | 33 +- .../include/pipeline_line_fitting/randsac.hpp | 117 ++--- .../launch/pipeline_line_fitting.launch.py | 24 +- pipeline-line-fitting/package.xml | 4 +- .../src/linedetectorPipe.cpp | 459 +++++++++--------- .../src/pipeline_line_fitting_ros.cpp | 166 ++++--- pipeline-line-fitting/src/randsac.cpp | 297 ++++++------ 8 files changed, 620 insertions(+), 593 deletions(-) diff --git a/pipeline-line-fitting/include/pipeline_line_fitting/linedetectorPipe.hpp b/pipeline-line-fitting/include/pipeline_line_fitting/linedetectorPipe.hpp index 589c9a7..618d5ec 100644 --- a/pipeline-line-fitting/include/pipeline_line_fitting/linedetectorPipe.hpp +++ b/pipeline-line-fitting/include/pipeline_line_fitting/linedetectorPipe.hpp @@ -2,65 +2,60 @@ #include #include -//using namespace cv; +// using namespace cv; using namespace std; using namespace arma; -class LinedetectorPipe{ - int n; - int k; - float t; - float fracOfPoints; - float removeT; - float finalScorethresh; - float minTurnAngle; - int size; - cv::Mat orgImg; // Original image - double original_width; // Member variable for original image width - double original_height; // Member variable for original image height - double scaleX; // Scaling factor for x-axis - double scaleY; // Scaling factor for y-axis - - RANDSAC randsac; - cv::Mat processedImg; - - //quick summary of the pipeline - public: - void _preprocess(cv::Mat &img, bool dist=true); - int detectSingleLine(const mat &points, const mat &values, const vector &lines, const int i); - void _postprocess(); - void _getEndPoints(Line &line, bool swap=false); - - public: - - ~LinedetectorPipe(); - //call operator is the entry point for the pipeline - vector operator()(const cv::Mat &img, const int maxLines); - cv::Mat drawResults(const cv::Mat &img, const vector &lines); - - LinedetectorPipe( - int n = 5, - int k = 500, - float t = 50.0, - float fracOfPoints = 0.001, - float removeT = 1000.0, - float finalScorethresh = 65.0, - float minTurnAngle = 1.5, - int size = 200) - : n(n), k(k), t(t), fracOfPoints(fracOfPoints), removeT(removeT), finalScorethresh(finalScorethresh), - minTurnAngle(minTurnAngle), size(size) { - /* - A line detector using RANSAC to find to lines in a bitmask image - n: How many random points to choose to start each iteration - k: How many iterations to run - t: Threshold for points to be considered inliers, for the purpose of scoring - fracOfPoints: minium fraction of points that need to bee inliers for a line to be considered - removeT: Threshold for points to be removed between iterations - finalScorethresh: required score to be counted as a detected line - minTurnAngle: minimum angle difference between lines, radians (ofc) - size: what size of square the image will be resized to during preprocessing - */ - randsac = RANDSAC(n, k, t, 2, removeT, finalScorethresh, minTurnAngle); - } - -}; \ No newline at end of file +class LinedetectorPipe { + int n; + int k; + float t; + float fracOfPoints; + float removeT; + float finalScorethresh; + float minTurnAngle; + int size; + cv::Mat orgImg; // Original image + double original_width; // Member variable for original image width + double original_height; // Member variable for original image height + double scaleX; // Scaling factor for x-axis + double scaleY; // Scaling factor for y-axis + + RANDSAC randsac; + cv::Mat processedImg; + + // quick summary of the pipeline +public: + void _preprocess(cv::Mat &img, bool dist = true); + int detectSingleLine(const mat &points, const mat &values, + const vector &lines, const int i); + void _postprocess(); + void _getEndPoints(Line &line, bool swap = false); + +public: + ~LinedetectorPipe(); + // call operator is the entry point for the pipeline + vector operator()(const cv::Mat &img, const int maxLines); + cv::Mat drawResults(const cv::Mat &img, const vector &lines); + + LinedetectorPipe(int n = 5, int k = 500, float t = 50.0, + float fracOfPoints = 0.001, float removeT = 1000.0, + float finalScorethresh = 65.0, float minTurnAngle = 1.5, + int size = 200) + : n(n), k(k), t(t), fracOfPoints(fracOfPoints), removeT(removeT), + finalScorethresh(finalScorethresh), minTurnAngle(minTurnAngle), + size(size) { + /* + A line detector using RANSAC to find to lines in a bitmask image + n: How many random points to choose to start each iteration + k: How many iterations to run + t: Threshold for points to be considered inliers, for the purpose of scoring + fracOfPoints: minimum fraction of points that need to bee inliers for a line + to be considered removeT: Threshold for points to be removed between + iterations finalScorethresh: required score to be counted as a detected line + minTurnAngle: minimum angle difference between lines, radians (ofc) + size: what size of square the image will be resized to during preprocessing + */ + randsac = RANDSAC(n, k, t, 2, removeT, finalScorethresh, minTurnAngle); + } +}; diff --git a/pipeline-line-fitting/include/pipeline_line_fitting/pipeline_line_fitting_ros.hpp b/pipeline-line-fitting/include/pipeline_line_fitting/pipeline_line_fitting_ros.hpp index d92e097..c17a9ae 100644 --- a/pipeline-line-fitting/include/pipeline_line_fitting/pipeline_line_fitting_ros.hpp +++ b/pipeline-line-fitting/include/pipeline_line_fitting/pipeline_line_fitting_ros.hpp @@ -1,34 +1,33 @@ #ifndef PIPELINE_LINE_FITTING_ROS_HPP #define PIPELINE_LINE_FITTING_ROS_HPP -#include -#include -#include -#include #include +#include #include +#include +#include #include +#include -class PipelineLineFittingNode : public rclcpp::Node{ +class PipelineLineFittingNode : public rclcpp::Node { public: - PipelineLineFittingNode(const rclcpp::NodeOptions & options); + PipelineLineFittingNode(const rclcpp::NodeOptions &options); - ~PipelineLineFittingNode(){}; + ~PipelineLineFittingNode() {}; private: + rclcpp::Subscription::SharedPtr image_sub_; + rclcpp::Publisher::SharedPtr + image_visualization_pub_; + rclcpp::Publisher::SharedPtr pose_array_pub_; - rclcpp::Subscription::SharedPtr image_sub_; - rclcpp::Publisher::SharedPtr image_visualization_pub_; - rclcpp::Publisher::SharedPtr pose_array_pub_; - - LinedetectorPipe pipeline; - bool publish_visualization_; + LinedetectorPipe pipeline; + bool publish_visualization_; - void image_callback(const sensor_msgs::msg::Image::SharedPtr msg); + void image_callback(const sensor_msgs::msg::Image::SharedPtr msg); - cv::Mat draw_lines(cv::Mat &image, const vector &lines); + cv::Mat draw_lines(cv::Mat &image, const vector &lines); }; - -#endif //PIPELINE_LINE_FITTING_ROS_HPP \ No newline at end of file +#endif // PIPELINE_LINE_FITTING_ROS_HPP diff --git a/pipeline-line-fitting/include/pipeline_line_fitting/randsac.hpp b/pipeline-line-fitting/include/pipeline_line_fitting/randsac.hpp index 76a5c0b..d9cca4a 100644 --- a/pipeline-line-fitting/include/pipeline_line_fitting/randsac.hpp +++ b/pipeline-line-fitting/include/pipeline_line_fitting/randsac.hpp @@ -5,75 +5,76 @@ using namespace std; using namespace arma; -struct Line{ - double slope; - double intercept; - double score; - bool vertical; - cv::Point start; - cv::Point end; +struct Line { + double slope; + double intercept; + double score; + bool vertical; + cv::Point start; + cv::Point end; }; mat squareErrorLoss(const mat &y_true, const mat &y_pred); double value(const mat &inliersVal); -using LossFunction = mat(*)(const mat&, const mat&); -using MetricFunction = double(*)(const mat&); +using LossFunction = mat (*)(const mat &, const mat &); +using MetricFunction = double (*)(const mat &); -struct LinearRegressor{ - vector params; - bool vertical; - LinearRegressor(); - ~LinearRegressor(); - void fit(const mat &X, const mat &y); - mat predict(const mat &X); +struct LinearRegressor { + vector params; + bool vertical; + LinearRegressor(); + ~LinearRegressor(); + void fit(const mat &X, const mat &y); + mat predict(const mat &X); }; -class RANDSAC{ - int n; - int k; - float t; - float remT; - float turnAngle; - LinearRegressor model; - LossFunction loss; - MetricFunction metric; - mat points; - bool failed; +class RANDSAC { + int n; + int k; + float t; + float remT; + float turnAngle; + LinearRegressor model; + LossFunction loss; + MetricFunction metric; + mat points; + bool failed; + void _reset(); + bool legalAlpha(double alpha, const vector &lines); +public: + double bestScore; + LinearRegressor bestFit; - void _reset(); - bool legalAlpha(double alpha, const vector &lines); - public: + vector orgPoints; + vector rempointids; - double bestScore; - LinearRegressor bestFit; + int d; - vector orgPoints; - vector rempointids; + RANDSAC() {}; + RANDSAC(int n, int k, float t, int d, float remT, float finalScorethresh, + float turnAngle, LinearRegressor model = LinearRegressor(), + LossFunction loss = squareErrorLoss, MetricFunction metric = value) + : n(n), k(k), t(t), d(d), remT(remT), turnAngle(turnAngle), loss(loss), + metric(metric), model(model) { + /* + n: Minimum number of data points to estimate parameters + k: Maximum iterations allowed + t: Threshold value to determine if points are fit well + d: Number of close data points required to assert model fits well + remT: Threshold value to determine if what points should be removed + model: class implementing `fit` and `predict` + loss: function of `y_true` and `y_pred` that returns a vector, this is the + basis for the threshold metric: function of inliers that returns a scalar, + larger is better + */ + _reset(); + } - int d; - - RANDSAC(){}; - RANDSAC( - int n, int k, float t, int d, float remT, float finalScorethresh, float turnAngle, - LinearRegressor model = LinearRegressor(), LossFunction loss = squareErrorLoss, MetricFunction metric = value) - : n(n), k(k), t(t), d(d), remT(remT), turnAngle(turnAngle), loss(loss), metric(metric), model(model){ - /* - n: Minimum number of data points to estimate parameters - k: Maximum iterations allowed - t: Threshold value to determine if points are fit well - d: Number of close data points required to assert model fits well - remT: Threshold value to determine if what points should be removed - model: class implementing `fit` and `predict` - loss: function of `y_true` and `y_pred` that returns a vector, this is the basis for the threshold - metric: function of inliers that returns a scalar, larger is better - */ - _reset(); - } - - ~RANDSAC(); - mat predict(const mat &X); - void fit (const mat &X, const mat &y, const mat &values, const vector &lines); -}; \ No newline at end of file + ~RANDSAC(); + mat predict(const mat &X); + void fit(const mat &X, const mat &y, const mat &values, + const vector &lines); +}; diff --git a/pipeline-line-fitting/launch/pipeline_line_fitting.launch.py b/pipeline-line-fitting/launch/pipeline_line_fitting.launch.py index 79eb2cf..9d87c85 100644 --- a/pipeline-line-fitting/launch/pipeline_line_fitting.launch.py +++ b/pipeline-line-fitting/launch/pipeline_line_fitting.launch.py @@ -1,16 +1,22 @@ import os + from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch_ros.actions import Node + def generate_launch_description(): pipeline_line_fitting_node = Node( - package='pipeline_line_fitting', - executable='pipeline_line_fitting_node', - name='pipeline_line_fitting_node', - parameters=[os.path.join(get_package_share_directory('pipeline_line_fitting'),'params','pipeline_line_fitting_params.yaml')], - output='screen', - ) - return LaunchDescription([ - pipeline_line_fitting_node - ]) \ No newline at end of file + package="pipeline_line_fitting", + executable="pipeline_line_fitting_node", + name="pipeline_line_fitting_node", + parameters=[ + os.path.join( + get_package_share_directory("pipeline_line_fitting"), + "params", + "pipeline_line_fitting_params.yaml", + ) + ], + output="screen", + ) + return LaunchDescription([pipeline_line_fitting_node]) diff --git a/pipeline-line-fitting/package.xml b/pipeline-line-fitting/package.xml index 0408672..9a6ae26 100644 --- a/pipeline-line-fitting/package.xml +++ b/pipeline-line-fitting/package.xml @@ -18,8 +18,8 @@ cv_bridge python3-opencv armadillo - + ament_cmake - \ No newline at end of file + diff --git a/pipeline-line-fitting/src/linedetectorPipe.cpp b/pipeline-line-fitting/src/linedetectorPipe.cpp index d512242..e2f5461 100644 --- a/pipeline-line-fitting/src/linedetectorPipe.cpp +++ b/pipeline-line-fitting/src/linedetectorPipe.cpp @@ -1,271 +1,284 @@ +#include +#include #include -#include #include -#include -#include +#include using namespace std; - -void thinningIteration(cv::Mat& img, int iter) { - cv::Mat marker = cv::Mat::zeros(img.size(), CV_8UC1); - - for (int i = 1; i < img.rows - 1; i++) { - for (int j = 1; j < img.cols - 1; j++) { - uchar p2 = img.at(i - 1, j); - uchar p3 = img.at(i - 1, j + 1); - uchar p4 = img.at(i, j + 1); - uchar p5 = img.at(i + 1, j + 1); - uchar p6 = img.at(i + 1, j); - uchar p7 = img.at(i + 1, j - 1); - uchar p8 = img.at(i, j - 1); - uchar p9 = img.at(i - 1, j - 1); - - int A = (p2 == 0 && p3 == 1) + (p3 == 0 && p4 == 1) + - (p4 == 0 && p5 == 1) + (p5 == 0 && p6 == 1) + - (p6 == 0 && p7 == 1) + (p7 == 0 && p8 == 1) + - (p8 == 0 && p9 == 1) + (p9 == 0 && p2 == 1); - int B = p2 + p3 + p4 + p5 + p6 + p7 + p8 + p9; - int m1 = iter == 0 ? (p2 * p4 * p6) : (p2 * p4 * p8); - int m2 = iter == 0 ? (p4 * p6 * p8) : (p2 * p6 * p8); - - if (A == 1 && (B >= 2 && B <= 6) && m1 == 0 && m2 == 0) { - marker.at(i, j) = 1; - } - } +void thinningIteration(cv::Mat &img, int iter) { + cv::Mat marker = cv::Mat::zeros(img.size(), CV_8UC1); + + for (int i = 1; i < img.rows - 1; i++) { + for (int j = 1; j < img.cols - 1; j++) { + uchar p2 = img.at(i - 1, j); + uchar p3 = img.at(i - 1, j + 1); + uchar p4 = img.at(i, j + 1); + uchar p5 = img.at(i + 1, j + 1); + uchar p6 = img.at(i + 1, j); + uchar p7 = img.at(i + 1, j - 1); + uchar p8 = img.at(i, j - 1); + uchar p9 = img.at(i - 1, j - 1); + + int A = (p2 == 0 && p3 == 1) + (p3 == 0 && p4 == 1) + + (p4 == 0 && p5 == 1) + (p5 == 0 && p6 == 1) + + (p6 == 0 && p7 == 1) + (p7 == 0 && p8 == 1) + + (p8 == 0 && p9 == 1) + (p9 == 0 && p2 == 1); + int B = p2 + p3 + p4 + p5 + p6 + p7 + p8 + p9; + int m1 = iter == 0 ? (p2 * p4 * p6) : (p2 * p4 * p8); + int m2 = iter == 0 ? (p4 * p6 * p8) : (p2 * p6 * p8); + + if (A == 1 && (B >= 2 && B <= 6) && m1 == 0 && m2 == 0) { + marker.at(i, j) = 1; + } } + } - img &= ~marker; + img &= ~marker; } -void thinning(cv::Mat& img) { - img /= 255; +void thinning(cv::Mat &img) { + img /= 255; - cv::Mat prev = cv::Mat::zeros(img.size(), CV_8UC1); - cv::Mat diff; + cv::Mat prev = cv::Mat::zeros(img.size(), CV_8UC1); + cv::Mat diff; - do { - thinningIteration(img, 0); - thinningIteration(img, 1); - cv::absdiff(img, prev, diff); - img.copyTo(prev); - } while (cv::countNonZero(diff) > 0); + do { + thinningIteration(img, 0); + thinningIteration(img, 1); + cv::absdiff(img, prev, diff); + img.copyTo(prev); + } while (cv::countNonZero(diff) > 0); - img *= 255; + img *= 255; } -void removeBorderArtifacts(cv::Mat& img) { - img.row(0).setTo(cv::Scalar(0)); - img.row(img.rows - 1).setTo(cv::Scalar(0)); - img.col(0).setTo(cv::Scalar(0)); - img.col(img.cols - 1).setTo(cv::Scalar(0)); +void removeBorderArtifacts(cv::Mat &img) { + img.row(0).setTo(cv::Scalar(0)); + img.row(img.rows - 1).setTo(cv::Scalar(0)); + img.col(0).setTo(cv::Scalar(0)); + img.col(img.cols - 1).setTo(cv::Scalar(0)); } -void LinedetectorPipe::_preprocess(cv::Mat& img, bool dist) { - // Calculate scaling factors - scaleX = static_cast(size) / img.cols; - scaleY = static_cast(size) / img.rows; +void LinedetectorPipe::_preprocess(cv::Mat &img, bool dist) { + // Calculate scaling factors + scaleX = static_cast(size) / img.cols; + scaleY = static_cast(size) / img.rows; + cv::resize(img, img, cv::Size(size, size)); - cv::resize(img, img, cv::Size(size, size)); + // Apply distance transform to get the center lines + if (dist) { + cv::Mat dist_img; + cv::distanceTransform(img, dist_img, cv::DIST_L2, 5); + cv::normalize(dist_img, dist_img, 0, 1.0, cv::NORM_MINMAX); - // Apply distance transform to get the center lines - if (dist) { - cv::Mat dist_img; - cv::distanceTransform(img, dist_img, cv::DIST_L2, 5); - cv::normalize(dist_img, dist_img, 0, 1.0, cv::NORM_MINMAX); - - // Threshold the distance transform image to get the skeleton - cv::threshold(dist_img, img, 0.2, 1.0, cv::THRESH_BINARY); - } + // Threshold the distance transform image to get the skeleton + cv::threshold(dist_img, img, 0.2, 1.0, cv::THRESH_BINARY); + } - // Convert the image to 8-bit - img.convertTo(img, CV_8U, 255); + // Convert the image to 8-bit + img.convertTo(img, CV_8U, 255); - // Apply morphological operations to clean up the result - cv::Mat kernel = cv::getStructuringElement(cv::MORPH_RECT, cv::Size(3, 3)); - cv::morphologyEx(img, img, cv::MORPH_CLOSE, kernel); + // Apply morphological operations to clean up the result + cv::Mat kernel = cv::getStructuringElement(cv::MORPH_RECT, cv::Size(3, 3)); + cv::morphologyEx(img, img, cv::MORPH_CLOSE, kernel); - // Skeletonize the image using Zhang-Suen thinning algorithm - thinning(img); + // Skeletonize the image using Zhang-Suen thinning algorithm + thinning(img); - removeBorderArtifacts(img); + removeBorderArtifacts(img); } -LinedetectorPipe::~LinedetectorPipe(){ - // Destructor +LinedetectorPipe::~LinedetectorPipe() { + // Destructor } void LinedetectorPipe::_postprocess() { - // Postprocess + // Postprocess } -int LinedetectorPipe::detectSingleLine(const mat &points, const mat &values, const vector &lines, const int i) { - // Extract columns and reshape - if (points.n_rows < 5) { - return 1; - } - - mat X = points.col(1); - X.reshape(points.n_rows, 1); - mat y = points.col(0); - y.reshape(points.n_rows, 1); - - // Set the d parameter for RANSAC - int d = points.n_elem * fracOfPoints; - randsac.d = d; - - // Fit the RANSAC model - randsac.fit(X, y, values, lines); - - // Check the best_fit and bestValue conditions - if (randsac.bestFit.params.size() == 0 || randsac.bestScore < finalScorethresh) { - return 1; - } - cout << "Found line " << i+1 << " with score: " << randsac.bestScore << endl; - - return 0; +int LinedetectorPipe::detectSingleLine(const mat &points, const mat &values, + const vector &lines, const int i) { + // Extract columns and reshape + if (points.n_rows < 5) { + return 1; + } + + mat X = points.col(1); + X.reshape(points.n_rows, 1); + mat y = points.col(0); + y.reshape(points.n_rows, 1); + + // Set the d parameter for RANSAC + int d = points.n_elem * fracOfPoints; + randsac.d = d; + + // Fit the RANSAC model + randsac.fit(X, y, values, lines); + + // Check the best_fit and bestValue conditions + if (randsac.bestFit.params.size() == 0 || + randsac.bestScore < finalScorethresh) { + return 1; + } + cout << "Found line " << i + 1 << " with score: " << randsac.bestScore + << endl; + + return 0; } void LinedetectorPipe::_getEndPoints(Line &line, bool swap) { - int min_x = -1; - int max_x = -1; - int min_x_yval; - int max_x_yval; - for (double x = 0; x < size; x += 0.05) { - int y = line.slope * x + line.intercept; - - if (y < 0 || y >= size) { - continue; - } - int pixel; - if (swap) { - pixel = orgImg.at(x, y); - } - else{ - pixel = orgImg.at(y, x); - } - if (pixel > 0) { - if (min_x == -1) { - min_x = x; - min_x_yval = y; - } - if (x > max_x) { - max_x = x; - max_x_yval = y; - } - } + int min_x = -1; + int max_x = -1; + int min_x_yval; + int max_x_yval; + for (double x = 0; x < size; x += 0.05) { + int y = line.slope * x + line.intercept; + + if (y < 0 || y >= size) { + continue; } - - if (swap){ - line.start = cv::Point(static_cast(min_x_yval / scaleX), static_cast(min_x / scaleY)); - line.end = cv::Point(static_cast(max_x_yval / scaleX), static_cast(max_x / scaleY)); + int pixel; + if (swap) { + pixel = orgImg.at(x, y); + } else { + pixel = orgImg.at(y, x); } - else{ - line.start = cv::Point(static_cast(min_x / scaleX), static_cast(min_x_yval / scaleY)); - line.end = cv::Point(static_cast(max_x / scaleX), static_cast(max_x_yval / scaleY)); + if (pixel > 0) { + if (min_x == -1) { + min_x = x; + min_x_yval = y; + } + if (x > max_x) { + max_x = x; + max_x_yval = y; + } } + } + + if (swap) { + line.start = cv::Point(static_cast(min_x_yval / scaleX), + static_cast(min_x / scaleY)); + line.end = cv::Point(static_cast(max_x_yval / scaleX), + static_cast(max_x / scaleY)); + } else { + line.start = cv::Point(static_cast(min_x / scaleX), + static_cast(min_x_yval / scaleY)); + line.end = cv::Point(static_cast(max_x / scaleX), + static_cast(max_x_yval / scaleY)); + } } -vector LinedetectorPipe::operator()(const cv::Mat &img, const int maxLines=3){ - orgImg = img.clone(); - cv::resize(orgImg, orgImg, cv::Size(size, size)); - processedImg = img.clone(); - _preprocess(processedImg); - - // Find points where img > 0 - vector pointList; - cv::findNonZero(processedImg, pointList); - - // Convert points to arma::mat - mat points(pointList.size(), 2); - for (size_t i = 0; i < pointList.size(); ++i) { - points(i, 0) = pointList[i].y; - points(i, 1) = pointList[i].x; +vector LinedetectorPipe::operator()(const cv::Mat &img, + const int maxLines = 3) { + orgImg = img.clone(); + cv::resize(orgImg, orgImg, cv::Size(size, size)); + processedImg = img.clone(); + _preprocess(processedImg); + + // Find points where img > 0 + vector pointList; + cv::findNonZero(processedImg, pointList); + + // Convert points to arma::mat + mat points(pointList.size(), 2); + for (size_t i = 0; i < pointList.size(); ++i) { + points(i, 0) = pointList[i].y; + points(i, 1) = pointList[i].x; + } + + // Extract values from the image at the points + mat values(pointList.size(), 1); + for (size_t i = 0; i < pointList.size(); ++i) { + values(i, 0) = processedImg.at(pointList[i].y, pointList[i].x); + } + + // function to rotate points 45 degrees + mat rotate45 = {{0.7071, -0.7071}, {0.7071, 0.7071}}; + + vector lines; + + for (int i = 0; i < maxLines; ++i) { + int returnCode = detectSingleLine(points, values, lines, i); + Line line; + + if (returnCode) { + cout << "RANSAC failed to find line number " << i + 1 << endl; + // rotate points and retry + mat newPoints(points.n_cols, points.n_rows); + for (size_t j = 0; j < points.n_rows; ++j) { + newPoints(0, j) = points(j, 1); + newPoints(1, j) = points(j, 0); + } + + newPoints = newPoints.t(); + + returnCode = detectSingleLine(newPoints, values, lines, i); + + if (returnCode) { + cout << "RANSAC failed to find line number " << i + 1 << " again" + << endl; + continue; + } + + line = Line{randsac.bestFit.params[1], + randsac.bestFit.params[0], + randsac.bestScore, + randsac.bestFit.vertical, + {0, 0}, + {0, 0}}; + // use a rotated image to get end points also + _getEndPoints(line, true); + // rotate back end points + // line.end = cv::Point(line.end.y, line.end.x); + // line.start = cv::Point(line.start.y, line.start.x); + cout << "Line " << i + 1 << " rotated ---------------------------" + << endl; + + } else { + line = Line{randsac.bestFit.params[1], + randsac.bestFit.params[0], + randsac.bestScore, + randsac.bestFit.vertical, + {0, 0}, + {0, 0}}; + _getEndPoints(line); } - // Extract values from the image at the points - mat values(pointList.size(), 1); - for (size_t i = 0; i < pointList.size(); ++i) { - values(i, 0) = processedImg.at(pointList[i].y, pointList[i].x); + // Remove points for next iteration + mat newPoints; + mat newValues; + for (size_t j = 0; j < points.n_rows; ++j) { + if (find(randsac.rempointids.begin(), randsac.rempointids.end(), j) == + randsac.rempointids.end()) { + newPoints.insert_rows(newPoints.n_rows, points.row(j)); + newValues.insert_rows(newValues.n_rows, values.row(j)); + } } + points = newPoints; + values = newValues; - // function to rotate points 45 degrees - mat rotate45 = {{0.7071, -0.7071}, {0.7071, 0.7071}}; + lines.push_back(line); + } - vector lines; - - for (int i = 0; i < maxLines; ++i) { - int returnCode = detectSingleLine(points, values, lines, i); - Line line; - - if (returnCode) { - cout << "RANSAC failed to find line number " << i + 1 << endl; - //rotate points and retry - mat newPoints(points.n_cols, points.n_rows); - for (size_t j = 0; j < points.n_rows; ++j) { - newPoints(0, j) = points(j, 1); - newPoints(1, j) = points(j, 0); - } - - newPoints = newPoints.t(); - - - returnCode = detectSingleLine(newPoints, values, lines, i); - - if (returnCode) { - cout << "RANSAC failed to find line number " << i + 1 << " again" << endl; - continue; - } - - line = Line{randsac.bestFit.params[1], randsac.bestFit.params[0], randsac.bestScore, randsac.bestFit.vertical, {0, 0}, {0, 0}}; - //use a rotated image to get end points also - _getEndPoints(line, true); - //roate back end points - //line.end = cv::Point(line.end.y, line.end.x); - //line.start = cv::Point(line.start.y, line.start.x); - cout << "Line " << i+1 << " rotated ---------------------------" << endl; - - } - else{ - line = Line{randsac.bestFit.params[1], randsac.bestFit.params[0], randsac.bestScore, randsac.bestFit.vertical, {0, 0}, {0, 0}}; - _getEndPoints(line); - - } - - - // Remove points for next iteration - mat newPoints; - mat newValues; - for (size_t j = 0; j < points.n_rows; ++j) { - if (find(randsac.rempointids.begin(), randsac.rempointids.end(), j) == randsac.rempointids.end()) { - newPoints.insert_rows(newPoints.n_rows, points.row(j)); - newValues.insert_rows(newValues.n_rows, values.row(j)); - } - } - points = newPoints; - values = newValues; - - - lines.push_back(line); - } - - return lines; + return lines; } -cv::Mat LinedetectorPipe::drawResults(const cv::Mat &img, const vector &lines){ - // Draw the lines - cv::Mat img2 = img.clone(); +cv::Mat LinedetectorPipe::drawResults(const cv::Mat &img, + const vector &lines) { + // Draw the lines + cv::Mat img2 = img.clone(); - _preprocess(img2); - cv::resize(img2, img2, cv::Size(size, size)); - img2.convertTo(img2, CV_8U); - - cv::cvtColor(img2, img2, cv::COLOR_GRAY2BGR); + _preprocess(img2); + cv::resize(img2, img2, cv::Size(size, size)); + img2.convertTo(img2, CV_8U); - for (int i = 0; i < lines.size(); i += 1) { - // Scale line endpoints back to original image coordinates if necessary - cv::line(img2, lines[i].start, lines[i].end, cv::Scalar(255, 0, 255), 2); - } - return img2; + cv::cvtColor(img2, img2, cv::COLOR_GRAY2BGR); + + for (int i = 0; i < lines.size(); i += 1) { + // Scale line endpoints back to original image coordinates if necessary + cv::line(img2, lines[i].start, lines[i].end, cv::Scalar(255, 0, 255), 2); + } + return img2; } diff --git a/pipeline-line-fitting/src/pipeline_line_fitting_ros.cpp b/pipeline-line-fitting/src/pipeline_line_fitting_ros.cpp index 77c13e4..474fc03 100644 --- a/pipeline-line-fitting/src/pipeline_line_fitting_ros.cpp +++ b/pipeline-line-fitting/src/pipeline_line_fitting_ros.cpp @@ -2,90 +2,100 @@ using std::placeholders::_1; -PipelineLineFittingNode::PipelineLineFittingNode(const rclcpp::NodeOptions & options) : Node("pipeline_line_fitting_node", options) -{ - auto qos_profile = rclcpp::QoS(rclcpp::KeepLast(1)).reliability(RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT); - - auto image_sub_topic = this->declare_parameter("image_sub_topic", "/cam_down/image_color"); - auto image_visualization_pub_topic = this->declare_parameter("image_visualization_pub_topic", "/linefitting/visualization"); - auto pose_array_pub_topic = this->declare_parameter("pose_array_pub_topic", "/linefitting/pose_array"); - - image_sub_ = this->create_subscription(image_sub_topic, qos_profile, std::bind(&PipelineLineFittingNode::image_callback, this, _1)); - - publish_visualization_ = this->declare_parameter("publish_visualization", true); - if (publish_visualization_){ - image_visualization_pub_ = this->create_publisher(image_visualization_pub_topic, qos_profile); - } - pose_array_pub_ = this->create_publisher(pose_array_pub_topic, qos_profile); - - - pipeline = LinedetectorPipe(); +PipelineLineFittingNode::PipelineLineFittingNode( + const rclcpp::NodeOptions &options) + : Node("pipeline_line_fitting_node", options) { + auto qos_profile = rclcpp::QoS(rclcpp::KeepLast(1)) + .reliability(RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT); + + auto image_sub_topic = this->declare_parameter( + "image_sub_topic", "/cam_down/image_color"); + auto image_visualization_pub_topic = this->declare_parameter( + "image_visualization_pub_topic", "/linefitting/visualization"); + auto pose_array_pub_topic = this->declare_parameter( + "pose_array_pub_topic", "/linefitting/pose_array"); + + image_sub_ = this->create_subscription( + image_sub_topic, qos_profile, + std::bind(&PipelineLineFittingNode::image_callback, this, _1)); + + publish_visualization_ = + this->declare_parameter("publish_visualization", true); + if (publish_visualization_) { + image_visualization_pub_ = this->create_publisher( + image_visualization_pub_topic, qos_profile); + } + pose_array_pub_ = this->create_publisher( + pose_array_pub_topic, qos_profile); + + pipeline = LinedetectorPipe(); } -void PipelineLineFittingNode::image_callback(const sensor_msgs::msg::Image::SharedPtr msg) -{ - cv::Mat img; - try { - img = cv_bridge::toCvCopy(msg, "mono8")->image; - } catch (cv_bridge::Exception& e) { - RCLCPP_ERROR(this->get_logger(), "cv_bridge exception: %s", e.what()); - return; - } - - vector lines = pipeline(img, 2); - auto message = geometry_msgs::msg::PoseArray(); - message.header = msg->header; - - for (const auto& line : lines) { - geometry_msgs::msg::Pose start_pose; - start_pose.position.x = line.start.x; - start_pose.position.y = line.start.y; - start_pose.position.z = 0.0; - start_pose.orientation.x = 0.0; - start_pose.orientation.y = 0.0; - start_pose.orientation.z = 0.0; - start_pose.orientation.w = 1.0; - message.poses.push_back(start_pose); - - geometry_msgs::msg::Pose end_pose; - end_pose.position.x = line.end.x; - end_pose.position.y = line.end.y; - end_pose.position.z = 0.0; - end_pose.orientation.x = 0.0; - end_pose.orientation.y = 0.0; - end_pose.orientation.z = 0.0; - end_pose.orientation.w = 1.0; - message.poses.push_back(end_pose); - } - - pose_array_pub_->publish(message); - - if (publish_visualization_){ - auto img_color = draw_lines(img, lines); - auto output_image = cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", img_color).toImageMsg(); - output_image->header = msg->header; - image_visualization_pub_->publish(*output_image); - } +void PipelineLineFittingNode::image_callback( + const sensor_msgs::msg::Image::SharedPtr msg) { + cv::Mat img; + try { + img = cv_bridge::toCvCopy(msg, "mono8")->image; + } catch (cv_bridge::Exception &e) { + RCLCPP_ERROR(this->get_logger(), "cv_bridge exception: %s", e.what()); + return; + } + + vector lines = pipeline(img, 2); + auto message = geometry_msgs::msg::PoseArray(); + message.header = msg->header; + + for (const auto &line : lines) { + geometry_msgs::msg::Pose start_pose; + start_pose.position.x = line.start.x; + start_pose.position.y = line.start.y; + start_pose.position.z = 0.0; + start_pose.orientation.x = 0.0; + start_pose.orientation.y = 0.0; + start_pose.orientation.z = 0.0; + start_pose.orientation.w = 1.0; + message.poses.push_back(start_pose); + + geometry_msgs::msg::Pose end_pose; + end_pose.position.x = line.end.x; + end_pose.position.y = line.end.y; + end_pose.position.z = 0.0; + end_pose.orientation.x = 0.0; + end_pose.orientation.y = 0.0; + end_pose.orientation.z = 0.0; + end_pose.orientation.w = 1.0; + message.poses.push_back(end_pose); + } + + pose_array_pub_->publish(message); + + if (publish_visualization_) { + auto img_color = draw_lines(img, lines); + auto output_image = + cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", img_color) + .toImageMsg(); + output_image->header = msg->header; + image_visualization_pub_->publish(*output_image); + } } -cv::Mat PipelineLineFittingNode::draw_lines(cv::Mat &image, const vector &lines) -{ - cv::Mat img_color; - //intregrate with the size parameter - //pipeline._preprocess(image); - cv::cvtColor(image, img_color, cv::COLOR_GRAY2BGR); +cv::Mat PipelineLineFittingNode::draw_lines(cv::Mat &image, + const vector &lines) { + cv::Mat img_color; + // integrate with the size parameter + // pipeline._preprocess(image); + cv::cvtColor(image, img_color, cv::COLOR_GRAY2BGR); - cv::Mat img_with_lines = img_color.clone(); + cv::Mat img_with_lines = img_color.clone(); - for (const auto& line : lines) { - cv::Point start(line.start.x, line.start.y); - cv::Point end(line.end.x, line.end.y); - cv::line(img_color, start, end, cv::Scalar(255, 0, 255), 4); - } - cv::addWeighted(img_with_lines, 0.5, img_color, 0.5, 0, img_color); + for (const auto &line : lines) { + cv::Point start(line.start.x, line.start.y); + cv::Point end(line.end.x, line.end.y); + cv::line(img_color, start, end, cv::Scalar(255, 0, 255), 4); + } + cv::addWeighted(img_with_lines, 0.5, img_color, 0.5, 0, img_color); - return img_color; + return img_color; } - -RCLCPP_COMPONENTS_REGISTER_NODE(PipelineLineFittingNode) \ No newline at end of file +RCLCPP_COMPONENTS_REGISTER_NODE(PipelineLineFittingNode) diff --git a/pipeline-line-fitting/src/randsac.cpp b/pipeline-line-fitting/src/randsac.cpp index 29d1118..ba74407 100644 --- a/pipeline-line-fitting/src/randsac.cpp +++ b/pipeline-line-fitting/src/randsac.cpp @@ -5,175 +5,178 @@ using namespace std; using namespace arma; -//Utils -mat squareErrorLoss(const mat &y_true, const mat &y_pred){ - return square(y_true - y_pred); -} - -double value(const mat &inliersVal){ - return inliersVal.size(); +// Utils +mat squareErrorLoss(const mat &y_true, const mat &y_pred) { + return square(y_true - y_pred); } +double value(const mat &inliersVal) { return inliersVal.size(); } -//LinearRegressor -LinearRegressor::LinearRegressor(){ - params = {}; -} +// LinearRegressor +LinearRegressor::LinearRegressor() { params = {}; } -LinearRegressor::~LinearRegressor(){ - params.clear(); -} +LinearRegressor::~LinearRegressor() { params.clear(); } -void LinearRegressor::fit(const mat &X, const mat &y){ - //make sure to account for matrix not invertible error - mat X_ = join_horiz(ones(X.n_rows, 1), X); +void LinearRegressor::fit(const mat &X, const mat &y) { + // make sure to account for matrix not invertible error + mat X_ = join_horiz(ones(X.n_rows, 1), X); - mat XtX = X_.t() * X_; - //double cond_number = cond(XtX); + mat XtX = X_.t() * X_; + // double cond_number = cond(XtX); - if (X.min() == X.max() && X.size() >= 5) { // Threshold for condition number - // Handle the case where the matrix is not invertible - // For example, you can set params to a default value or use a regularization method - vertical = true; - params = {X.at(0), 0}; + if (X.min() == X.max() && X.size() >= 5) { // Threshold for condition number + // Handle the case where the matrix is not invertible + // For example, you can set params to a default value or use a + // regularization method + vertical = true; + params = {X.at(0), 0}; - } else { - // Solve the normal equation - params = conv_to>::from(solve(XtX, X_.t() * y)); - vertical = false; - } - //params = conv_to>::from(solve(X_, y)); + } else { + // Solve the normal equation + params = conv_to>::from(solve(XtX, X_.t() * y)); + vertical = false; + } + // params = conv_to>::from(solve(X_, y)); } -mat LinearRegressor::predict(const mat &X){ - if (vertical){ - return ones(X.n_rows) * params.at(0); // the vertical line will return x-predections, instead of y-predictions - } +mat LinearRegressor::predict(const mat &X) { + if (vertical) { + return ones(X.n_rows) * + params.at(0); // the vertical line will return x-predections, instead + // of y-predictions + } - mat X_ = join_horiz(ones(X.n_rows, 1), X); - return X_ * conv_to::from(params); + mat X_ = join_horiz(ones(X.n_rows, 1), X); + return X_ * conv_to::from(params); } -//RANDSAC -void RANDSAC::_reset(){ - bestScore = -999999; - failed = false; +// RANDSAC +void RANDSAC::_reset() { + bestScore = -999999; + failed = false; } -RANDSAC::~RANDSAC(){ - _reset(); -} +RANDSAC::~RANDSAC() { _reset(); } -bool RANDSAC::legalAlpha(double alpha, const vector &lines){ - for (int i = 0; i < lines.size(); i++){ - if (abs(atan(lines[i].slope) - atan(alpha)) < turnAngle){ - return false; - } +bool RANDSAC::legalAlpha(double alpha, const vector &lines) { + for (int i = 0; i < lines.size(); i++) { + if (abs(atan(lines[i].slope) - atan(alpha)) < turnAngle) { + return false; } - return true; -} -mat RANDSAC::predict(const mat &X){ - return bestFit.predict(X); + } + return true; } +mat RANDSAC::predict(const mat &X) { return bestFit.predict(X); } + +void RANDSAC::fit(const mat &X, const mat &y, const mat &values, + const vector &lines) { + _reset(); + + int verticals = 0; + int vertScore = 0; + int others = 0; + int otherScore = 0; + + random_device rd; + mt19937 rng(rd()); + vector ids(X.n_rows); + iota(ids.begin(), ids.end(), 0); + mat y_pred; + mat x_pred; + umat thresholded; + + for (int i = 0; i < k; i++) { + shuffle(ids.begin(), ids.end(), rng); + + vector maybe_inliers(ids.begin(), ids.begin() + n); + uvec maybeInliersUvec = conv_to::from(maybe_inliers); + + LinearRegressor maybe_model = model; + maybe_model.fit(X.rows(maybeInliersUvec), y.rows(maybeInliersUvec)); + + y_pred.clear(); + x_pred.clear(); + thresholded.clear(); + + if (maybe_model.vertical && false) { + verticals++; + // in case of a vertical line, the dimensions must be swapped + thresholded.clear(); + + x_pred = maybe_model.predict( + y.rows(conv_to::from(vector(ids.begin() + n, ids.end())))); + thresholded = loss(X.rows(conv_to::from( + vector(ids.begin() + n, ids.end()))), + x_pred) < t; + } else { + others++; + y_pred = maybe_model.predict( + X.rows(conv_to::from(vector(ids.begin() + n, ids.end())))); + thresholded = loss(y.rows(conv_to::from( + vector(ids.begin() + n, ids.end()))), + y_pred) < t; + } -void RANDSAC::fit (const mat &X, const mat &y, const mat &values, const vector &lines){ - _reset(); - - int verticals = 0; - int vertScore = 0; - int others = 0; - int otherScore = 0; - - random_device rd; - mt19937 rng(rd()); - vector ids(X.n_rows); - iota(ids.begin(), ids.end(), 0); - mat y_pred; - mat x_pred; - umat thresholded; - - for (int i = 0; i < k; i++) { - shuffle(ids.begin(), ids.end(), rng); - - vector maybe_inliers(ids.begin(), ids.begin() + n); - uvec maybeInliersUvec = conv_to::from(maybe_inliers); - - LinearRegressor maybe_model = model; - maybe_model.fit(X.rows(maybeInliersUvec), y.rows(maybeInliersUvec)); - - y_pred.clear(); - x_pred.clear(); - thresholded.clear(); - - - if (maybe_model.vertical && false){ - verticals++; - //in case of a vertical line, the dimensions must be swapped - thresholded.clear(); - - x_pred = maybe_model.predict(y.rows(conv_to::from(vector(ids.begin() + n, ids.end())))); - thresholded = loss(X.rows(conv_to::from(vector(ids.begin() + n, ids.end()))), x_pred) < t; - } - else{ - others++; - y_pred = maybe_model.predict(X.rows(conv_to::from(vector(ids.begin() + n, ids.end())))); - thresholded = loss(y.rows(conv_to::from(vector(ids.begin() + n, ids.end()))), y_pred) < t; + vector inlier_ids; + for (size_t j = n; j < ids.size(); j++) { + if (thresholded(j - n)) { + inlier_ids.push_back(ids[j]); + } + } + // if (maybe_model.vertical){cout << "inlier_ids: " << inlier_ids.size() << + // " " << maybe_model.params.at(0) << endl;} + + if (inlier_ids.size() > 10 && legalAlpha(maybe_model.params[1], lines)) { + failed = false; + + vector inlier_points = maybe_inliers; + uvec inlierIdsUvec = conv_to::from(inlier_ids); + inlier_points.insert(inlier_points.end(), inlier_ids.begin(), + inlier_ids.end()); + + LinearRegressor better_model = model; + better_model.fit(X.rows(inlierIdsUvec), y.rows(inlierIdsUvec)); + + double thisValue = metric(values.rows(inlierIdsUvec)); + + if (thisValue > bestScore) { + + orgPoints = maybe_inliers; + + // find points that are inliers to remove + umat rem_thresholded; + mat rem_y_pred = better_model.predict(X.rows( + conv_to::from(vector(ids.begin() + n, ids.end())))); + mat rem_x_pred; + rem_thresholded = loss(y.rows(conv_to::from( + vector(ids.begin() + n, ids.end()))), + rem_y_pred) < remT; + + if (better_model.vertical) { + vertScore++; + rem_x_pred = better_model.predict(y.rows( + conv_to::from(vector(ids.begin() + n, ids.end())))); + rem_thresholded = loss(X.rows(conv_to::from( + vector(ids.begin() + n, ids.end()))), + rem_x_pred) < remT; + } else { + otherScore++; } - - vector inlier_ids; + rempointids.clear(); for (size_t j = n; j < ids.size(); j++) { - if (thresholded(j - n)) { - inlier_ids.push_back(ids[j]); - } - } - //if (maybe_model.vertical){cout << "inlier_ids: " << inlier_ids.size() << " " << maybe_model.params.at(0) << endl;} - - if (inlier_ids.size() > 10 && legalAlpha(maybe_model.params[1], lines)) { - failed = false; - - vector inlier_points = maybe_inliers; - uvec inlierIdsUvec = conv_to::from(inlier_ids); - inlier_points.insert(inlier_points.end(), inlier_ids.begin(), inlier_ids.end()); - - LinearRegressor better_model = model; - better_model.fit(X.rows(inlierIdsUvec), y.rows(inlierIdsUvec)); - - - double thisValue = metric(values.rows(inlierIdsUvec)); - - if (thisValue > bestScore) { - - orgPoints = maybe_inliers; - - //find points that are inliers to remove - umat rem_thresholded; - mat rem_y_pred = better_model.predict(X.rows(conv_to::from(vector(ids.begin() + n, ids.end())))); - mat rem_x_pred; - rem_thresholded = loss(y.rows(conv_to::from(vector(ids.begin() + n, ids.end()))), rem_y_pred) < remT; - - if (better_model.vertical){ - vertScore++; - rem_x_pred = better_model.predict(y.rows(conv_to::from(vector(ids.begin() + n, ids.end())))); - rem_thresholded = loss(X.rows(conv_to::from(vector(ids.begin() + n, ids.end()))), rem_x_pred) < remT; - } - else{ - otherScore++; - } - - rempointids.clear(); - for (size_t j = n; j < ids.size(); j++) { - if (rem_thresholded(j - n)) { - rempointids.push_back(ids[j]); - } - } - - bestScore = thisValue; - bestFit = better_model; - } + if (rem_thresholded(j - n)) { + rempointids.push_back(ids[j]); + } } + + bestScore = thisValue; + bestFit = better_model; + } } - /* - cout << "Verticals: " << verticals << " Others: " << others << endl; - cout << "Verticals with score: " << vertScore << " Others with score: " << otherScore << endl;*/ + } + /* + cout << "Verticals: " << verticals << " Others: " << others << endl; + cout << "Verticals with score: " << vertScore << " Others with score: " << + otherScore << endl;*/ }