save clustering image

Author: thr0player

CMakeLists.txt

@@ -24,7 +24,7 @@ catkin_package(CATKIN_DEPENDS
         )
 
 add_definitions(-std=c++11)
-SET(CMAKE_CXX_FLAGS "-O2 -g -Wall ${CMAKE_CXX_FLAGS}")
+SET(CMAKE_CXX_FLAGS "-O3 -g -Wall ${CMAKE_CXX_FLAGS}")
 
 #set(OpenCV_DIR "/usr/local/share/OpenCV")
 

include/aggregation.h

@@ -19,20 +19,32 @@ namespace LaneDetect {
             countimg = cv::Mat(IMAGE_HEIGHT, IMAGE_WIDTH, CV_32SC1);
             imgzmin = cv::Mat(IMAGE_HEIGHT, IMAGE_WIDTH, CV_32SC1);
             imgzmax= cv::Mat(IMAGE_HEIGHT, IMAGE_WIDTH, CV_32SC1);
+
+            save_labelimg_ = true;
+            labelimg_empty_ = true;
+
+            if(save_labelimg_){
+
+//                std::
+            }
         };
 
         void Process(const std::deque<Frame> &data_buffer, const Eigen::Matrix4f &world_imu, const Eigen::Matrix4f &imu_vel,
-                     int frame_size, pcl::PointCloud<PPoint>::Ptr &pc, pcl::PointCloud<PPoint>::Ptr& labelpc);
+                     int frame_size, pcl::PointCloud<PPoint>::Ptr &pc, pcl::PointCloud<PPoint>::Ptr& labelpc, uint64_t timestamp);
 
         void ToPclPc(pcl::PointCloud<PPoint>::Ptr &pc, pcl::PointCloud<pcl::PointXYZ>::Ptr &pclpc);
 
+        static void SaveLabelImg(std::map<int, std::vector<cv::Point>> &label_map_point);
+
     private:
         cv::Mat img; //(IMAGE_HEIGHT, IMAGE_WIDTH, CV_8UC1);
         cv::Mat avgimg; //(IMAGE_HEIGHT, IMAGE_WIDTH, CV_32FC1);
         cv::Mat countimg; //(IMAGE_HEIGHT, IMAGE_WIDTH, CV_32SC1);
         cv::Mat imgzmin; //(IMAGE_HEIGHT, IMAGE_WIDTH, CV_32SC1);
         cv::Mat imgzmax; //(IMAGE_HEIGHT, IMAGE_WIDTH, CV_32SC1);
 
+        bool save_labelimg_;
+        bool labelimg_empty_;
     };
 
 }

include/utils/common.h

@@ -16,9 +16,9 @@
 
 namespace LaneDetect {
 
-    const int IMAGE_HEIGHT = 600;
-    const int IMAGE_WIDTH = 2000;
-    const float RES_STEP = 0.08;
+    const int IMAGE_HEIGHT = 900;
+    const int IMAGE_WIDTH = 3000;
+    const float RES_STEP = 0.04;//0.08
 
     const float EPS = 1e-6;
 

main.cpp

@@ -65,7 +65,7 @@ namespace LaneDetect {
                            const geometry_msgs::PoseStampedConstPtr &imu_msg);
     };
 
-    DetectManager::DetectManager() : node_handle_("~"), lidar_size_(20) {
+    DetectManager::DetectManager() : node_handle_("~"), lidar_size_(30) {
         ROS_INFO("Inititalizing BucketFiltering node ...");
         node_handle_.param<std::string>("point_topic", point_topic_, "/velodyne_points");
         ROS_INFO("Input Point Cloud: %s", point_topic_.c_str());
@@ -177,7 +177,8 @@ namespace LaneDetect {
 
         pcl::PointCloud<PPoint>::Ptr out_pc(new pcl::PointCloud<PPoint>());
         pcl::PointCloud<PPoint>::Ptr out_labelpc(new pcl::PointCloud<PPoint>());
-        aggregation_.Process(data_buffer_, world_imu_, imu_vel_, 20, out_pc, out_labelpc);
+        aggregation_.Process(data_buffer_, world_imu_, imu_vel_, 20, out_pc, out_labelpc,
+                             in_cloud_msg->header.stamp.toNSec());
 
         out_pc->header = c_in_cloud->header;
         out_labelpc->header = c_in_cloud->header;

src/aggregation.cc

@@ -11,11 +11,16 @@
 #include "cca.cc"
 #include <map>
 
+#include<boost/filesystem.hpp>
+
 #include <assert.h>
 namespace LaneDetect {
+namespace bf=boost::filesystem;//简单别名
+
 
 void Aggregation::Process(const std::deque<Frame> &data_buffer, const Eigen::Matrix4f &world_imu, const Eigen::Matrix4f &imu_vel,
-             int frame_size, pcl::PointCloud<PPoint>::Ptr& pc, pcl::PointCloud<PPoint>::Ptr& labelpc) {
+                          int frame_size, pcl::PointCloud<PPoint>::Ptr &pc, pcl::PointCloud<PPoint>::Ptr &labelpc,
+                          uint64_t timestamp) {
 
     if (data_buffer.size() <= frame_size) {
         return;
@@ -228,24 +233,24 @@ void Aggregation::Process(const std::deque<Frame> &data_buffer, const Eigen::Mat
     double dur = elap_timet.toSec();
     ROS_INFO("CCA time: %f",dur);
 
-    std::map<int,int> label_map;
-    std::map<int,int> label_map_idx;
+    std::map<int,int> label_map;     // label : cnt(0 -> n)
+    std::map<int,int> label_map_idx; // label : label_idx(2 -> n-2)
+    std::map<int,std::vector<cv::Point>> label_map_point;
     int labelcnt = 2;
     for(auto &si:labels){
         label_map[si] = label_map.size();
         label_map_idx[si] = labelcnt;
+        label_map_point[si] = std::vector<cv::Point>();
         labelcnt++;
     }
 
-
     std::vector<cv::Vec3b> color_table;
     for (size_t ci = 0; ci < labels.size()+10; ++ci) {
         color_table.emplace_back(cv::Vec3b(rand()%255, rand()%255, rand()%255));
     }
     cv::Mat color_label;
     cv::cvtColor(img, color_label, CV_GRAY2BGR);
 
-
     for (int row = 0; row < _lableImg.rows; ++row) {
         for (int col = 0; col < _lableImg.cols; ++col) {
             const int &label = _lableImg.at<int>(row, col);
@@ -254,13 +259,70 @@ void Aggregation::Process(const std::deque<Frame> &data_buffer, const Eigen::Mat
                 color_label.at<cv::Vec3b>(row,col)[0] = color_table.at(idx).val[0];
                 color_label.at<cv::Vec3b>(row,col)[1] = color_table.at(idx).val[1];
                 color_label.at<cv::Vec3b>(row,col)[2] = color_table.at(idx).val[2];
+
+                label_map_point[idx].emplace_back(cv::Point(col, row));
             }
         }
     }
+
 //    cv::imshow("_lableImg",_lableImg);
+//    cv::imshow("color_label",color_label);
+//    cv::waitKey(15);
+
+
+    /////// check all the points
+    std::string save_dir = "/tmp/save_dir/";
+    bf::path path(save_dir);
+
+    if (!bf::exists(path)) {
+        //目录不存在，创建
+        ROS_INFO("Create %s", path.c_str());
+        bf::create_directory(path);
+    }
+
+    bf::path file_path = path / (std::to_string(timestamp) + "_saveimg"); //path重载了 / 运算符
+    bf::create_directory(file_path);
+
+
+    cv::Mat testimg = color_label.clone();
+    testimg.setTo(0);
+    for (auto &label_map_p: label_map_point) {
+        if (label_map_p.second.size() < 10) continue;
+        if (label_map_p.second.size() > 1500) continue;
+
+        // get range
+        cv::Point minp(-1, -1), maxp(-1, -1);
+        for (auto &pp: label_map_p.second) {
+            if (minp.x < 0)minp.x = pp.x;
+            if (minp.y < 0)minp.y = pp.y;
+            if (maxp.x < 0)maxp.x = pp.x;
+            if (maxp.x < 0)maxp.y = pp.y;
+
+            if (minp.x > pp.x) minp.x = pp.x;
+            if (minp.y > pp.y) minp.y = pp.y;
+            if (maxp.x < pp.x) maxp.x = pp.x;
+            if (maxp.y < pp.y) maxp.y = pp.y;
+        }
+
+        //draw img
+
+        cv::Mat saveimg(maxp.y - minp.y + 10, maxp.x - minp.x + 10, CV_8SC1,cv::Scalar(0));
+        for (auto &pp: label_map_p.second) {
+            cv::circle(saveimg, cv::Point(pp.x - minp.x, pp.y - minp.y), 1, cv::Scalar(255));
+        }
+
+        //save
+        bf::path filename = file_path / bf::path(std::to_string(label_map_p.first) + ".png");
+        cv::imwrite(filename.string(), saveimg);
+
+
+//        cv::imshow("lableImg",testimg);
+//        cv::waitKey();
+    }
+    bf::path filename = file_path / bf::path(std::to_string(timestamp) + ".png");
+
+    cv::imwrite(filename.string(), color_label);
 
-    cv::imshow("color_label",color_label);
-    cv::waitKey(15);
 
     //// assign label to pc
     if (labelpc == nullptr) {