diff --git a/.gitignore b/.gitignore index 0dc7b4b..4ffc326 100644 --- a/.gitignore +++ b/.gitignore @@ -1 +1,3 @@ -/.vscode \ No newline at end of file +/.vscode +/lib +/build \ No newline at end of file diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..f16260d --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,23 @@ +cmake_minimum_required(VERSION 2.8.3) +project(oh_loam) + +set(CMAKE_BUILD_TYPE "Release") +set(CMAKE_CXX_FLAGS "-std=c++17") +set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g") + +find_package(PCL QUIET) +if (NOT PCL_FOUND) + message(FATAL_ERROR "PCL not found.") +endif() + +include_directories( + ${PROJECT_SOURCE_DIR}/common + ${PROJECT_SOURCE_DIR}/src + ${PCL_INCLUDE_DIRS} +) + +set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/lib) + +add_library(${PROJECT_NAME} SHARED + ${PROJECT_SOURCE_DIR}/src/base_feature_extractor.cc +) \ No newline at end of file diff --git a/common/filter.h b/common/filter.h index e1ea1b9..6439d18 100644 --- a/common/filter.h +++ b/common/filter.h @@ -1,6 +1,6 @@ #pragma once -#include "common/utils.h" +#include "utils.h" namespace oh_loam { @@ -20,22 +20,22 @@ void RemovePointsIf(const pcl::PointCloud& cloud_in, } cloud_out->points.resize(j); cloud_out->height = 1; - cloud_out->widht = static_cast(j); + cloud_out->width = 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); }); + 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) { + RemovePointsIf(cloud_in, cloud_out, [&](const PointT& pt) { return DistanceSqure(pt) < min_dist * min_dist; }); } diff --git a/common/types.h b/common/types.h index 2ceeb2a..2854f76 100644 --- a/common/types.h +++ b/common/types.h @@ -1,5 +1,6 @@ #pragma once +#include #include #include @@ -16,13 +17,13 @@ enum class PointType { 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; + PointType type = PointType::NORNAL; PointXYZTCT() { x = y = z = 0.0f; @@ -55,7 +56,7 @@ struct EIGEN_ALIGN16 PointXYZTCT { EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; -using IPoint = pcl::PointXYZTCT; +using IPoint = PointXYZTCT; using IPointCloud = pcl::PointCloud; using IPointCloudPtr = IPointCloud::Ptr; using IPointCloudConstPtr = IPointCloud::ConstPtr; diff --git a/common/utils.h b/common/utils.h index b0e824e..a3ba97c 100644 --- a/common/utils.h +++ b/common/utils.h @@ -1,6 +1,6 @@ #pragma once -#include "common/types.h" +#include "types.h" namespace oh_loam { @@ -28,7 +28,7 @@ inline double NormalizeAngle(double ang) { 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_start = -atan2(pts[0].y, pts[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}; diff --git a/src/base_feature_extractor.cc b/src/base_feature_extractor.cc index c4433ca..9f637c2 100644 --- a/src/base_feature_extractor.cc +++ b/src/base_feature_extractor.cc @@ -1,7 +1,7 @@ -#include "src/base_feature_extractor.h" +#include "base_feature_extractor.h" #include -#include "common/filter.h" +#include "filter.h" namespace oh_loam { @@ -14,26 +14,26 @@ bool FeaturePointsExtractor::Extract(const PointCloud& cloud_in, RemoveNaNPoint(cloud_in, cloud.get()); RemoveClosedPoints(*cloud, cloud.get(), kPointMinDist); std::vector scans; - ScanSplit(*cloud, &scans); - for (auto& scan : sccans) { - ComputeCurvature(&scan); - AssignType(&scan); + SplitScan(*cloud, &scans); + for (auto& scan : scans) { + ComputePointCurvature(&scan); + AssignPointType(&scan); } - for (const auto& scan : sccans) { + for (const auto& scan : scans) { *(feature->laser_cloud) += scan; for (const auto& pt : scan.points) { switch (pt.type) { case PointType::FLAT: - feature->flat_surf_points.emplace_back(pt); + feature->flat_surf_points->points.emplace_back(pt); break; case PointType::LESS_FLAT: - feature->less_flat_surf_points.emplace_back(pt); + feature->less_flat_surf_points->points.emplace_back(pt); break; case PointType::LESS_SHARP: - feature->less_sharp_corner_points.emplace_back(pt); + feature->less_sharp_corner_points->points.emplace_back(pt); break; case PointType::SHARP: - feature->sharp_corner_points.emplace_back(pt); + feature->sharp_corner_points->points.emplace_back(pt); break; default: break; @@ -44,8 +44,9 @@ bool FeaturePointsExtractor::Extract(const PointCloud& cloud_in, void FeaturePointsExtractor::SplitScan( const PointCloud& cloud, std::vector* const scans) const { - scans.resize(num_scans_); - const auto & [ yaw_start, yaw_end ] = GetYawRange(cloud); + scans->resize(num_scans_); + double yaw_start, yaw_end; + std::tie(yaw_start, yaw_end) = GetYawRange(cloud); const double yaw_range = yaw_end - yaw_start; bool half_passed = false; for (const auto& pt : cloud.points) { @@ -59,14 +60,15 @@ void FeaturePointsExtractor::SplitScan( half_passed = true; yaw_start += 2 * M_PI; } - scans[scan_id].emplace_back(pt.x, pt.y, pt.z, yaw_diff / yaw_range); + (*scans)[scan_id].points.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) { + 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; diff --git a/src/base_feature_extractor.h b/src/base_feature_extractor.h index 712f42e..2e61c9b 100644 --- a/src/base_feature_extractor.h +++ b/src/base_feature_extractor.h @@ -1,6 +1,6 @@ #pragma once -#include "common/utils.h" +#include "utils.h" namespace oh_loam { @@ -32,7 +32,7 @@ class FeaturePointsExtractor { int num_scans() const { return num_scans_; } protected: - virtual GetScanID(const Point& pt) const = 0; + virtual int GetScanID(const Point& pt) const = 0; void SplitScan(const PointCloud& cloud, std::vector* const scans) const; diff --git a/src/feature_extractor_VLP16.h b/src/feature_extractor_VLP16.h index a4307e0..3df5352 100644 --- a/src/feature_extractor_VLP16.h +++ b/src/feature_extractor_VLP16.h @@ -1,7 +1,7 @@ #pragma once -#include "common/utils.h" -#include "src/base_feature_extractor.h" +#include "base_feature_extractor.h" +#include "utils.h" namespace oh_loam {