Bots engaged

pipeline-line-fitting/include/pipeline_line_fitting/linedetectorPipe.hpp

@@ -2,65 +2,60 @@
 #include <iostream>
 #include <pipeline_line_fitting/randsac.hpp>
 
-//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<Line> &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<Line> operator()(const cv::Mat &img, const int maxLines);
-        cv::Mat drawResults(const cv::Mat &img, const vector<Line> &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);
-            }
-
-};
+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<Line> &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<Line> operator()(const cv::Mat &img, const int maxLines);
+  cv::Mat drawResults(const cv::Mat &img, const vector<Line> &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);
+  }
+};

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 <rclcpp/rclcpp.hpp>
-#include <geometry_msgs/msg/pose_array.hpp>
-#include <sensor_msgs/msg/image.hpp>
-#include <pipeline_line_fitting/linedetectorPipe.hpp>
 #include <cv_bridge/cv_bridge.h>
+#include <geometry_msgs/msg/pose_array.hpp>
 #include <opencv2/opencv.hpp>
+#include <pipeline_line_fitting/linedetectorPipe.hpp>
+#include <rclcpp/rclcpp.hpp>
 #include <rclcpp_components/register_node_macro.hpp>
+#include <sensor_msgs/msg/image.hpp>
 
-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<sensor_msgs::msg::Image>::SharedPtr image_sub_;
+  rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr
+      image_visualization_pub_;
+  rclcpp::Publisher<geometry_msgs::msg::PoseArray>::SharedPtr pose_array_pub_;
 
-    rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr image_sub_;
-    rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr image_visualization_pub_;   
-    rclcpp::Publisher<geometry_msgs::msg::PoseArray>::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<Line> &lines);
+  cv::Mat draw_lines(cv::Mat &image, const vector<Line> &lines);
 };
-
 
-#endif //PIPELINE_LINE_FITTING_ROS_HPP
+#endif // PIPELINE_LINE_FITTING_ROS_HPP

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<double> params;
-    bool vertical;
-    LinearRegressor();
-    ~LinearRegressor();
-    void fit(const mat &X, const mat &y);
-    mat predict(const mat &X);
+struct LinearRegressor {
+  vector<double> 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<Line> &lines);
 
+public:
+  double bestScore;
+  LinearRegressor bestFit;
 
-    void _reset();
-    bool legalAlpha(double alpha, const vector<Line> &lines);
-    public:
+  vector<int> orgPoints;
+  vector<int> rempointids;
 
-        double bestScore;
-        LinearRegressor bestFit;
+  int d;
 
-        vector<int> orgPoints;
-        vector<int> 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<Line> &lines);
-};
+  ~RANDSAC();
+  mat predict(const mat &X);
+  void fit(const mat &X, const mat &y, const mat &values,
+           const vector<Line> &lines);
+};

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
-    ])
+        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])

pipeline-line-fitting/package.xml

@@ -18,8 +18,8 @@
   <exec_depend>cv_bridge</exec_depend>
   <exec_depend>python3-opencv</exec_depend>
   <exec_depend>armadillo</exec_depend>
-  
+
   <export>
     <build_type>ament_cmake</build_type>
   </export>
-</package>
+</package>