From 82eee40dc86d48cdf992885969b28063fcdfb5da Mon Sep 17 00:00:00 2001 From: feixyz10 Date: Fri, 9 Oct 2020 21:04:25 +0800 Subject: [PATCH] feature points extractor: writing... --- .gitignore | 1 + common/filter.h | 43 ++++++++++++++++ common/tic_toc.h | 25 ++++++++++ common/types.h | 63 ++++++++++++++++++++++++ common/utils.h | 37 ++++++++++++++ src/base_feature_extractor.cc | 93 +++++++++++++++++++++++++++++++++++ src/base_feature_extractor.h | 47 ++++++++++++++++++ src/feature_extractor_VLP16.h | 20 ++++++++ 8 files changed, 329 insertions(+) create mode 100644 common/filter.h create mode 100644 common/tic_toc.h create mode 100644 common/types.h create mode 100644 common/utils.h create mode 100644 src/base_feature_extractor.cc create mode 100644 src/base_feature_extractor.h create mode 100644 src/feature_extractor_VLP16.h diff --git a/.gitignore b/.gitignore index e69de29..0dc7b4b 100644 --- a/.gitignore +++ b/.gitignore @@ -0,0 +1 @@ +/.vscode \ No newline at end of file diff --git a/common/filter.h b/common/filter.h new file mode 100644 index 0000000..e1ea1b9 --- /dev/null +++ b/common/filter.h @@ -0,0 +1,43 @@ +#pragma once + +#include "common/utils.h" + +namespace oh_loam { + +template +void RemovePointsIf(const pcl::PointCloud& cloud_in, + pcl::PointCloud* const cloud_out, + std::function cond) { + if (&cloud_in != cloud_out) { + cloud_out->header = cloud_in.header; + cloud_out->points.resize(cloud_in.points.size()); + } + size_t j = 0; + for (size_t i = 0; i < cloud_in.points.size(); ++i) { + const auto pt = cloud_in.points[i]; + if (cond(pt)) continue; + cloud_out->points[j++] = pt; + } + cloud_out->points.resize(j); + cloud_out->height = 1; + cloud_out->widht = static_cast(j); + cloud_out->is_dense = true; +} + +template +void RemoveNaNPoint(const pcl::PointCloud& cloud_in, + pcl::PointCloud* const cloud_out) { + RemovePointsIf(cloud_in, cloud_out, + [](const PointT& pt) { return !IsFinite(pt); }); +} + +template +void RemoveClosedPoints(const pcl::PointCloud& cloud_in, + pcl::PointCloud* const cloud_out, + double min_dist = 0.1) { + RemovePointsIf(cloud_in, cloud_out, [](const PointT& pt) { + return DistanceSqure(pt) < min_dist * min_dist; + }); +} + +} // oh_loam \ No newline at end of file diff --git a/common/tic_toc.h b/common/tic_toc.h new file mode 100644 index 0000000..2a9a8f8 --- /dev/null +++ b/common/tic_toc.h @@ -0,0 +1,25 @@ +#pragma once + +#include +#include +#include + +namespace oh_loam { + +class TicToc { + public: + TicToc() { tic(); } + + void tic() { start_ = std::chrono::system_clock::now(); } + + double toc() { + end_ = std::chrono::system_clock::now(); + std::chrono::duration elapsed_seconds = end_ - start_; + return elapsed_seconds.count() * 1000; + } + + private: + std::chrono::time_point start_, end_; +}; + +} // oh_loam \ No newline at end of file diff --git a/common/types.h b/common/types.h new file mode 100644 index 0000000..2ceeb2a --- /dev/null +++ b/common/types.h @@ -0,0 +1,63 @@ +#pragma once + +#include +#include + +namespace oh_loam { + +using Point = pcl::PointXYZ; +using PointCloud = pcl::PointCloud; +using PointCloudPtr = PointCloud::Ptr; +using PointCloudConstPtr = PointCloud::ConstPtr; + +enum class PointType { + FLAT = -2, + LESS_FLAT = -1, + NORNAL = 0, + LESS_SHARP = 1, + SHARP = 2, +} + +struct EIGEN_ALIGN16 PointXYZTCT { + PCL_ADD_POINT4D; + float time = 0.0f; + float curvature = NAN; + char type = PointType::NORNAL; + + PointXYZTCT() { + x = y = z = 0.0f; + data[3] = 1.0f; + } + + PointXYZTCT(float x, float y, float z, float time = 0.0f, + float curvature = NAN, PointType type = PointType::NORNAL) + : x(x), y(y), z(z), time(time), curvature(curvature), type(type) { + data[3] = 1.0f; + } + + PointXYZTCT(const Point& p) { + x = p.x; + y = p.y; + z = p.z; + data[3] = 1.0f; + } + + PointXYZTCT(const PointXYZTCT& p) { + x = p.x; + y = p.y; + z = p.z; + time = p.time; + curvature = p.curvature; + type = p.type; + data[3] = 1.0f; + } + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +using IPoint = pcl::PointXYZTCT; +using IPointCloud = pcl::PointCloud; +using IPointCloudPtr = IPointCloud::Ptr; +using IPointCloudConstPtr = IPointCloud::ConstPtr; + +} // oh_loam \ No newline at end of file diff --git a/common/utils.h b/common/utils.h new file mode 100644 index 0000000..b0e824e --- /dev/null +++ b/common/utils.h @@ -0,0 +1,37 @@ +#pragma once + +#include "common/types.h" + +namespace oh_loam { + +template +inline double Distance(const PointT& pt) { + return std::hypot(pt.x, pt.y, pt.z); +} + +template +inline double DistanceSqure(const PointT& pt) { + return pt.x * pt.x + pt.y * pt.y + pt.z * pt.z; +} + +template +inline double IsFinite(const PointT& pt) { + return std::isfinite(pt.x) && std::isfinite(pt.y) && std::isfinite(pt.z); +} + +// normalize an angle to [-pi, pi) +inline double NormalizeAngle(double ang) { + const double& two_pi = 2 * M_PI; + return ang - two_pi * std::floor((ang + M_PI) / two_pi); +} + +std::pair GetYawRange(const PointCloud& cloud) { + const auto& pts = cloud.points; + int pt_num = pts.size(); + double yaw_start = -atan2(pts[0].y, pt[0].x); + double yaw_end = -atan2(pts[pt_num - 1].y, pts[pt_num - 1].x) + 2 * M_PI; + double yaw_diff = NormalizeAngle(yaw_end - yaw_start); + return {yaw_start, yaw_start + yaw_diff + 2 * M_PI}; +} + +} // oh_loam \ No newline at end of file diff --git a/src/base_feature_extractor.cc b/src/base_feature_extractor.cc new file mode 100644 index 0000000..c4433ca --- /dev/null +++ b/src/base_feature_extractor.cc @@ -0,0 +1,93 @@ +#include "src/base_feature_extractor.h" + +#include +#include "common/filter.h" + +namespace oh_loam { + +const double kPointMinDist = 0.1; +const int kScanSegNum = 6; + +bool FeaturePointsExtractor::Extract(const PointCloud& cloud_in, + FeaturePoints* const feature) const { + PointCloudPtr cloud(new PointCloud); + RemoveNaNPoint(cloud_in, cloud.get()); + RemoveClosedPoints(*cloud, cloud.get(), kPointMinDist); + std::vector scans; + ScanSplit(*cloud, &scans); + for (auto& scan : sccans) { + ComputeCurvature(&scan); + AssignType(&scan); + } + for (const auto& scan : sccans) { + *(feature->laser_cloud) += scan; + for (const auto& pt : scan.points) { + switch (pt.type) { + case PointType::FLAT: + feature->flat_surf_points.emplace_back(pt); + break; + case PointType::LESS_FLAT: + feature->less_flat_surf_points.emplace_back(pt); + break; + case PointType::LESS_SHARP: + feature->less_sharp_corner_points.emplace_back(pt); + break; + case PointType::SHARP: + feature->sharp_corner_points.emplace_back(pt); + break; + default: + break; + } + } + } +} + +void FeaturePointsExtractor::SplitScan( + const PointCloud& cloud, std::vector* const scans) const { + scans.resize(num_scans_); + const auto & [ yaw_start, yaw_end ] = GetYawRange(cloud); + const double yaw_range = yaw_end - yaw_start; + bool half_passed = false; + for (const auto& pt : cloud.points) { + int scan_id = GetScanID(pt); + if (scan_id >= num_scans_ || scan_id < 0) continue; + double yaw = -atan2(pt.y, pt.x); + double yaw_diff = NormalizeAngle(yaw - yaw_start); + if (yaw_diff > 0) { + if (half_passed) yaw_start += 2 * M_PI; + } else { + half_passed = true; + yaw_start += 2 * M_PI; + } + scans[scan_id].emplace_back(pt.x, pt.y, pt.z, yaw_diff / yaw_range); + } +} + +void FeaturePointsExtractor::ComputePointCurvature( + IPointCloud* const scan) const { + auto& pts = scan->points; + for (int 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::AssignPointType(IPointCloud* const scan) const { + int pt_num = scan->size(); + int pt_num_seg = pt_num / kScanSegNum; + std::vector 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; + } +} + +} // oh_loam diff --git a/src/base_feature_extractor.h b/src/base_feature_extractor.h new file mode 100644 index 0000000..712f42e --- /dev/null +++ b/src/base_feature_extractor.h @@ -0,0 +1,47 @@ +#pragma once + +#include "common/utils.h" + +namespace oh_loam { + +struct FeaturePoints { + IPointCloudPtr laser_cloud; + IPointCloudPtr sharp_corner_points; + IPointCloudPtr less_sharp_corner_points; + IPointCloudPtr flat_surf_points; + IPointCloudPtr less_flat_surf_points; + + FeaturePoints() { + laser_cloud.reset(new IPointCloud); + sharp_corner_points.reset(new IPointCloud); + less_sharp_corner_points.reset(new IPointCloud); + flat_surf_points.reset(new IPointCloud); + less_flat_surf_points.reset(new IPointCloud); + } +}; + +class FeaturePointsExtractor { + public: + FeaturePointsExtractor() = default; + virtual ~FeaturePointsExtractor() = default; + FeaturePointsExtractor(const FeaturePointsExtractor&) = delete; + FeaturePointsExtractor& operator=(const FeaturePointsExtractor&) = delete; + + bool Extract(const PointCloud& cloud_in, FeaturePoints* const feature) const; + + int num_scans() const { return num_scans_; } + + protected: + virtual GetScanID(const Point& pt) const = 0; + + void SplitScan(const PointCloud& cloud, + std::vector* const scans) const; + + void ComputePointCurvature(IPointCloud* const scan) const; + + void AssignPointType(IPointCloud* const scan) const; + + int num_scans_ = 0; +}; + +} // oh_loam \ No newline at end of file diff --git a/src/feature_extractor_VLP16.h b/src/feature_extractor_VLP16.h new file mode 100644 index 0000000..a4307e0 --- /dev/null +++ b/src/feature_extractor_VLP16.h @@ -0,0 +1,20 @@ +#pragma once + +#include "common/utils.h" +#include "src/base_feature_extractor.h" + +namespace oh_loam { + +// for VLP-16 +class FeatureExtractorVLP16 : public FeatureExtractor { + public: + FeatureExtractorVLP16() { num_scans_ = 16; } + + private: + int GetScanID(const Point& pt) const override final { + double omega = std::atan2(pt.z, Distance(pt)) * 180 * M_1_PI + 15.0; + return static_cast(std::round(omega) + 0.01); + } +} + +} // oh_loam \ No newline at end of file