12345qiupeng
/
oh_my_loam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

refactoring: extractor ok

main
feixyz10 2021-01-05 02:09:40 +08:00 committed by feixyz
parent 64c06113dd
commit 6c544c56eb
15 changed files with 38 additions and 36 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
CMakeLists.txt
View File

@ -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")

2
common/CMakeLists.txt
View File

@ -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})

1
common/config/yaml_config.h
View File

@ -1,6 +1,7 @@
#pragma once
#include <yaml-cpp/yaml.h>
#include <string>
#include "common/log/log.h"

3
common/macro/macros.h
View File

@ -5,8 +5,7 @@
#include <mutex>
// 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; \

2
common/pcl/pcl_types.h
View File

@ -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

12
common/pcl/pcl_utils.h
View File

@ -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 <typename PointType>
void RemovePoints(const typename pcl::PointCloud<PointType>::ConstPtr cloud_in,
void RemovePoints(const pcl::PointCloud<PointType>& cloud_in,
std::function<bool(const PointType&)> check,
pcl::PointCloud<PointType>* 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;
}

5
common/time/timer.cc
View File

@ -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";
}
}

1
common/time/timer.h
View File

@ -2,6 +2,7 @@
#include <chrono>
#include <string>
#include "common/macro/macros.h"
namespace common {

2
common/visualizer/lidar_visualizer.h
View File

@ -73,7 +73,7 @@ class LidarVisualizer {
*/
virtual void Draw() {
auto frame = GetCurrentFrame<LidarVisFrame>();
DrawPointCloud(frame.cloud, {255, 255, 255}, "point cloud");
DrawPointCloud<Point>(frame.cloud, WHITE, "point cloud");
}
/**

1
common/visualizer/lidar_visualizer_utils.h
View File

@ -2,6 +2,7 @@
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/visualization/point_cloud_handlers.h>
#include <pcl/visualization/impl/pcl_visualizer.hpp>
#include <pcl/visualization/impl/point_cloud_geometry_handlers.hpp>
#include <string>

2
oh_my_loam/base/feature.h
View File

@ -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);

35
oh_my_loam/extractor/extractor.cc
View File

@ -1,9 +1,10 @@
#include "extractor.h"
#include "base/helper.h"
#include <cmath>
#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<TCTPoint>(*scan, scan, [](const TCTPoint& pt) {
return !std::isfinite(pt.curvature);
});
RemovePoints<TCTPoint>(
*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<ExtractorVisFrame> frame(new ExtractorVisFrame);
frame->timestamp = timestamp;
frame->cloud = cloud;
frame->cloud = cloud->makeShared();
frame->feature = feature;
visualizer_->Render(frame);
}

2
oh_my_loam/extractor/extractor.h
View File

@ -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_;

2
oh_my_loam/extractor/extractor_VLP16.cc
View File

@ -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<int>(std::round(omega / 2.0) + 1.e-5);
};

2
oh_my_loam/extractor/extractor_VLP16.h
View File

@ -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