Remove Segment object

Signed-off-by: Alberto Tudela <[email protected]>

scitos2_charging_dock/include/scitos2_charging_dock/cluster.hpp

@@ -31,6 +31,8 @@
 namespace scitos2_charging_dock
 {
 
+using Pcloud = pcl::PointCloud<pcl::PointXYZ>;
+
 /**
  * @class scitos2_charging_dock::Cluster
  * @brief Class to represent a cluster of points.
@@ -40,46 +42,91 @@ struct Cluster
   // Identifier of the cluster
   int id;
   // Original pointcloud of the cluster.
-  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
+  pcl::PointCloud<pcl::PointXYZ> cloud;
   // Pointcloud of the cluster after perform ICP.
-  pcl::PointCloud<pcl::PointXYZ>::Ptr matched_cloud;
+  pcl::PointCloud<pcl::PointXYZ> matched_cloud;
   // Score of the ICP.
   double icp_score;
   // Pose of the dock.
   geometry_msgs::msg::PoseStamped icp_pose;
 
+  /**
+   * @brief Get the size of the cluster.
+   * @return int The size of the cluster.
+   */
+  int size() const {return cloud.size();}
+
+  /**
+   * @brief Clear the cluster.
+   */
+  void clear() {cloud.clear();}
+
+  /**
+   * @brief Push a point at the end of the cluster.
+   *
+   * @param point The point to push.
+   */
+  void push_back(geometry_msgs::msg::Point point)
+  {
+    pcl::PointXYZ pcl_point;
+    pcl_point.x = point.x;
+    pcl_point.y = point.y;
+    pcl_point.z = point.z;
+    cloud.push_back(pcl_point);
+  }
+
   /**
    * @brief Get the centroid of the dock.
    *
    * @return geometry_msgs::msg::Point The centroid of the dock.
    */
-  geometry_msgs::msg::Point getCentroid()
+  geometry_msgs::msg::Point centroid() const
   {
     Eigen::Vector4f centroid_vec_4f(0, 0, 0, 0);
-    pcl::compute3DCentroid(*cloud, centroid_vec_4f);
+    pcl::compute3DCentroid(cloud, centroid_vec_4f);
     geometry_msgs::msg::Point centroid;
     centroid.x = static_cast<double>(centroid_vec_4f[0]);
     centroid.y = static_cast<double>(centroid_vec_4f[1]);
     centroid.z = static_cast<double>(centroid_vec_4f[2]);
     return centroid;
   }
 
+  /**
+   * @brief Get the length of the centroid.
+   *
+   * @return double The length of the centroid.
+   */
+  double centroid_length() const
+  {
+    auto c = centroid();
+    return sqrt(c.x * c.x + c.y * c.y);
+  }
+
   /**
    * @brief Get the width of the cluster.
    *
    * @return double The width of the cluster.
    */
   double width() const
   {
-    if (!cloud) {
+    if (cloud.empty()) {
       return 0.0;
     }
 
-    double dx = cloud->back().x - cloud->front().x;
-    double dy = cloud->back().y - cloud->front().y;
+    double dx = cloud.back().x - cloud.front().x;
+    double dy = cloud.back().y - cloud.front().y;
     return std::hypot(dx, dy);
   }
 
+  /**
+   * @brief Get the width squared of the segment.
+   * @return double The width squared of the segment.
+   */
+  double width_squared() const
+  {
+    return width() * width();
+  }
+
   /**
    * @brief Check if the cluster is valid.
    *
@@ -89,7 +136,7 @@ struct Cluster
   bool valid(double ideal_size) const
   {
     // If there are no points this cannot be valid.
-    if (!cloud) {
+    if (cloud.empty()) {
       return false;
     }
 

scitos2_charging_dock/include/scitos2_charging_dock/perception.hpp

@@ -39,9 +39,6 @@
 namespace scitos2_charging_dock
 {
 
-using Clusters = std::vector<Cluster>;
-using Pcloud = pcl::PointCloud<pcl::PointXYZ>;
-
 /**
  * @class scitos2_charging_dock::Perception
  * @brief Class to perform ICP matching on clusters to get the docking station.
@@ -100,32 +97,14 @@ class Perception
   Clusters extractClustersFromScan(const sensor_msgs::msg::LaserScan & scan);
 
 protected:
-  /**
-   * @brief Convert the segments to clusters.
-   *
-   * @param frame The frame of the segments
-   * @param segments The segments to convert
-   * @return Clusters The clusters
-   */
-  Clusters segmentsToClusters(std::string frame, const Segments & segments);
-
-  /**
-   * @brief Convert the segment to a pointcloud.
-   *
-   * @param frame The frame of the segment
-   * @param segments The segments to convert
-   * @return Pcloud The pointcloud
-   */
-  Pcloud::Ptr segmentToPcloud(std::string frame, const Segment & segment);
-
   /**
    * @brief Refine the cluster pose using Iterative Closest Point.
    *
    * @param cluster The cluster to perform ICP on
    * @param cloud_template The template to match with the cluster
    * @return bool If the ICP was successful and the template is aligned with the cluster
    */
-  bool refineClusterPose(Cluster & cluster, Pcloud::ConstPtr cloud_template);
+  bool refineClusterPose(Cluster & cluster, const Pcloud & cloud_template);
 
   /**
    * @brief Refine all the clusters poses using Iterative Closest Point to find the dock.

scitos2_charging_dock/include/scitos2_charging_dock/segmentation.hpp

@@ -24,16 +24,16 @@
 #include "rclcpp_lifecycle/lifecycle_node.hpp"
 #include "geometry_msgs/msg/point.hpp"
 #include "sensor_msgs/msg/laser_scan.hpp"
-#include "scitos2_charging_dock/segment.hpp"
+#include "scitos2_charging_dock/cluster.hpp"
 
 namespace scitos2_charging_dock
 {
 
-using Segments = std::vector<Segment>;
+using Clusters = std::vector<Cluster>;
 
 /**
  * @class scitos2_charging_dock::Segmentation
- * @brief Class to perform segmentation on the laserscan to get the segments.
+ * @brief Class to perform segmentation on the laserscan to get the clusters.
  *
  */
 class Segmentation
@@ -56,19 +56,19 @@ class Segmentation
   /**
    * @brief Perform a segmentation using a euclidean distance base clustering.
    *
-   * @param scan The laserscan to segment
-   * @param segments The segmented points
+   * @param scan The laserscan to clustering
+   * @param clusters The clusters obtained
    * @return bool If the segmentation was successful
    */
-  bool performSegmentation(const sensor_msgs::msg::LaserScan & scan, Segments & segments);
+  bool performSegmentation(const sensor_msgs::msg::LaserScan & scan, Clusters & clusters);
 
   /**
-   * @brief Filter the segments based on the number of points, the distance between them.
+   * @brief Filter the clusters based on the number of points, the distance between them, etc.
    *
-   * @param segments The segments to filter
-   * @return Segments The filtered segments
+   * @param clusters The clusters to filter
+   * @return Clusters The filtered clusters
    */
-  Segments filterSegments(const Segments & segments);
+  Clusters filterClusters(const Clusters & clusters);
 
   /**
    * @brief Callback executed when a parameter change is detected
@@ -125,18 +125,18 @@ class Segmentation
 
   // Distance threshold for the segmentation
   double distance_threshold_;
-  // Minimum number of points in a segment
-  int min_points_segment_;
-  // Maximum number of points in a segment
-  int max_points_segment_;
-  // Minimum distance from sensor to the centroid of the segment
+  // Minimum number of points in a cluster
+  int min_points_cluster_;
+  // Maximum number of points in a cluster
+  int max_points_cluster_;
+  // Minimum distance from sensor to the centroid of the cluster
   double min_avg_distance_from_sensor_;
-  // Maximum distance from sensor to the centroid of the segment
+  // Maximum distance from sensor to the centroid of the cluster
   double max_avg_distance_from_sensor_;
-  // Minimum width of the segment
-  double min_segment_width_;
-  // Maximum width of the segment
-  double max_segment_width_;
+  // Minimum width of the cluster
+  double min_cluster_width_;
+  // Maximum width of the cluster
+  double max_cluster_width_;
 };
 
 }  // namespace scitos2_charging_dock

scitos2_charging_dock/src/dock_saver.cpp

@@ -163,15 +163,14 @@ bool DockSaver::saveDockCallback(
   // Find the cluster in front of the robot
   std::vector<double> angles;
   for (auto cluster : clusters) {
-    double angle = std::atan2(cluster.getCentroid().y, cluster.getCentroid().x);
+    double angle = std::atan2(cluster.centroid().y, cluster.centroid().x);
     angles.push_back(angles::normalize_angle_positive(angle));
   }
   auto min_angle = std::min_element(angles.begin(), angles.end());
   int idx = std::distance(angles.begin(), min_angle);
 
   // Store the dock pointcloud to a file
-  scitos2_charging_dock::Pcloud::Ptr dock = clusters[idx].cloud;
-  response->result = perception_->storeDockPointcloud(filename, *dock);
+  response->result = perception_->storeDockPointcloud(filename, clusters[idx].cloud);
 
   return true;
 }