From 397abb2bf3f6ed814e28262f2f38c2a61a3e55fe Mon Sep 17 00:00:00 2001 From: feixyz10 Date: Mon, 12 Oct 2020 21:09:32 +0800 Subject: [PATCH] add ros --- CMakeLists.txt | 50 +++++++--- common/CMakeLists.txt | 0 common/filter.h | 9 +- common/tic_toc.h | 4 +- common/types.h | 53 ++++++++--- common/utils.h | 40 +++++--- main.cc | 31 ++++++ package.xml | 39 ++++++++ src/CMakeLists.txt | 7 ++ src/base_feature_extractor.cc | 95 ------------------- src/base_feature_extractor.h | 47 --------- src/feature_extractor_VLP16.h | 19 ++-- src/feature_extractor_base.cc | 101 ++++++++++++++++++++ src/feature_extractor_base.h | 39 ++++++++ src/oh_my_loam.cc | 31 ++++++ src/oh_my_loam.h | 29 ++++++ src/visualizer_base.h | 163 ++++++++++++++++++++++++++++++++ src/visualizer_feature_points.h | 60 ++++++++++++ 18 files changed, 625 insertions(+), 192 deletions(-) create mode 100644 common/CMakeLists.txt create mode 100644 main.cc create mode 100644 package.xml create mode 100644 src/CMakeLists.txt delete mode 100644 src/base_feature_extractor.cc delete mode 100644 src/base_feature_extractor.h create mode 100644 src/feature_extractor_base.cc create mode 100644 src/feature_extractor_base.h create mode 100644 src/oh_my_loam.cc create mode 100644 src/oh_my_loam.h create mode 100644 src/visualizer_base.h create mode 100644 src/visualizer_feature_points.h diff --git a/CMakeLists.txt b/CMakeLists.txt index f16260d..7029e04 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,23 +1,47 @@ cmake_minimum_required(VERSION 2.8.3) -project(oh_loam) +project(oh_my_loam) set(CMAKE_BUILD_TYPE "Release") -set(CMAKE_CXX_FLAGS "-std=c++17") +set(CMAKE_CXX_FLAGS "-std=c++17 -lpthread") set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g") -find_package(PCL QUIET) -if (NOT PCL_FOUND) - message(FATAL_ERROR "PCL not found.") -endif() +find_package(Ceres REQUIRED) +find_package(PCL REQUIRED) -include_directories( - ${PROJECT_SOURCE_DIR}/common - ${PROJECT_SOURCE_DIR}/src - ${PCL_INCLUDE_DIRS} +find_package(catkin REQUIRED COMPONENTS + geometry_msgs + nav_msgs + sensor_msgs + roscpp + rospy + rosbag + std_msgs + image_transport + cv_bridge + tf ) -set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/lib) +include_directories( + ${catkin_INCLUDE_DIRS} + ${PCL_INCLUDE_DIRS} + ${CERES_INCLUDE_DIRS} + src + common +) -add_library(${PROJECT_NAME} SHARED - ${PROJECT_SOURCE_DIR}/src/base_feature_extractor.cc +catkin_package( + CATKIN_DEPENDS geometry_msgs nav_msgs roscpp rospy std_msgs + DEPENDS EIGEN3 PCL + INCLUDE_DIRS src common +) + +add_subdirectory(common) +add_subdirectory(src) + +add_executable(oh_my_loam main.cc) +target_link_libraries(oh_my_loam + ${catkin_LIBRARIES} + ${PCL_LIBRARIES} + ${CERES_LIBRARIES} + OhMyLoam ) \ No newline at end of file diff --git a/common/CMakeLists.txt b/common/CMakeLists.txt new file mode 100644 index 0000000..e69de29 diff --git a/common/filter.h b/common/filter.h index 6439d18..eff1a69 100644 --- a/common/filter.h +++ b/common/filter.h @@ -2,7 +2,7 @@ #include "utils.h" -namespace oh_loam { +namespace oh_my_loam { template void RemovePointsIf(const pcl::PointCloud& cloud_in, @@ -10,14 +10,15 @@ void RemovePointsIf(const pcl::PointCloud& cloud_in, std::function cond) { if (&cloud_in != cloud_out) { cloud_out->header = cloud_in.header; - cloud_out->points.resize(cloud_in.points.size()); + cloud_out->points.resize(cloud_in.size()); } size_t j = 0; - for (size_t i = 0; i < cloud_in.points.size(); ++i) { + for (size_t i = 0; i < cloud_in.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->width = static_cast(j); @@ -40,4 +41,4 @@ void RemoveClosedPoints(const pcl::PointCloud& cloud_in, }); } -} // oh_loam \ No newline at end of file +} // oh_my_loam \ No newline at end of file diff --git a/common/tic_toc.h b/common/tic_toc.h index 2a9a8f8..89d9b6c 100644 --- a/common/tic_toc.h +++ b/common/tic_toc.h @@ -4,7 +4,7 @@ #include #include -namespace oh_loam { +namespace oh_my_loam { class TicToc { public: @@ -22,4 +22,4 @@ class TicToc { std::chrono::time_point start_, end_; }; -} // oh_loam \ No newline at end of file +} // oh_my_loam \ No newline at end of file diff --git a/common/types.h b/common/types.h index 2854f76..710e0df 100644 --- a/common/types.h +++ b/common/types.h @@ -2,9 +2,11 @@ #include #include +#include +#include #include -namespace oh_loam { +namespace oh_my_loam { using Point = pcl::PointXYZ; using PointCloud = pcl::PointCloud; @@ -19,11 +21,37 @@ enum class PointType { SHARP = 2, }; +struct Color { + uint8_t r, g, b; + Color(uint8_t r, uint8_t g, uint8_t b) : r(r), g(g), b(b) {} +}; +#define BLACK Color(0, 0, 0) +#define WHITE Color(255, 255, 255) +#define RED Color(255, 0, 0) +#define GREEN Color(0, 255, 0) +#define BLUE Color(0, 0, 255) +#define GRAY Color(50, 50, 50) +#define CYAN Color(0, 255, 255) +#define PURPLE Color(160, 32, 240) +#define ORANGE Color(255, 97, 0) + +struct PointXYZTCT; + +using IPoint = PointXYZTCT; +using IPointCloud = pcl::PointCloud; +using IPointCloudPtr = IPointCloud::Ptr; +using IPointCloudConstPtr = IPointCloud::ConstPtr; + +using PCLVisualizer = pcl::visualization::PCLVisualizer; +#define PCLColorHandlerCustom pcl::visualization::PointCloudColorHandlerCustom +#define PCLColorHandlerGenericField \ + pcl::visualization::PointCloudColorHandlerGenericField + struct EIGEN_ALIGN16 PointXYZTCT { PCL_ADD_POINT4D; float time = 0.0f; - float curvature = NAN; - PointType type = PointType::NORNAL; + float curvature = std::nanf(""); + // PointType label = PointType::NORNAL; PointXYZTCT() { x = y = z = 0.0f; @@ -31,8 +59,8 @@ struct EIGEN_ALIGN16 PointXYZTCT { } 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) { + float curvature = NAN) + : x(x), y(y), z(z), time(time), curvature(curvature) { data[3] = 1.0f; } @@ -49,16 +77,19 @@ struct EIGEN_ALIGN16 PointXYZTCT { z = p.z; time = p.time; curvature = p.curvature; - type = p.type; + // type = p.type; data[3] = 1.0f; } EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; -using IPoint = PointXYZTCT; -using IPointCloud = pcl::PointCloud; -using IPointCloudPtr = IPointCloud::Ptr; -using IPointCloudConstPtr = IPointCloud::ConstPtr; +} // oh_my_loam -} // oh_loam \ No newline at end of file +// clang-format off +POINT_CLOUD_REGISTER_POINT_STRUCT( + oh_my_loam::PointXYZTCT, + (float, x, x)(float, y, y)(float, z, z) + (float, time, time)(float, curvature, curvatur) +) +// clang-format on \ No newline at end of file diff --git a/common/utils.h b/common/utils.h index a3ba97c..472cc15 100644 --- a/common/utils.h +++ b/common/utils.h @@ -2,18 +2,18 @@ #include "types.h" -namespace oh_loam { - -template -inline double Distance(const PointT& pt) { - return std::hypot(pt.x, pt.y, pt.z); -} +namespace oh_my_loam { template inline double DistanceSqure(const PointT& pt) { return pt.x * pt.x + pt.y * pt.y + pt.z * pt.z; } +template +inline double Distance(const PointT& pt) { + return std::sqrt(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); @@ -25,13 +25,25 @@ inline double NormalizeAngle(double ang) { 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, 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}; +template +void DrawPointCloud(const pcl::PointCloud& cloud, const Color& color, + const std::string& id, PCLVisualizer* const viewer, + int pt_size = 3) { + PCLColorHandlerCustom color_handler(cloud.makeShared(), color.r, + color.g, color.b); + viewer->addPointCloud(cloud.makeShared(), color_handler, id); + viewer->setPointCloudRenderingProperties( + pcl::visualization::PCL_VISUALIZER_POINT_SIZE, pt_size, id); } -} // oh_loam \ No newline at end of file +template +void DrawPointCloud(const pcl::PointCloud& cloud, + const std::string& field, const std::string& id, + PCLVisualizer* const viewer, int pt_size = 3) { + PCLColorHandlerGenericField color_handler(cloud.makeShared(), field); + viewer->addPointCloud(cloud.makeShared(), color_handler, id); + viewer->setPointCloudRenderingProperties( + pcl::visualization::PCL_VISUALIZER_POINT_SIZE, pt_size, id); +} + +} // oh_my_loam \ No newline at end of file diff --git a/main.cc b/main.cc new file mode 100644 index 0000000..3cd6537 --- /dev/null +++ b/main.cc @@ -0,0 +1,31 @@ +#include +#include +#include +#include +#include "oh_my_loam.h" + +void PointCloudHandler(const sensor_msgs::PointCloud2ConstPtr& msg, + oh_my_loam::OhMyLoam* const slam); + +int main(int argc, char* argv[]) { + oh_my_loam::OhMyLoam slam; + slam.Init(); + + ros::init(argc, argv, "oh_my_loam"); + ros::NodeHandle nh; + ros::Subscriber sub_point_cloud = nh.subscribe( + "/velodyne_points", 100, + std::bind(PointCloudHandler, std::placeholders::_1, &slam)); + ros::spin(); + + return 0; +} + +void PointCloudHandler(const sensor_msgs::PointCloud2ConstPtr& msg, + oh_my_loam::OhMyLoam* const slam) { + oh_my_loam::PointCloud cloud; + pcl::fromROSMsg(*msg, cloud); + ROS_INFO_STREAM("Point num = " << cloud.size() + << "ts = " << msg->header.stamp.toSec()); + slam->Run(cloud, 0.0); +} \ No newline at end of file diff --git a/package.xml b/package.xml new file mode 100644 index 0000000..79bf967 --- /dev/null +++ b/package.xml @@ -0,0 +1,39 @@ + + + oh_my_loam + 0.0.0 + + + Reimplementation of LOAM. + + + qintong + + BSD + + Ji Zhang + + catkin + geometry_msgs + nav_msgs + roscpp + rospy + std_msgs + rosbag + sensor_msgs + tf + image_transport + + geometry_msgs + nav_msgs + sensor_msgs + roscpp + rospy + std_msgs + rosbag + tf + image_transport + + + + diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt new file mode 100644 index 0000000..2b3852c --- /dev/null +++ b/src/CMakeLists.txt @@ -0,0 +1,7 @@ +include_directories( + ${PROJECT_SOURCE_DIR}/common +) + +aux_source_directory(. SRC_FILES) + +add_library(OhMyLoam SHARED ${SRC_FILES}) diff --git a/src/base_feature_extractor.cc b/src/base_feature_extractor.cc deleted file mode 100644 index 9f637c2..0000000 --- a/src/base_feature_extractor.cc +++ /dev/null @@ -1,95 +0,0 @@ -#include "base_feature_extractor.h" - -#include -#include "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; - SplitScan(*cloud, &scans); - for (auto& scan : scans) { - ComputePointCurvature(&scan); - AssignPointType(&scan); - } - 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->points.emplace_back(pt); - break; - case PointType::LESS_FLAT: - feature->less_flat_surf_points->points.emplace_back(pt); - break; - case PointType::LESS_SHARP: - feature->less_sharp_corner_points->points.emplace_back(pt); - break; - case PointType::SHARP: - feature->sharp_corner_points->points.emplace_back(pt); - break; - default: - break; - } - } - } -} - -void FeaturePointsExtractor::SplitScan( - const PointCloud& cloud, std::vector* const scans) const { - 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) { - 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].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 (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::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 deleted file mode 100644 index 2e61c9b..0000000 --- a/src/base_feature_extractor.h +++ /dev/null @@ -1,47 +0,0 @@ -#pragma once - -#include "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 int 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 index 3df5352..5110a65 100644 --- a/src/feature_extractor_VLP16.h +++ b/src/feature_extractor_VLP16.h @@ -1,20 +1,27 @@ #pragma once -#include "base_feature_extractor.h" +#include "feature_extractor_base.h" #include "utils.h" -namespace oh_loam { +namespace oh_my_loam { // for VLP-16 -class FeatureExtractorVLP16 : public FeatureExtractor { +class FeatureExtractorVLP16 : public FeaturePointsExtractor { public: FeatureExtractorVLP16() { num_scans_ = 16; } - private: + protected: int GetScanID(const Point& pt) const override final { + static int i = 0; double omega = std::atan2(pt.z, Distance(pt)) * 180 * M_1_PI + 15.0; + if (i++ < 10) + std::cout << "OMEGA: " + << std::atan2(pt.z, Distance(pt)) * 180 * M_1_PI + 15.0 + << " id = " << static_cast(std::round(omega) + 0.01) + << " z = " << pt.z << " " + << " d = " << Distance(pt) << std::endl; return static_cast(std::round(omega) + 0.01); } -} +}; -} // oh_loam \ No newline at end of file +} // oh_my_loam \ No newline at end of file diff --git a/src/feature_extractor_base.cc b/src/feature_extractor_base.cc new file mode 100644 index 0000000..b230cab --- /dev/null +++ b/src/feature_extractor_base.cc @@ -0,0 +1,101 @@ +#include "feature_extractor_base.h" + +#include +#include "filter.h" + +namespace oh_my_loam { + +const double kPointMinDist = 0.1; +const int kScanSegNum = 6; +const double kTwoPi = kTwoPi; +const int kMinPtsNum = 100; + +void FeaturePointsExtractor::Extract(const PointCloud& cloud_in, + FeaturePoints* const feature) const { + PointCloudPtr cloud(new PointCloud); + std::cout << "BEFORE REMOVE, num = " << cloud->size() << std::endl; + RemoveNaNPoint(cloud_in, cloud.get()); + RemoveClosedPoints(*cloud, cloud.get(), kPointMinDist); + std::cout << "AFTER REMOVE, num = " << cloud->size() << std::endl; + if (cloud->size() < kMinPtsNum) { + return; + } + std::vector scans; + SplitScan(*cloud, &scans); + for (auto& scan : scans) { + std::cout << scan.size() << " "; + // ComputePointCurvature(&scan); + // AssignPointType(&scan); + } + std::cout << std::endl; + for (const auto& scan : scans) { + feature->feature_pts += scan; + // for (const auto& pt : scan.points) { + // switch (pt.type) { + // case PointType::FLAT: + // feature->flat_surf_pts->points.emplace_back(pt); + // break; + // case PointType::LESS_FLAT: + // feature->less_flat_surf_pts->points.emplace_back(pt); + // break; + // case PointType::LESS_SHARP: + // feature->less_sharp_corner_pts->points.emplace_back(pt); + // break; + // case PointType::SHARP: + // feature->sharp_corner_pts->points.emplace_back(pt); + // break; + // default: + // break; + // } + // } + } +} + +void FeaturePointsExtractor::SplitScan( + const PointCloud& cloud, std::vector* const scans) const { + scans->resize(num_scans_); + double yaw_start = -atan2(cloud.points[0].y, cloud.points[0].x); + 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 += kTwoPi; + } else { + half_passed = true; + yaw_start += kTwoPi; + } + (*scans)[scan_id].points.emplace_back(pt.x, pt.y, pt.z, yaw_diff / kTwoPi); + } +} + +// 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::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_my_loam diff --git a/src/feature_extractor_base.h b/src/feature_extractor_base.h new file mode 100644 index 0000000..9b54064 --- /dev/null +++ b/src/feature_extractor_base.h @@ -0,0 +1,39 @@ +#pragma once + +#include "utils.h" + +namespace oh_my_loam { + +struct FeaturePoints { + IPointCloud feature_pts; // all feature points + IPointCloud sharp_corner_pts; + IPointCloud less_sharp_corner_pts; + IPointCloud flat_surf_pts; + IPointCloud less_flat_surf_pts; +}; + +class FeaturePointsExtractor { + public: + FeaturePointsExtractor() = default; + virtual ~FeaturePointsExtractor() = default; + FeaturePointsExtractor(const FeaturePointsExtractor&) = delete; + FeaturePointsExtractor& operator=(const FeaturePointsExtractor&) = delete; + + void Extract(const PointCloud& cloud_in, FeaturePoints* const feature) const; + + int num_scans() const { return num_scans_; } + + protected: + virtual int 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_my_loam \ No newline at end of file diff --git a/src/oh_my_loam.cc b/src/oh_my_loam.cc new file mode 100644 index 0000000..f3f4bdf --- /dev/null +++ b/src/oh_my_loam.cc @@ -0,0 +1,31 @@ +#include "oh_my_loam.h" + +namespace oh_my_loam { + +bool OhMyLoam::Init() { + is_vis_ = false; + feature_extractor_.reset(new FeatureExtractorVLP16); + if (is_vis_) { + visualizer_.reset(new FeaturePointsVisualizer); + } + return true; +} + +void OhMyLoam::Run(const PointCloud& cloud, double timestamp) { + pcl::PointCloud cl; + std::shared_ptr feature_pts(new FeaturePoints); + feature_extractor_->Extract(cloud, feature_pts.get()); + if (is_vis_) Visualize(cloud, feature_pts, timestamp); +} + +void OhMyLoam::Visualize( + const PointCloud& cloud, + const std::shared_ptr& feature_pts, double timestamp) { + std::shared_ptr frame; + frame->timestamp = timestamp; + frame->cloud = cloud.makeShared(); + frame->feature_pts = feature_pts; + visualizer_->Render(frame); +} + +} // oh_my_loam \ No newline at end of file diff --git a/src/oh_my_loam.h b/src/oh_my_loam.h new file mode 100644 index 0000000..fb65e62 --- /dev/null +++ b/src/oh_my_loam.h @@ -0,0 +1,29 @@ +#pragma once + +#include "feature_extractor_VLP16.h" +#include "visualizer_feature_points.h" + +namespace oh_my_loam { + +class OhMyLoam { + public: + OhMyLoam() = default; + ~OhMyLoam() = default; + OhMyLoam(const OhMyLoam&) = delete; + OhMyLoam& operator=(const OhMyLoam&) = delete; + + bool Init(); + + void Run(const PointCloud& cloud, double timestamp); + + private: + void Visualize(const PointCloud& cloud, + const std::shared_ptr& feature_pts, + double timestamp = std::nanf("")); + + bool is_vis_ = false; + std::unique_ptr feature_extractor_ = nullptr; + std::unique_ptr visualizer_ = nullptr; +}; + +} // oh_my_loam \ No newline at end of file diff --git a/src/visualizer_base.h b/src/visualizer_base.h new file mode 100644 index 0000000..2573298 --- /dev/null +++ b/src/visualizer_base.h @@ -0,0 +1,163 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include "types.h" + +namespace oh_my_loam { + +struct VisFrame { + double timestamp = std::nanf(""); + PointCloudPtr cloud = nullptr; +}; + +template +class Visualizer { + public: + Visualizer(const std::string &name, size_t max_history_size) + : name_(name), max_history_size_(max_history_size) { + // Start the thread to draw + thread_.reset(new std::thread([&]() { + viewer_.reset(new PCLVisualizer(name_)); + // Set background color + const Color &bg_color = {0, 0, 0}; + viewer_->setBackgroundColor(bg_color.r, bg_color.g, bg_color.b); + // Set camera position. + viewer_->setCameraPosition(0, 0, 200, 0, 0, 0, 1, 0, 0, 0); + viewer_->setSize(2500, 1500); + viewer_->addCoordinateSystem(1.0); + // Add mouse and keyboard callback. + viewer_->registerKeyboardCallback( + [&](const pcl::visualization::KeyboardEvent &event) -> void { + KeyboardEventCallback(event); + }); + Run(); + })); + } + + virtual ~Visualizer() { + is_stopped_ = true; + viewer_->close(); + if (thread_->joinable()) { + thread_->join(); + } + } + + void Render(const std::shared_ptr &frame) { + std::lock_guard lock(mutex_); + while (history_frames_.size() > max_history_size_) { + history_frames_.pop_back(); + } + history_frames_.emplace_front(frame); + curr_frame_iter_ = history_frames_.begin(); + is_updated_ = true; + } + + std::string Name() { return name_; } + + protected: + void Run() { + while (!is_stopped_) { + if (is_updated_) { + RemoveRenderedObjects(); + Draw(); + is_updated_ = false; + } + viewer_->spinOnce(20); // ms + } + } + + /** + * @brief Draw objects. This method should be overrided for customization. + * + * virtual void Draw() { + * FrameT frame = GetCurrentFrame(); + * { // Update cloud + * std::string id = "point cloud"; + * DrawPointCloud(*frame.cloud, {255, 255, 255}, id, viewer_.get()); + * rendered_cloud_ids_.push_back(id); + * } + * } + */ + virtual void Draw() = 0; + + /** + * @brief Keyboard event callback function, This method should be overrided + * for customization + * + * @param event Keyboard event + */ + virtual void KeyboardEventCallback( + const pcl::visualization::KeyboardEvent &event) { + if (event.getKeySym() == "p" && event.keyDown()) { + std::lock_guard lock(mutex_); + ++curr_frame_iter_; + if (curr_frame_iter_ == history_frames_.end()) { + --curr_frame_iter_; + } + is_updated_ = true; + } else if (event.getKeySym() == "n" && event.keyDown()) { + std::lock_guard lock(mutex_); + if (curr_frame_iter_ != history_frames_.begin()) { + --curr_frame_iter_; + } + is_updated_ = true; + } else if (event.getKeySym() == "r" && event.keyDown()) { + viewer_->setCameraPosition(0, 0, 200, 0, 0, 0, 1, 0, 0, 0); + viewer_->setSize(2500, 1500); + is_updated_ = true; + } + } + + /** + * @brief Remove all old rendered point clouds + * + */ + void RemoveRenderedObjects() { + for (const auto &id : rendered_cloud_ids_) { + viewer_->removePointCloud(id); + } + rendered_cloud_ids_.clear(); + viewer_->removeAllShapes(); + } + + FrameT GetCurrentFrame() const { + std::lock_guard lock(mutex_); + return *(*curr_frame_iter_); + } + + // visualizer name + std::string name_; + size_t max_history_size_; + + // atomic bool variable + std::atomic_bool is_stopped_{false}; + + // mutex for visualizer_frame. + mutable std::mutex mutex_; + + // bool flag indicates whether the Visualizer frame is updated. + std::atomic_bool is_updated_{false}; + + // The visualizer frame list + std::deque> history_frames_; + + // The current displayed frame iter. + typename std::deque>::const_iterator curr_frame_iter_; + + // The rendered cloud ids. + std::vector rendered_cloud_ids_; + + // thread for visualization + std::unique_ptr thread_ = nullptr; + + // viewer + std::unique_ptr viewer_ = nullptr; +}; + +} // oh_my_loam \ No newline at end of file diff --git a/src/visualizer_feature_points.h b/src/visualizer_feature_points.h new file mode 100644 index 0000000..85ce447 --- /dev/null +++ b/src/visualizer_feature_points.h @@ -0,0 +1,60 @@ +#pragma once + +#include "feature_extractor_base.h" +#include "visualizer_base.h" + +namespace oh_my_loam { + +struct FeaturePointsVisFrame : public VisFrame { + std::shared_ptr feature_pts; +}; + +class FeaturePointsVisualizer : public Visualizer { + public: + explicit FeaturePointsVisualizer( + const std::string &name = "FeaturePointsVisFrame", + size_t max_history_size = 10) + : Visualizer(name, max_history_size) {} + + protected: + void Draw() override final { + auto frame = GetCurrentFrame(); + { // add raw point cloud + std::string id = "raw point cloud"; + DrawPointCloud(*frame.cloud, WHITE, id, viewer_.get()); + rendered_cloud_ids_.push_back(id); + } + { // add all feature_pts + std::string id = "feature_pts"; + DrawPointCloud(frame.feature_pts->feature_pts, GRAY, id, + viewer_.get()); + rendered_cloud_ids_.push_back(id); + } + // { // add flat_surf_pts + // std::string id = "flat_surf_pts"; + // DrawPointCloud(*frame.feature_ptsflat_surf_pts, CYAN, id, + // viewer_.get()); + // rendered_cloud_ids_.push_back(id); + // } + // { // add less_flat_surf_pts + // std::string id = "less_flat_surf_pts"; + // DrawPointCloud(*frame.feature_ptsless_flat_surf_pts, GREEN, id, + // viewer_.get()); + // rendered_cloud_ids_.push_back(id); + // } + // { // add less_sharp_corner_pts + // std::string id = "less_sharp_corner_pts"; + // DrawPointCloud(*frame.feature_ptsless_sharp_corner_pts, ORANGE, id, + // viewer_.get()); + // rendered_cloud_ids_.push_back(id); + // } + // { // add sharp_corner_pts + // std::string id = "sharp_corner_pts"; + // DrawPointCloud(*frame.feature_pts.sharp_corner_pts, ORANGE, id, + // viewer_.get()); + // rendered_cloud_ids_.push_back(id); + // } + } +}; + +} // oh_my_loam