diff --git a/evaluator/segmentation_evaluator/include/segmentation_evaluator/metrics/segmentation_metrics.hpp b/evaluator/segmentation_evaluator/include/segmentation_evaluator/metrics/segmentation_metrics.hpp index 98b0c5611052c..3d44f8fc2413a 100644 --- a/evaluator/segmentation_evaluator/include/segmentation_evaluator/metrics/segmentation_metrics.hpp +++ b/evaluator/segmentation_evaluator/include/segmentation_evaluator/metrics/segmentation_metrics.hpp @@ -32,19 +32,19 @@ using sensor_msgs::msg::PointCloud2; * @return Calculated statistics */ Stat updatePclStats( - const PointCloud2 & pcl, const PointCloud2 & pcl_gt_ground, const PointCloud2 & pcl_gt_obj, - const Stat stat_prev, PointCloud2 & pcl_no_ex); + const PointCloud2 & pcl, const PointCloud2 & pcl_gt_negative_cls, + const PointCloud2 & pcl_gt_positive_cls, const Stat 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 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 index 0f402a78a136b..7b2d5f24b56d3 100644 --- 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 updateStat( const Stat 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/evaluator/segmentation_evaluator/include/segmentation_evaluator/segmentation_evaluator_node.hpp b/evaluator/segmentation_evaluator/include/segmentation_evaluator/segmentation_evaluator_node.hpp index 1e5bd2911c85b..212ffda19e1eb 100644 --- a/evaluator/segmentation_evaluator/include/segmentation_evaluator/segmentation_evaluator_node.hpp +++ b/evaluator/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 & metric_stat) const; private: - geometry_msgs::msg::Pose getCurrentEgoPose() const; - // ROS - message_filters::Subscriber pcl_sub_, pcl_gt_ground_sub_, pcl_gt_obj_sub_; - rclcpp::Publisher::SharedPtr pcl_no_ex_pub_; + message_filters::Subscriber pcl_sub_, pcl_gt_negative_cls_sub_, + pcl_gt_positive_cls_sub_; typedef message_filters::sync_policies::ApproximateTime SyncPolicy; diff --git a/evaluator/segmentation_evaluator/include/segmentation_evaluator/stat.hpp b/evaluator/segmentation_evaluator/include/segmentation_evaluator/stat.hpp index fc1b6320f5511..5bd9142011abe 100644 --- 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::max(); - T max_ = std::numeric_limits::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 index e96e8761edc8f..bc54016be2468 100644 --- a/evaluator/segmentation_evaluator/launch/segmentation_evaluator.launch.xml +++ b/evaluator/segmentation_evaluator/launch/segmentation_evaluator.launch.xml @@ -1,12 +1,12 @@ - - + + - - + + diff --git a/evaluator/segmentation_evaluator/src/metrics/segmentation_metrics.cpp b/evaluator/segmentation_evaluator/src/metrics/segmentation_metrics.cpp index 0f09753774605..41b529e36bcc5 100644 --- 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 updatePclStats( - const PointCloud2 & pcl_to_eval, const PointCloud2 & pcl_gt_ground, - const PointCloud2 & pcl_gt_obj, const Stat 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 stat_prev) { Stat stat(stat_prev); std::vector 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 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::Ptr pcl_no_ex_xyz(new pcl::PointCloud); - 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 iter_x_pred(pcl_to_eval, "x"); sensor_msgs::PointCloud2ConstIterator iter_y_pred(pcl_to_eval, "y"); sensor_msgs::PointCloud2ConstIterator iter_z_pred(pcl_to_eval, "z"); - const size_t number_of_points_ground = pcl_gt_ground.width; - sensor_msgs::PointCloud2ConstIterator iter_x_g(pcl_gt_ground, "x"); - sensor_msgs::PointCloud2ConstIterator iter_y_g(pcl_gt_ground, "y"); - sensor_msgs::PointCloud2ConstIterator iter_z_g(pcl_gt_ground, "z"); + const size_t number_of_points_ground = pcl_gt_negative_cls.width; + sensor_msgs::PointCloud2ConstIterator iter_x_g(pcl_gt_negative_cls, "x"); + sensor_msgs::PointCloud2ConstIterator iter_y_g(pcl_gt_negative_cls, "y"); + sensor_msgs::PointCloud2ConstIterator iter_z_g(pcl_gt_negative_cls, "z"); - const size_t number_of_points_obj = pcl_gt_obj.width; - sensor_msgs::PointCloud2ConstIterator iter_x_o(pcl_gt_obj, "x"); - sensor_msgs::PointCloud2ConstIterator iter_y_o(pcl_gt_obj, "y"); - sensor_msgs::PointCloud2ConstIterator iter_z_o(pcl_gt_obj, "z"); + const size_t number_of_points_obj = pcl_gt_positive_cls.width; + sensor_msgs::PointCloud2ConstIterator iter_x_o(pcl_gt_positive_cls, "x"); + sensor_msgs::PointCloud2ConstIterator iter_y_o(pcl_gt_positive_cls, "y"); + sensor_msgs::PointCloud2ConstIterator iter_z_o(pcl_gt_positive_cls, "z"); - std::unordered_set, boost::hash > > set_pcl_gt_ground, - set_pcl_gt_obj; + std::unordered_set, boost::hash > > 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 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 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 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 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 index d722a75313297..7a52663ca3da7 100644 --- a/evaluator/segmentation_evaluator/src/metrics_calculator.cpp +++ b/evaluator/segmentation_evaluator/src/metrics_calculator.cpp @@ -20,11 +20,11 @@ namespace segmentation_diagnostics { Stat MetricsCalculator::updateStat( const Stat 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 " + diff --git a/evaluator/segmentation_evaluator/src/segmentation_evaluator_node.cpp b/evaluator/segmentation_evaluator/src/segmentation_evaluator_node.cpp index 582eabda8fad6..459274cbbc1dc 100644 --- a/evaluator/segmentation_evaluator/src/segmentation_evaluator_node.cpp +++ b/evaluator/segmentation_evaluator/src/segmentation_evaluator_node.cpp @@ -27,10 +27,11 @@ namespace segmentation_diagnostics SegmentationEvaluatorNode::SegmentationEvaluatorNode(const rclcpp::NodeOptions & node_options) : Node("segmentation_evaluator", node_options), pcl_sub_(this, "~/input/pcl/to/eval", rclcpp::QoS{1}.best_effort().get_rmw_qos_profile()), - pcl_gt_ground_sub_( - this, "~/input/pcl/gt/ground", rclcpp::QoS{1}.best_effort().get_rmw_qos_profile()), - pcl_gt_obj_sub_(this, "~/input/pcl/gt/obj", rclcpp::QoS{1}.best_effort().get_rmw_qos_profile()), - sync_(SyncPolicy(10), pcl_sub_, pcl_gt_ground_sub_, pcl_gt_obj_sub_) + pcl_gt_negative_cls_sub_( + this, "~/input/pcl/gt/negative_cls", rclcpp::QoS{1}.best_effort().get_rmw_qos_profile()), + pcl_gt_positive_cls_sub_( + this, "~/input/pcl/gt/positive_cls", rclcpp::QoS{1}.best_effort().get_rmw_qos_profile()), + sync_(SyncPolicy(10), pcl_sub_, pcl_gt_negative_cls_sub_, pcl_gt_positive_cls_sub_) { tf_buffer_ptr_ = std::make_unique(this->get_clock()); tf_listener_ptr_ = std::make_unique(*tf_buffer_ptr_); @@ -39,8 +40,6 @@ SegmentationEvaluatorNode::SegmentationEvaluatorNode(const rclcpp::NodeOptions & &SegmentationEvaluatorNode::syncCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - pcl_no_ex_pub_ = this->create_publisher("~/pcl_no_ex", rclcpp::SensorDataQoS()); - output_file_str_ = declare_parameter("output_file"); if (output_file_str_.empty()) { RCLCPP_INFO( @@ -67,61 +66,38 @@ DiagnosticStatus SegmentationEvaluatorNode::generateDiagnosticStatus( status.level = status.OK; status.name = metric_to_str.at(metric); diagnostic_msgs::msg::KeyValue key_value; - key_value.key = "min"; - key_value.value = boost::lexical_cast(metric_stat.min()); + key_value.key = "accuracy"; + key_value.value = boost::lexical_cast(metric_stat.accuracy()); status.values.push_back(key_value); - key_value.key = "max"; - key_value.value = boost::lexical_cast(metric_stat.max()); + key_value.key = "precision"; + key_value.value = boost::lexical_cast(metric_stat.precision()); status.values.push_back(key_value); - key_value.key = "mean"; - key_value.value = boost::lexical_cast(metric_stat.mean()); + key_value.key = "recall"; + key_value.value = boost::lexical_cast(metric_stat.recall()); status.values.push_back(key_value); return status; } void SegmentationEvaluatorNode::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) { - // RCLCPP_WARN(get_logger(), "Callback!"); DiagnosticArray metrics_msg; metrics_msg.header.stamp = now(); - PointCloud2 pcl_no_ex; + for (Metric metric : metrics_) { metrics_dict_[metric] = metrics_calculator_.updateStat( - metrics_dict_[metric], metric, *msg, *msg_gt_ground, *msg_gt_obj, pcl_no_ex); - // std::cout << metrics_dict_[metric] << "\n"; - // std::cout << "metric dict count" << metrics_dict_[metric].count() << "\n"; + metrics_dict_[metric], metric, *msg, *msg_gt_negative_cls, *msg_gt_positive_cls); + if (metrics_dict_[metric].count() > 0) { metrics_msg.status.push_back(generateDiagnosticStatus(metric, metrics_dict_[metric])); } } + if (!metrics_msg.status.empty()) { metrics_pub_->publish(metrics_msg); } - pcl_no_ex_pub_->publish(pcl_no_ex); } - -geometry_msgs::msg::Pose SegmentationEvaluatorNode::getCurrentEgoPose() const -{ - geometry_msgs::msg::TransformStamped tf_current_pose; - - geometry_msgs::msg::Pose p; - try { - tf_current_pose = tf_buffer_ptr_->lookupTransform( - "map", "base_link", rclcpp::Time(0), rclcpp::Duration::from_seconds(1.0)); - } catch (tf2::TransformException & ex) { - RCLCPP_ERROR(get_logger(), "%s", ex.what()); - return p; - } - - p.orientation = tf_current_pose.transform.rotation; - p.position.x = tf_current_pose.transform.translation.x; - p.position.y = tf_current_pose.transform.translation.y; - p.position.z = tf_current_pose.transform.translation.z; - return p; -} - } // namespace segmentation_diagnostics #include "rclcpp_components/register_node_macro.hpp" diff --git a/launch/tier4_perception_launch/launch/perception_ground_seg.launch.xml b/launch/tier4_perception_launch/launch/perception_ground_seg.launch.xml new file mode 100644 index 0000000000000..37fdda44108a4 --- /dev/null +++ b/launch/tier4_perception_launch/launch/perception_ground_seg.launch.xml @@ -0,0 +1,24 @@ + + + + + + + + + + + + + + + + + + + + + + + +