| 77 | } |
| 78 | |
| 79 | void callbackNode(const sensor_msgs::PointCloud2::ConstPtr &msg) { |
| 80 | cout << msg->header.seq << "th node come" << endl; |
| 81 | pcl::PointCloud<PointType> pc_curr = cloudmsg2cloud<PointType>(*msg); |
| 82 | pcl::PointCloud<PointType> pc_ground; |
| 83 | pcl::PointCloud<PointType> pc_non_ground; |
| 84 | pcl::PointCloud<PointType> pc_labeled; |
| 85 | pc_ground.header = pc_curr.header; |
| 86 | pc_non_ground.header = pc_curr.header; |
| 87 | pc_labeled.header = pc_curr.header; |
| 88 | pc_ground.reserve(pc_curr.size()); |
| 89 | pc_non_ground.reserve(pc_curr.size()); |
| 90 | pc_labeled.reserve(pc_curr.size()); |
| 91 | |
| 92 | static double time_taken; |
| 93 | |
| 94 | std::cout << "Operating patchwork..." << std::endl; |
| 95 | PatchworkGroundSeg->estimate_ground(pc_curr, pc_ground, pc_non_ground, time_taken); |
| 96 | |
| 97 | double hz = 1.0 / time_taken; |
| 98 | std::cout << "\033[1;32m" << msg->header.seq << "th| "; |
| 99 | std::cout << "Time takes " << std::fixed << std::setprecision(2) << time_taken * 1000 << " ms"; |
| 100 | std::cout << " (" << std::fixed << std::setprecision(2) << hz << " Hz). "; |
| 101 | std::cout << "# cloud: " << pc_curr.size() << " -> " << pc_ground.size() << "\033[0m" |
| 102 | << std::endl; |
| 103 | |
| 104 | double precision, recall, precision_naive, recall_naive; |
| 105 | if (is_kitti) { |
| 106 | calculate_precision_recall(pc_curr, pc_ground, precision, recall); |
| 107 | calculate_precision_recall(pc_curr, pc_ground, precision_naive, recall_naive, false); |
| 108 | cout << "\033[1;32m P: " << precision << " | R: " << recall << "\033[0m" << endl; |
| 109 | } |
| 110 | |
| 111 | // output_filename = "/home/shapelim/patchwork_debug.txt"; |
| 112 | // ofstream sc_output(output_filename, ios::app); |
| 113 | // sc_output << msg->header.seq << "," << time_taken << "," << precision << |
| 114 | // "," << recall << "," << precision_naive << "," << recall_naive; |
| 115 | // |
| 116 | // sc_output << std::endl; |
| 117 | // sc_output.close(); |
| 118 | |
| 119 | // Publish msg |
| 120 | pcl::PointCloud<PointType> TP; |
| 121 | pcl::PointCloud<PointType> FP; |
| 122 | pcl::PointCloud<PointType> FN; |
| 123 | pcl::PointCloud<PointType> TN; |
| 124 | TP.header = pc_curr.header; |
| 125 | FP.header = pc_curr.header; |
| 126 | FN.header = pc_curr.header; |
| 127 | TN.header = pc_curr.header; |
| 128 | |
| 129 | if (is_kitti) { |
| 130 | discern_ground(pc_ground, TP, FP); |
| 131 | discern_ground(pc_non_ground, FN, TN); |
| 132 | |
| 133 | if (save_flag) { |
| 134 | std::map<int, int> pc_curr_gt_counts, g_est_gt_counts; |
| 135 | double accuracy; |
| 136 | save_all_accuracy( |
nothing calls this directly
no test coverage detected