Generalize code

Signed-off-by: djargot <dominik.jargot@robotec.ai>
autowarefoundation · Oct 10, 2022 · 46a0a81 · 46a0a81
1 parent 9216b53
commit 46a0a81
Show file tree

Hide file tree

Showing 9 changed files with 109 additions and 162 deletions.
diff --git a/...or/segmentation_evaluator/include/segmentation_evaluator/metrics/segmentation_metrics.hpp b/...or/segmentation_evaluator/include/segmentation_evaluator/metrics/segmentation_metrics.hpp
@@ -32,19 +32,19 @@ using sensor_msgs::msg::PointCloud2;
  * @return Calculated statistics
  */
 Stat<double> updatePclStats(
-  const PointCloud2 & pcl, const PointCloud2 & pcl_gt_ground, const PointCloud2 & pcl_gt_obj,
-  const Stat<double> stat_prev, PointCloud2 & pcl_no_ex);
+  const PointCloud2 & pcl, const PointCloud2 & pcl_gt_negative_cls,
+  const PointCloud2 & pcl_gt_positive_cls, const Stat<double> stat_prev);
 
 /**
  * @brief Compute confustion matrix for point clouds
  * @param [in] pcl Point cloud
- * @param [in] pcl_gt_ground Ground truth point cloud with ground
- * @param [in] pcl_gt_obj Ground truth point cloud with objects
+ * @param [in] pcl_gt_negative_cls Ground truth point cloud with ground
+ * @param [in] pcl_gt_positive_cls Ground truth point cloud with objects
  * @return calculated statistics
  */
 std::vector<int> computeConfusionMatrix(
-  const PointCloud2 & pcl, const PointCloud2 & pcl_gt_ground, const PointCloud2 & pcl_gt_obj,
-  PointCloud2 & pcl_no_ex);
+  const PointCloud2 & pcl, const PointCloud2 & pcl_gt_negative_cls,
+  const PointCloud2 & pcl_gt_positive_cls);
 
 }  // namespace metrics
 }  // namespace segmentation_diagnostics

diff --git a/evaluator/segmentation_evaluator/include/segmentation_evaluator/metrics_calculator.hpp b/evaluator/segmentation_evaluator/include/segmentation_evaluator/metrics_calculator.hpp
@@ -48,14 +48,13 @@ class MetricsCalculator
    * @param [in] metric Previous metric enum value
    * @param [in] stat_prev Previous metric
    * @param [in] pcl Point Cloud
-   * @param [in] pcl_gt_ground Ground truth point cloud with ground
-   * @param [in] pcl_gt_obj Ground truth point cloud with objects
+   * @param [in] pcl_gt_negative_cls Ground truth point cloud with ground
+   * @param [in] pcl_gt_positive_cls Ground truth point cloud with objects
    * @return string describing the requested metric
    */
   Stat<double> updateStat(
     const Stat<double> stat_prev, const Metric metric, const PointCloud2 & pcl,
-    const PointCloud2 & pcl_gt_ground, const PointCloud2 & pcl_gt_obj,
-    PointCloud2 & pcl_no_ex) const;
+    const PointCloud2 & pcl_gt_negative_cls, const PointCloud2 & pcl_gt_positive_cls) const;
 };  // class MetricsCalculator
 
 }  // namespace segmentation_diagnostics

diff --git a/...tor/segmentation_evaluator/include/segmentation_evaluator/segmentation_evaluator_node.hpp b/...tor/segmentation_evaluator/include/segmentation_evaluator/segmentation_evaluator_node.hpp
@@ -51,12 +51,13 @@ class SegmentationEvaluatorNode : public rclcpp::Node
   /**
    * @brief synchronized callback on two point clouds
    * @param [in] msg Poind cloud message
-   * @param [in] msg_gt_ground Ground truth point cloud with ground
-   * @param [in] msg_gt_obj Ground truth point cloud with objects
+   * @param [in] msg_gt_negative_cls Ground truth point cloud with ground
+   * @param [in] msg_gt_positive_cls Ground truth point cloud with objects
    */
   void syncCallback(
-    const PointCloud2::ConstSharedPtr & msg, const PointCloud2::ConstSharedPtr & msg_gt_ground,
-    const PointCloud2::ConstSharedPtr & msg_gt_obj);
+    const PointCloud2::ConstSharedPtr & msg,
+    const PointCloud2::ConstSharedPtr & msg_gt_negative_cls,
+    const PointCloud2::ConstSharedPtr & msg_gt_positive_cls);
 
   /**
    * @brief publish the given metric statistic
@@ -65,11 +66,9 @@ class SegmentationEvaluatorNode : public rclcpp::Node
     const Metric & metric, const Stat<double> & metric_stat) const;
 
 private:
-  geometry_msgs::msg::Pose getCurrentEgoPose() const;
-
   // ROS
-  message_filters::Subscriber<PointCloud2> pcl_sub_, pcl_gt_ground_sub_, pcl_gt_obj_sub_;
-  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pcl_no_ex_pub_;
+  message_filters::Subscriber<PointCloud2> pcl_sub_, pcl_gt_negative_cls_sub_,
+    pcl_gt_positive_cls_sub_;
 
   typedef message_filters::sync_policies::ApproximateTime<PointCloud2, PointCloud2, PointCloud2>
     SyncPolicy;

diff --git a/evaluator/segmentation_evaluator/include/segmentation_evaluator/stat.hpp b/evaluator/segmentation_evaluator/include/segmentation_evaluator/stat.hpp
@@ -31,35 +31,42 @@ class Stat
 {
 public:
   /**
-   * @brief add a value
-   * @param value value to add
+   * @brief get cumulative true positives
    */
-  void add(const T & value)
-  {
-    if (value < min_) {
-      min_ = value;
-    }
-    if (value > max_) {
-      max_ = value;
-    }
-    ++count_;
-    mean_ = mean_ + (value - mean_) / count_;
-  }
+  int tp() const { return tp_accu_; }
 
   /**
-   * @brief get the mean value
+   * @brief get cumulative false positives
    */
-  long double mean() const { return mean_; }
+  int fp() const { return fp_accu_; }
+
+  /**
+   * @brief get cumulative false negatives
+   */
+  int fn() const { return fn_accu_; }
+
+  /**
+   * @brief get cumulative true negatives
+   */
+  int tn() const { return tn_accu_; }
+
+  /**
+   * @brief get cumulative accuracy
+   */
+  float accuracy() const
+  {
+    return float(tp_accu_ + tn_accu_) / float(tp_accu_ + tn_accu_ + fn_accu_ + fp_accu_);
+  }
 
   /**
-   * @brief get the minimum value
+   * @brief get cumulative precision
    */
-  T min() const { return min_; }
+  float precision() const { return float(tp_accu_) / float(tp_accu_ + fp_accu_); }
 
   /**
-   * @brief get the maximum value
+   * @brief get cumulative recall
    */
-  T max() const { return max_; }
+  float recall() const { return float(tp_accu_) / float(tp_accu_ + fn_accu_); }
 
   /**
    * @brief Print confusion matrix
@@ -138,9 +145,6 @@ class Stat
   int tp_ = 0, fn_ = 0, fp_ = 0, tn_ = 0;
   int tp_accu_ = 0, fn_accu_ = 0, fp_accu_ = 0, tn_accu_ = 0;
 
-  T min_ = std::numeric_limits<T>::max();
-  T max_ = std::numeric_limits<T>::min();
-  long double mean_ = 0.0;
   unsigned int count_ = 0;
 };
 

diff --git a/evaluator/segmentation_evaluator/launch/segmentation_evaluator.launch.xml b/evaluator/segmentation_evaluator/launch/segmentation_evaluator.launch.xml
@@ -1,12 +1,12 @@
 <launch>
   <arg name="input/pcl/to/eval" default="/perception/obstacle_segmentation/single_frame/pointcloud_raw"/>
-  <arg name="input/pcl/gt/ground" default="/pcl_div/pcl_gt_ground"/>
-  <arg name="input/pcl/gt/obj" default="/pcl_div/pcl_gt_obj"/>
+  <arg name="input/pcl/gt/negative_cls" default="/pcl_div/pcl_gt_negative_cls"/>
+  <arg name="input/pcl/gt/obj" default="/pcl_div/pcl_gt_positive_cls"/>
 
   <node name="segmentation_evaluator" exec="segmentation_evaluator" pkg="segmentation_evaluator" output="screen">
     <param from="$(find-pkg-share segmentation_evaluator)/param/segmentation_evaluator.defaults.yaml"/>
     <remap from="~/input/pcl/to/eval" to="$(var input/pcl/to/eval)"/>
-    <remap from="~/input/pcl/gt/ground" to="$(var input/pcl/gt/ground)"/>
-    <remap from="~/input/pcl/gt/obj" to="$(var input/pcl/gt/obj)"/>
+    <remap from="~/input/pcl/gt/negative_cls" to="$(var input/pcl/gt/negative_cls)"/>
+    <remap from="~/input/pcl/gt/positive_cls" to="$(var input/pcl/gt/obj)"/>
   </node>
 </launch>
diff --git a/evaluator/segmentation_evaluator/src/metrics/segmentation_metrics.cpp b/evaluator/segmentation_evaluator/src/metrics/segmentation_metrics.cpp
@@ -38,118 +38,63 @@ class HashFun
 };
 
 Stat<double> updatePclStats(
-  const PointCloud2 & pcl_to_eval, const PointCloud2 & pcl_gt_ground,
-  const PointCloud2 & pcl_gt_obj, const Stat<double> stat_prev, PointCloud2 & pcl_no_ex)
+  const PointCloud2 & pcl_to_eval, const PointCloud2 & pcl_gt_negative_cls,
+  const PointCloud2 & pcl_gt_positive_cls, const Stat<double> stat_prev)
 {
   Stat<double> stat(stat_prev);
   std::vector<int> conf_mat;
-  conf_mat = computeConfusionMatrix(pcl_to_eval, pcl_gt_ground, pcl_gt_obj, pcl_no_ex);
+  conf_mat = computeConfusionMatrix(pcl_to_eval, pcl_gt_negative_cls, pcl_gt_positive_cls);
   stat.setConfustionMatPerCloud(conf_mat);
-  stat.printConfMat();
-  stat.printConfMatAccu();
   return stat;
 }
 
 float r(float v) { return floor(v * 1000.0) / 1000.0; }
 
 std::vector<int> computeConfusionMatrix(
-  const PointCloud2 & pcl_to_eval, const PointCloud2 & pcl_gt_ground,
-  const PointCloud2 & pcl_gt_obj, PointCloud2 & pcl_no_ex)
+  const PointCloud2 & pcl_to_eval, const PointCloud2 & pcl_gt_negative_cls,
+  const PointCloud2 & pcl_gt_positive_cls)
 {
-  // std::cout << pcl_gt_ground.row_step << " + " << pcl_gt_obj.row_step << " = "
-  //           << pcl_gt_ground.row_step + pcl_gt_obj.row_step << "    !eval: " <<
-  //           pcl_to_eval.row_step
-  //           << std::endl;
-  pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_no_ex_xyz(new pcl::PointCloud<pcl::PointXYZ>);
-  pcl_no_ex_xyz->points.reserve(pcl_to_eval.row_step);
-
   int tp = 0, fn = 0, fp = 0, tn = 0;
   const size_t number_of_points = pcl_to_eval.width;
   sensor_msgs::PointCloud2ConstIterator<float> iter_x_pred(pcl_to_eval, "x");
   sensor_msgs::PointCloud2ConstIterator<float> iter_y_pred(pcl_to_eval, "y");
   sensor_msgs::PointCloud2ConstIterator<float> iter_z_pred(pcl_to_eval, "z");
 
-  const size_t number_of_points_ground = pcl_gt_ground.width;
-  sensor_msgs::PointCloud2ConstIterator<float> iter_x_g(pcl_gt_ground, "x");
-  sensor_msgs::PointCloud2ConstIterator<float> iter_y_g(pcl_gt_ground, "y");
-  sensor_msgs::PointCloud2ConstIterator<float> iter_z_g(pcl_gt_ground, "z");
+  const size_t number_of_points_ground = pcl_gt_negative_cls.width;
+  sensor_msgs::PointCloud2ConstIterator<float> iter_x_g(pcl_gt_negative_cls, "x");
+  sensor_msgs::PointCloud2ConstIterator<float> iter_y_g(pcl_gt_negative_cls, "y");
+  sensor_msgs::PointCloud2ConstIterator<float> iter_z_g(pcl_gt_negative_cls, "z");
 
-  const size_t number_of_points_obj = pcl_gt_obj.width;
-  sensor_msgs::PointCloud2ConstIterator<float> iter_x_o(pcl_gt_obj, "x");
-  sensor_msgs::PointCloud2ConstIterator<float> iter_y_o(pcl_gt_obj, "y");
-  sensor_msgs::PointCloud2ConstIterator<float> iter_z_o(pcl_gt_obj, "z");
+  const size_t number_of_points_obj = pcl_gt_positive_cls.width;
+  sensor_msgs::PointCloud2ConstIterator<float> iter_x_o(pcl_gt_positive_cls, "x");
+  sensor_msgs::PointCloud2ConstIterator<float> iter_y_o(pcl_gt_positive_cls, "y");
+  sensor_msgs::PointCloud2ConstIterator<float> iter_z_o(pcl_gt_positive_cls, "z");
 
-  std::unordered_set<std::vector<float>, boost::hash<std::vector<float> > > set_pcl_gt_ground,
-    set_pcl_gt_obj;
+  std::unordered_set<std::vector<float>, boost::hash<std::vector<float> > > set_pcl_gt_negative_cls,
+    set_pcl_gt_positive_cls;
   for (size_t i = 0; i < number_of_points_ground; ++i, ++iter_x_g, ++iter_y_g, ++iter_z_g) {
     std::vector<float> point({r(*iter_x_g), r(*iter_y_g), r(*iter_z_g)});
-    set_pcl_gt_ground.insert(point);
+    set_pcl_gt_negative_cls.insert(point);
   }
   for (size_t i = 0; i < number_of_points_obj; ++i, ++iter_x_o, ++iter_y_o, ++iter_z_o) {
     std::vector<float> point({r(*iter_x_o), r(*iter_y_o), r(*iter_z_o)});
-    set_pcl_gt_obj.insert(point);
+    set_pcl_gt_positive_cls.insert(point);
   }
-  // std::cout << set_pcl_gt_ground.size() << " + " << set_pcl_gt_obj.size() << " = "
-  //           << set_pcl_gt_ground.size() + set_pcl_gt_obj.size()
-  //           << "    eval: " << pcl_to_eval.width << std::endl;
 
-  int cnt_zero = 0;
-  int cnt_non_ex = 0;
   for (size_t i = 0; i < number_of_points; ++i, ++iter_x_pred, ++iter_y_pred, ++iter_z_pred) {
     std::vector<float> point({r(*iter_x_pred), r(*iter_y_pred), r(*iter_z_pred)});
 
-    int is_ground = set_pcl_gt_ground.count(point);
-    int is_obj = set_pcl_gt_obj.count(point);
-    if (point[0] == 0 && point[1] == 0 && point[2] == 0) {
-      cnt_zero++;
-    } else if (is_ground == 0 && is_obj == 0) {
-      cnt_non_ex++;
-      pcl_no_ex_xyz->push_back({point[0], point[1], point[2]});
-    } else if (is_ground) {
+    int is_ground = set_pcl_gt_negative_cls.count(point);
+    int is_obj = set_pcl_gt_positive_cls.count(point);
+    if (is_ground) {
       fp += 1;
     } else if (is_obj) {
       tp += 1;
-    } else {
-      std::cout << "No point";
     }
   }
   fn = number_of_points_obj - tp;
   tn = number_of_points_ground - fp;
 
-  // std::cout << "ZEROS in pred: " << cnt_zero << "\n";
-  // std::cout << "NONEX in pred: " << cnt_non_ex << "\n";
-  std::cout << "Accuracy: " << float(tp + tn) / float(tp + tn + fn + fp) << "\n";
-  std::cout << "Precision: " << float(tp) / float(tp + fp) << "\n";
-  std::cout << "Recall: " << float(tp) / float(tp + fn) << "\n";
-
-  pcl::toROSMsg(*pcl_no_ex_xyz, pcl_no_ex);
-  pcl_no_ex.set__header(pcl_to_eval.header);
-
-  // for (size_t i = 0; i < number_of_points_ground; ++i, ++iter_x_pred, ++iter_y_pred,
-  // ++iter_z_pred) {
-  //   float x = *iter_x_pred;
-  //   float y = *iter_y_pred;
-  //   float z = *iter_z_pred;
-  //   std::vector<float> point({x, y, z});
-
-  //   if (map_pcl[point] == 127 || map_pcl[point] == 10) {
-  //     fp += 1;
-  //   } else {
-  //     tp += 1;
-  //   }
-  //   map_pcl.erase(point);
-  // }
-
-  // for (auto & point : map_pcl) {
-  //   if (map_pcl[point.first] == 127 || map_pcl[point.first] == 10) {
-  //     tn += 1;
-  //   } else {
-  //     fn += 1;
-  //   }
-  // }
-
-  // std::cout << tp << "\n";
-
   return {tp, fn, fp, tn};
 }
 

diff --git a/evaluator/segmentation_evaluator/src/metrics_calculator.cpp b/evaluator/segmentation_evaluator/src/metrics_calculator.cpp
@@ -20,11 +20,11 @@ namespace segmentation_diagnostics
 {
 Stat<double> MetricsCalculator::updateStat(
   const Stat<double> stat_prev, const Metric metric, const PointCloud2 & pcl,
-  const PointCloud2 & pcl_gt_ground, const PointCloud2 & pcl_gt_obj, PointCloud2 & pcl_no_ex) const
+  const PointCloud2 & pcl_gt_negative_cls, const PointCloud2 & pcl_gt_positive_cls) const
 {
   switch (metric) {
     case Metric::segment_stat:
-      return metrics::updatePclStats(pcl, pcl_gt_ground, pcl_gt_obj, stat_prev, pcl_no_ex);
+      return metrics::updatePclStats(pcl, pcl_gt_negative_cls, pcl_gt_positive_cls, stat_prev);
     default:
       throw std::runtime_error(
         "[MetricsCalculator][calculate()] unknown Metric " +