feature extractor going...

common/common.h

@@ -4,4 +4,5 @@
 #include "log.h"
 #include "tic_toc.h"
 #include "types.h"
-#include "utils.h"
+#include "utils.h"
+#include "yaml-cpp/yaml.h"

common/log.h

@@ -20,21 +20,35 @@ class CustomSink {
  public:
   CustomSink() = default;
   explicit CustomSink(const std::string& log_file_name)
-      : log_file_name_(log_file_name), log_to_file_(true) {}
-  ~CustomSink() = default;
+      : log_file_name_(log_file_name), log_to_file_(true) {
+    ofs_.reset(new std::ofstream(log_file_name));
+  }
+  ~CustomSink() {
+    std::ostringstream oss;
+    oss << "\ng3log " << (log_to_file_ ? "FileSink" : "StdSink")
+        << " shutdown at: ";
+    auto now = std::chrono::system_clock::now();
+    oss << g3::localtime_formatted(now, "%Y%m%d %H:%M:%S.%f3");
+    if (log_to_file_ && ofs_ != nullptr) {
+      (*ofs_) << oss.str() << std::endl;
+    }
+    std::clog << oss.str() << std::endl;
+  };
 
   void StdLogMessage(g3::LogMessageMover logEntry) {
     std::clog << ColorFormatedMessage(logEntry.get()) << std::endl;
   }
 
   void FileLogMessage(g3::LogMessageMover logEntry) {
-    static std::ofstream ofs(log_file_name_);
-    ofs << FormatedMessage(logEntry.get()) << std::endl;
+    if (log_to_file_ && ofs_ != nullptr) {
+      (*ofs_) << FormatedMessage(logEntry.get()) << std::endl;
+    }
   }
 
  private:
   std::string log_file_name_;
-  bool log_to_file_ = false;
+  bool log_to_file_{false};
+  std::unique_ptr<std::ofstream> ofs_{nullptr};
 
   std::string FormatedMessage(const g3::LogMessage& msg) const {
     std::ostringstream oss;

common/types.h

@@ -54,26 +54,34 @@ using PCLVisualizer = pcl::visualization::PCLVisualizer;
 
 struct PointXYZTCT {
   PCL_ADD_POINT4D;
-  float time = 0.0f;
-  float curvature = std::nanf("");
-  int8_t type = 0;  // -2, -1, 0, 1, or 2
+  union EIGEN_ALIGN16 {
+    float data_c[4];
+    struct {
+      float time;
+      float curvature;
+      int type;  // -2, -1, 0, 1, or 2
+    };
+  };
 
   PointXYZTCT() {
     x = y = z = 0.0f;
+    time = 0.0f;
+    curvature = std::nanf("");
+    type = 0;
     data[3] = 1.0f;
   }
 
   PointXYZTCT(float x, float y, float z, float time = 0.0f,
-              float curvature = NAN, int8_t type = 0)
-      : x(x), y(y), z(z), time(time), curvature(curvature), type(type) {
-    data[3] = 1.0f;
-  }
+              float curvature = std::nanf(""), int type = 0)
+      : x(x), y(y), z(z), time(time), curvature(curvature), type(type) {}
 
   PointXYZTCT(const Point& p) {
     x = p.x;
     y = p.y;
     z = p.z;
-    data[3] = 1.0f;
+    time = 0.0f;
+    curvature = std::nanf("");
+    type = 0;
   }
 
   PointXYZTCT(const PointXYZTCT& p) {
@@ -83,7 +91,6 @@ struct PointXYZTCT {
     time = p.time;
     curvature = p.curvature;
     type = p.type;
-    data[3] = 1.0f;
   }
 
   PointType Type() const { return static_cast<PointType>(type); }
@@ -100,7 +107,7 @@ POINT_CLOUD_REGISTER_POINT_STRUCT(
   (float, y, y)
   (float, z, z)
   (float, time, time)
-  (float, curvature, curvatur)
+  (float, curvature, curvature)
   (int8_t, type, type)
 )
 // clang-format on

config/config.yaml

@@ -1,6 +1,4 @@
-%YAML:1.0
-
 lidar: VPL16
 
 feature_points_extractor_config:
-  min_point_num: 10
+  min_scan_point_num: 66

main.cc

@@ -12,15 +12,16 @@ void PointCloudHandler(const sensor_msgs::PointCloud2ConstPtr& msg,
                        oh_my_loam::OhMyLoam* const slam);
 
 int main(int argc, char* argv[]) {
-  // logging
-  g3::InitG3Logging<false>("oh_my_loam", ".log");
-
   // configurations
   YAML::Node config = YAML::LoadFile("./config/config.yaml");
+
+  // logging
+  g3::InitG3Logging<true>("oh_my_loam", ".log");
   AWARN << config["lidar"].as<std::string>();
+
   // SLAM system
   oh_my_loam::OhMyLoam slam;
-  slam.Init(config);
+  slam.Init(config["feature_points_extractor_config"]);
 
   // ros
   ros::init(argc, argv, "oh_my_loam");

src/feature_extractor_base.cc

@@ -13,6 +13,11 @@ const double kTwoPi = 2 * M_PI;
 const int kMinPtsNum = 100;
 }  // namespace
 
+bool FeaturePointsExtractor::Init(const YAML::Node& config) {
+  config_ = config;
+  return true;
+}
+
 void FeaturePointsExtractor::Extract(const PointCloud& cloud_in,
                                      FeaturePoints* const feature) const {
   PointCloudPtr cloud(new PointCloud);
@@ -25,12 +30,17 @@ void FeaturePointsExtractor::Extract(const PointCloud& cloud_in,
   }
   std::vector<IPointCloud> scans;
   SplitScan(*cloud, &scans);
+  std::ostringstream oss;
   for (auto& scan : scans) {
-    std::cout << scan.size() << "  ";
-    // ComputePointCurvature(&scan);
-    // AssignPointType(&scan);
+    oss << scan.size() << ":";
+    ComputePointCurvature(&scan);
+    RemovePointsIf<IPoint>(scan, &scan, [](const IPoint& pt) {
+      return !std::isfinite(pt.curvature);
+    });
+    oss << scan.size() << " ";
+    AssignPointType(&scan);
   }
-  std::cout << std::endl;
+  AWARN << oss.str();
   for (const auto& scan : scans) {
     feature->feature_pts += scan;
     for (const auto& pt : scan.points) {
@@ -74,31 +84,33 @@ void FeaturePointsExtractor::SplitScan(
   }
 }
 
-// void FeaturePointsExtractor::ComputePointCurvature(
-//     IPointCloud* const scan) const {
-//   auto& pts = scan->points;
-//   for (size_t i = 5; i < pts.size() - 5; ++i) {
-//     float diffX = pts[i - 5].x + pts[i - 4].x + pts[i - 3].x + pts[i - 2].x +
-//                   pts[i - 1].x + pts[i + 1].x + pts[i + 2].x + pts[i + 3].x +
-//                   pts[i + 4].x + pts[i + 5].x - 10 * pts[i].x;
-//     float diffY = pts[i - 5].y + pts[i - 4].y + pts[i - 3].y + pts[i - 2].y +
-//                   pts[i - 1].y + pts[i + 1].y + pts[i + 2].y + pts[i + 3].y +
-//                   pts[i + 4].y + pts[i + 5].y - 10 * pts[i].y;
-//     float diffZ = pts[i - 5].z + pts[i - 4].z + pts[i - 3].z + pts[i - 2].z +
-//                   pts[i - 1].z + pts[i + 1].z + pts[i + 2].z + pts[i + 3].z +
-//                   pts[i + 4].z + pts[i + 5].z - 10 * pts[i].z;
-//     pts[i].curvature = diffX * diffX + diffY * diffY + diffZ * diffZ;
-//   }
-// }
+//
+void FeaturePointsExtractor::ComputePointCurvature(
+    IPointCloud* const scan) const {
+  if (scan->size() < 20) return;
+  auto& pts = scan->points;
+  for (size_t i = 5; i < pts.size() - 5; ++i) {
+    float diffX = pts[i - 5].x + pts[i - 4].x + pts[i - 3].x + pts[i - 2].x +
+                  pts[i - 1].x + pts[i + 1].x + pts[i + 2].x + pts[i + 3].x +
+                  pts[i + 4].x + pts[i + 5].x - 10 * pts[i].x;
+    float diffY = pts[i - 5].y + pts[i - 4].y + pts[i - 3].y + pts[i - 2].y +
+                  pts[i - 1].y + pts[i + 1].y + pts[i + 2].y + pts[i + 3].y +
+                  pts[i + 4].y + pts[i + 5].y - 10 * pts[i].y;
+    float diffZ = pts[i - 5].z + pts[i - 4].z + pts[i - 3].z + pts[i - 2].z +
+                  pts[i - 1].z + pts[i + 1].z + pts[i + 2].z + pts[i + 3].z +
+                  pts[i + 4].z + pts[i + 5].z - 10 * pts[i].z;
+    pts[i].curvature = std::sqrt(diffX * diffX + diffY * diffY + diffZ * diffZ);
+  }
+}
 
-// void FeaturePointsExtractor::AssignPointType(IPointCloud* const scan) const {
-//   int pt_num = scan->size();
-//   int pt_num_seg = pt_num / kScanSegNum;
-//   std::vector<bool> picked(pt_num, false);
-//   for (int i = 0; i < kScanSegNum; ++i) {
-//     int begin = i * pt_num_seg;
-//     int end = i * pt_num_seg + pt_num_seg;
-//   }
-// }
+void FeaturePointsExtractor::AssignPointType(IPointCloud* const scan) const {
+  int pt_num = scan->size();
+  int pt_num_seg = (pt_num - 1) / kScanSegNum + 1;
+  std::vector<bool> picked(pt_num, false);
+  for (int i = 0; i < kScanSegNum; ++i) {
+    int begin = i * pt_num_seg;
+    int end = std::max((i + 1) * pt_num_seg, pt_num);
+  }
+}
 
 }  // namespace oh_my_loam

src/feature_extractor_base.h

@@ -19,6 +19,7 @@ class FeaturePointsExtractor {
   FeaturePointsExtractor(const FeaturePointsExtractor&) = delete;
   FeaturePointsExtractor& operator=(const FeaturePointsExtractor&) = delete;
 
+  bool Init(const YAML::Node& config);
   void Extract(const PointCloud& cloud_in, FeaturePoints* const feature) const;
 
   int num_scans() const { return num_scans_; }
@@ -29,11 +30,12 @@ class FeaturePointsExtractor {
   void SplitScan(const PointCloud& cloud,
                  std::vector<IPointCloud>* const scans) const;
 
-  // void ComputePointCurvature(IPointCloud* const scan) const;
+  void ComputePointCurvature(IPointCloud* const scan) const;
 
-  // void AssignPointType(IPointCloud* const scan) const;
+  void AssignPointType(IPointCloud* const scan) const;
 
   int num_scans_ = 0;
+  YAML::Node config_;
 };
 
 }  // namespace oh_my_loam

src/visualizer_feature_points.h

@@ -26,7 +26,7 @@ class FeaturePointsVisualizer : public Visualizer<FeaturePointsVisFrame> {
     }
     {  // add all feature_pts
       std::string id = "feature_pts";
-      DrawPointCloud<IPoint>(frame.feature_pts->feature_pts, "time", id,
+      DrawPointCloud<IPoint>(frame.feature_pts->feature_pts, "curvature", id,
                              viewer_.get());
       rendered_cloud_ids_.push_back(id);
     }