From 6c544c56eb7be433379d0b15e8a61dcbd1a17c07 Mon Sep 17 00:00:00 2001 From: feixyz10 Date: Tue, 5 Jan 2021 02:09:40 +0800 Subject: [PATCH] refactoring: extractor ok --- CMakeLists.txt | 2 +- common/CMakeLists.txt | 2 -- common/config/yaml_config.h | 1 + common/macro/macros.h | 3 +- common/pcl/pcl_types.h | 2 +- common/pcl/pcl_utils.h | 12 ++++---- common/time/timer.cc | 5 ++-- common/time/timer.h | 1 + common/visualizer/lidar_visualizer.h | 2 +- common/visualizer/lidar_visualizer_utils.h | 1 + oh_my_loam/base/feature.h | 2 +- oh_my_loam/extractor/extractor.cc | 35 +++++++++++----------- oh_my_loam/extractor/extractor.h | 2 +- oh_my_loam/extractor/extractor_VLP16.cc | 2 +- oh_my_loam/extractor/extractor_VLP16.h | 2 +- 15 files changed, 38 insertions(+), 36 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 1f55639..46dd37d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 3.15) +cmake_minimum_required(VERSION 3.10) project(oh_my_loam) set(CMAKE_CXX_FLAGS "-std=c++17 -Wall -lpthread") diff --git a/common/CMakeLists.txt b/common/CMakeLists.txt index 1741fa4..dbc26d6 100644 --- a/common/CMakeLists.txt +++ b/common/CMakeLists.txt @@ -2,6 +2,4 @@ file(GLOB SRC_FILES **/*.cc) message(STATUS "common dir: " ${CMAKE_CURRENT_SOURCE_DIR}) -# include_directories(${CMAKE_CURRENT_SOURCE_DIR}) - add_library(common SHARED ${SRC_FILES}) diff --git a/common/config/yaml_config.h b/common/config/yaml_config.h index 68af038..b186a65 100644 --- a/common/config/yaml_config.h +++ b/common/config/yaml_config.h @@ -1,6 +1,7 @@ #pragma once #include + #include #include "common/log/log.h" diff --git a/common/macro/macros.h b/common/macro/macros.h index 5de26b8..b7108ec 100644 --- a/common/macro/macros.h +++ b/common/macro/macros.h @@ -5,8 +5,7 @@ #include // format timestamp -#define FMT_TIMESTAMP(timestamp) \ - std::fixed << std::setprecision(3) << timestamp; +#define FMT_TIMESTAMP(timestamp) std::fixed << std::setprecision(3) << timestamp #define DISALLOW_COPY_AND_ASSIGN(classname) \ classname(const classname &) = delete; \ diff --git a/common/pcl/pcl_types.h b/common/pcl/pcl_types.h index ee94125..5cf5cb1 100644 --- a/common/pcl/pcl_types.h +++ b/common/pcl/pcl_types.h @@ -71,6 +71,6 @@ POINT_CLOUD_REGISTER_POINT_STRUCT( (float, y, y) (float, z, z) (float, time, time) - // (float, intensity, intensity) + (float, intensity, intensity) ) // clang-format on \ No newline at end of file diff --git a/common/pcl/pcl_utils.h b/common/pcl/pcl_utils.h index 4e78b95..a536709 100644 --- a/common/pcl/pcl_utils.h +++ b/common/pcl/pcl_utils.h @@ -1,6 +1,6 @@ #pragma once -#include "log/log.h" +#include "common/log/log.h" #include "pcl_types.h" namespace common { @@ -38,16 +38,16 @@ inline double IsFinite(const PointType& pt) { // Remove point if the condition evaluated to true on it template -void RemovePoints(const typename pcl::PointCloud::ConstPtr cloud_in, +void RemovePoints(const pcl::PointCloud& cloud_in, std::function check, pcl::PointCloud* const cloud_out) { - if (cloud_in.get() != cloud_out) { + if (&cloud_in != cloud_out) { cloud_out->header = cloud_in.header; - cloud_out->resize(cloud_in->size()); + cloud_out->resize(cloud_in.size()); } size_t j = 0; - for (size_t i = 0; i < cloud_in->size(); ++i) { - const auto pt = cloud_in->points[i]; + for (size_t i = 0; i < cloud_in.size(); ++i) { + const auto pt = cloud_in.points[i]; if (check(pt)) continue; cloud_out->points[j++] = pt; } diff --git a/common/time/timer.cc b/common/time/timer.cc index bf451d2..057ae25 100644 --- a/common/time/timer.cc +++ b/common/time/timer.cc @@ -1,4 +1,5 @@ #include "timer.h" + #include "common/log/log.h" namespace common { @@ -18,10 +19,10 @@ double Timer::Toc(char unit) { TimerWrapper::~TimerWrapper() { double duration = timer_.Toc(); if (duration_ms_ < 0) { - AINFO << msg_ << ": time elapsed (ms): " << FMT_TIMESTAMP(duration); + AINFO << msg_ << ": time elapsed: " << FMT_TIMESTAMP(duration) << " ms"; } if (duration_ms_ > 0 && duration > duration_ms_) { - AWARN << msg_ << ": time elapsed (ms): " << FMT_TIMESTAMP(duration); + AWARN << msg_ << ": time elapsed: " << FMT_TIMESTAMP(duration) << " ms"; } } diff --git a/common/time/timer.h b/common/time/timer.h index bd83f6d..8888480 100644 --- a/common/time/timer.h +++ b/common/time/timer.h @@ -2,6 +2,7 @@ #include #include + #include "common/macro/macros.h" namespace common { diff --git a/common/visualizer/lidar_visualizer.h b/common/visualizer/lidar_visualizer.h index 4521d37..fd6143f 100644 --- a/common/visualizer/lidar_visualizer.h +++ b/common/visualizer/lidar_visualizer.h @@ -73,7 +73,7 @@ class LidarVisualizer { */ virtual void Draw() { auto frame = GetCurrentFrame(); - DrawPointCloud(frame.cloud, {255, 255, 255}, "point cloud"); + DrawPointCloud(frame.cloud, WHITE, "point cloud"); } /** diff --git a/common/visualizer/lidar_visualizer_utils.h b/common/visualizer/lidar_visualizer_utils.h index d771bd1..92bb734 100644 --- a/common/visualizer/lidar_visualizer_utils.h +++ b/common/visualizer/lidar_visualizer_utils.h @@ -2,6 +2,7 @@ #include #include + #include #include #include diff --git a/oh_my_loam/base/feature.h b/oh_my_loam/base/feature.h index 56ac26f..697dbd5 100644 --- a/oh_my_loam/base/feature.h +++ b/oh_my_loam/base/feature.h @@ -10,7 +10,7 @@ struct Feature { common::TPointCloudPtr cloud_flat_surf; common::TPointCloudPtr cloud_less_flat_surf; - FeaturePoints() { + Feature() { cloud_sharp_corner.reset(new common::TPointCloud); cloud_less_sharp_corner.reset(new common::TPointCloud); cloud_flat_surf.reset(new common::TPointCloud); diff --git a/oh_my_loam/extractor/extractor.cc b/oh_my_loam/extractor/extractor.cc index 557a55f..16a6af3 100644 --- a/oh_my_loam/extractor/extractor.cc +++ b/oh_my_loam/extractor/extractor.cc @@ -1,9 +1,10 @@ #include "extractor.h" -#include "base/helper.h" - #include +#include "base/helper.h" +#include "common/pcl/pcl_utils.h" + namespace oh_my_loam { namespace { @@ -35,12 +36,12 @@ void Extractor::Process(const PointCloudConstPtr& cloud, SplitScan(*cloud, &scans); // compute curvature for each point in each scan for (auto& scan : scans) { - ComputePointCurvature(&scan); + ComputeCurvature(&scan); } // assign type to each point in each scan: FLAT, LESS_FLAT, // NORMAL, LESS_SHARP or SHARP for (auto& scan : scans) { - AssignPointType(&scan); + AssignType(&scan); } // store points into feature point clouds according to their type for (const auto& scan : scans) { @@ -89,9 +90,9 @@ void Extractor::ComputeCurvature(TCTPointCloud* const scan) const { pts[i + 4].z + pts[i + 5].z - 10 * pts[i].z; pts[i].curvature = std::sqrt(dx * dx + dy * dy + dz * dz); } - RemovePoints(*scan, scan, [](const TCTPoint& pt) { - return !std::isfinite(pt.curvature); - }); + RemovePoints( + *scan, [](const TCTPoint& pt) { return !std::isfinite(pt.curvature); }, + scan); AINFO_IF(verbose_) << "Extractor::ComputeCurvature: " << FMT_TIMESTAMP(timer.Toc()) << " ms"; } @@ -126,9 +127,9 @@ void Extractor::AssignType(TCTPointCloud* const scan) const { scan->at(ix).curvature > corner_point_curvature_thres) { ++corner_pt_picked_num; if (corner_pt_picked_num <= sharp_corner_point_num) { - scan->at(ix).type = PointType::SHARP; + scan->at(ix).type = Type::SHARP; } else if (corner_pt_picked_num <= corner_point_num) { - scan->at(ix).type = PointType::LESS_SHARP; + scan->at(ix).type = Type::LESS_SHARP; } else { break; } @@ -144,9 +145,9 @@ void Extractor::AssignType(TCTPointCloud* const scan) const { scan->at(ix).curvature < surf_point_curvature_thres) { ++surf_pt_picked_num; if (surf_pt_picked_num <= flat_surf_point_num) { - scan->at(ix).type = PointType::FLAT; + scan->at(ix).type = Type::FLAT; } else if (surf_pt_picked_num <= surf_point_num) { - scan->at(ix).type = PointType::LESS_FLAT; + scan->at(ix).type = Type::LESS_FLAT; } else { break; } @@ -164,18 +165,18 @@ void Extractor::GenerateFeature(const TCTPointCloud& scan, Timer timer; TPointCloudPtr cloud_less_flat_surf(new TPointCloud); for (const auto& pt : scan.points) { - TPoint point(pt.x, pt, y, pt.z, pt.time); + TPoint point(pt.x, pt.y, pt.z, pt.time); switch (pt.type) { - case PointType::FLAT: + case Type::FLAT: feature->cloud_flat_surf->points.emplace_back(point); // no break: FLAT points are also LESS_FLAT - case PointType::LESS_FLAT: + case Type::LESS_FLAT: cloud_less_flat_surf->points.emplace_back(point); break; - case PointType::SHARP: + case Type::SHARP: feature->cloud_sharp_corner->points.emplace_back(point); // no break: SHARP points are also LESS_SHARP - case PointType::LESS_SHARP: + case Type::LESS_SHARP: feature->cloud_less_sharp_corner->points.emplace_back(point); break; default: @@ -196,7 +197,7 @@ void Extractor::Visualize(const PointCloudConstPtr& cloud, const Feature& feature, double timestamp) { std::shared_ptr frame(new ExtractorVisFrame); frame->timestamp = timestamp; - frame->cloud = cloud; + frame->cloud = cloud->makeShared(); frame->feature = feature; visualizer_->Render(frame); } diff --git a/oh_my_loam/extractor/extractor.h b/oh_my_loam/extractor/extractor.h index 034badb..90eb2d7 100644 --- a/oh_my_loam/extractor/extractor.h +++ b/oh_my_loam/extractor/extractor.h @@ -19,7 +19,7 @@ class Extractor { int num_scans() const { return num_scans_; } protected: - virtual int GetScanID(const Point& pt) const = 0; + virtual int GetScanID(const common::Point& pt) const = 0; YAML::Node config_; diff --git a/oh_my_loam/extractor/extractor_VLP16.cc b/oh_my_loam/extractor/extractor_VLP16.cc index 3483219..a0d35f1 100644 --- a/oh_my_loam/extractor/extractor_VLP16.cc +++ b/oh_my_loam/extractor/extractor_VLP16.cc @@ -2,7 +2,7 @@ namespace oh_my_loam { -int ExtractorVLP16::GetScanID(const Point& pt) const { +int ExtractorVLP16::GetScanID(const common::Point& pt) const { double omega = std::atan2(pt.z, std::hypot(pt.x, pt.y)) * 180 * M_1_PI + 15.0; return static_cast(std::round(omega / 2.0) + 1.e-5); }; diff --git a/oh_my_loam/extractor/extractor_VLP16.h b/oh_my_loam/extractor/extractor_VLP16.h index cd679ef..31ed5f4 100644 --- a/oh_my_loam/extractor/extractor_VLP16.h +++ b/oh_my_loam/extractor/extractor_VLP16.h @@ -10,7 +10,7 @@ class ExtractorVLP16 : public Extractor { ExtractorVLP16() { num_scans_ = 16; } private: - int GetScanID(const Point& pt) const override; + int GetScanID(const common::Point& pt) const override; }; } // namespace oh_my_loam \ No newline at end of file