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

extractor v1: ok

main
feixyz10 2020-10-18 01:14:43 +08:00 committed by feixyz
parent 93b707c82f
commit 2ff08e7484
29 changed files with 516 additions and 337 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
.gitignore vendored
View File

@ -1,3 +1,3 @@
/.vscode
/.log/*
/.log
/build

2
CMakeLists.txt
View File

@ -54,6 +54,6 @@ target_link_libraries(main
g3log
common
oh_my_loam
feature_points_extractor
extractor
visualizer
)

23
common/color.h Normal file
View File

@ -0,0 +1,23 @@
#pragma once
namespace oh_my_loam {
struct Color {
unsigned char r, g, b;
Color(unsigned char r, unsigned char g, unsigned char 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 VIOLET Color(238, 130, 238)
#define ORANGE Color(255, 97, 0)
#define PINK Color(255, 182, 193)
#define YELLOW Color(255, 255, 0)
} // namespace oh_my_loam

1
common/common.h
View File

@ -1,5 +1,6 @@
#pragma once
#include "color.h"
#include "config.h"
#include "log.h"
#include "macros.h"

31
common/log.h
View File

@ -7,17 +7,21 @@
#include <iostream>
const LEVELS ERROR{WARNING.value + 100, "ERROR"};
const LEVELS USER(ERROR.value + 100, "USER");
#define ADEBUG LOG(DEBUG) << "[DEBUG] "
#define ADEBUG LOG(DEBUG)
#define AINFO LOG(INFO)
#define AWARN LOG(WARNING)
#define AERROR LOG(ERROR)
#define AUSER LOG(USER)
#define AFATAL LOG(FATAL)
// LOG_IF
#define ADEBUG_IF(cond) LOG_IF(DEBUG, cond)
#define AINFO_IF(cond) LOG_IF(INFO, cond)
#define AWARN_IF(cond) LOG_IF(WARNING, cond)
#define AERROR_IF(cond) LOG_IF(ERROR, cond)
#define AUSER_IF(cond) LOG_IF(USER, cond)
#define AFATAL_IF(cond) LOG_IF(FATAL, cond)
#define ACHECK(cond) CHECK(cond)
@ -37,10 +41,11 @@ class CustomSink {
<< " shutdown at: ";
auto now = std::chrono::system_clock::now();
oss << g3::localtime_formatted(now, "%Y%m%d %H:%M:%S.%f3");
if (log_to_file_ && ofs_ != nullptr) {
if (log_to_file_) {
(*ofs_) << oss.str() << std::endl;
} else {
std::clog << oss.str() << std::endl;
}
std::clog << oss.str() << std::endl;
};
void StdLogMessage(g3::LogMessageMover logEntry) {
@ -48,7 +53,7 @@ class CustomSink {
}
void FileLogMessage(g3::LogMessageMover logEntry) {
if (log_to_file_ && ofs_ != nullptr) {
if (log_to_file_) {
(*ofs_) << FormatedMessage(logEntry.get()) << std::endl;
}
}
@ -67,25 +72,27 @@ class CustomSink {
std::string ColorFormatedMessage(const g3::LogMessage& msg) const {
std::ostringstream oss;
oss << "\033[" << GetColor(msg._level) << "m" << FormatedMessage(msg)
<< "\033[m";
oss << GetColorCode(msg._level) << FormatedMessage(msg) << "\033[m";
return oss.str();
}
int GetColor(const LEVELS& level) const {
std::string GetColorCode(const LEVELS& level) const {
if (level.value == WARNING.value) {
return 33; // yellow
return "\033[33m"; // yellow
}
if (level.value == DEBUG.value) {
return 32; // green
return "\033[32m"; // green
}
if (level.value == ERROR.value) {
return 31; // red
return "\033[31m"; // red
}
if (level.value == USER.value) {
return "\033[1m\033[34m"; // bold blue
}
if (g3::internal::wasFatal(level)) {
return 31; // red
return "\033[1m\033[31m"; // red
}
return 97; // white
return "\033[97m"; // white
}
};

2
common/tic_toc.h
View File

@ -15,7 +15,7 @@ class TicToc {
double toc() {
end_ = std::chrono::system_clock::now();
std::chrono::duration<double> elapsed_seconds = end_ - start_;
return elapsed_seconds.count() * 1000;
return elapsed_seconds.count() * 1000; // ms
}
private:

30
common/types.h
View File

@ -1,5 +1,7 @@
#pragma once
#include <pcl/filters/voxel_grid.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/visualization/pcl_visualizer.h>
@ -26,20 +28,6 @@ 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;
@ -59,7 +47,7 @@ struct PointXYZTCT {
struct {
float time;
float curvature;
int type; // -2, -1, 0, 1, or 2
PointType type;
};
};
@ -67,12 +55,12 @@ struct PointXYZTCT {
x = y = z = 0.0f;
time = 0.0f;
curvature = std::nanf("");
type = 0;
data[3] = 1.0f;
type = PointType::NORNAL;
}
PointXYZTCT(float x, float y, float z, float time = 0.0f,
float curvature = std::nanf(""), int type = 0)
float curvature = std::nanf(""),
PointType type = PointType::NORNAL)
: x(x), y(y), z(z), time(time), curvature(curvature), type(type) {}
PointXYZTCT(const Point& p) {
@ -81,7 +69,7 @@ struct PointXYZTCT {
z = p.z;
time = 0.0f;
curvature = std::nanf("");
type = 0;
type = PointType::NORNAL;
}
PointXYZTCT(const PointXYZTCT& p) {
@ -93,8 +81,6 @@ struct PointXYZTCT {
type = p.type;
}
PointType Type() const { return static_cast<PointType>(type); }
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
} EIGEN_ALIGN16;
@ -108,6 +94,6 @@ POINT_CLOUD_REGISTER_POINT_STRUCT(
(float, z, z)
(float, time, time)
(float, curvature, curvature)
(int8_t, type, type)
// (int8_t, type, type)
)
// clang-format on

14
common/utils.cc
View File

@ -7,4 +7,16 @@ double NormalizeAngle(double ang) {
return ang - two_pi * std::floor((ang + M_PI) / two_pi);
}
} // oh_my_loam
const std::vector<int> Range(int begin, int end, int step) {
ACHECK(step != 0) << "Step must non-zero";
int num = (end - begin) / step;
if (num <= 0) return {};
end = begin + step * num;
std::vector<int> seq(num);
for (int i = begin; i != end; i += step) seq[i] = i;
return seq;
}
const std::vector<int> Range(int end) { return Range(0, end, 1); }
} // namespace oh_my_loam

16
common/utils.h
View File

@ -1,5 +1,7 @@
#pragma once
#include "color.h"
#include "log.h"
#include "types.h"
namespace oh_my_loam {
@ -22,6 +24,10 @@ inline double IsFinite(const PointT& pt) {
// normalize an angle to [-pi, pi)
double NormalizeAngle(double ang);
// like Python built-in range, [begin, end)
const std::vector<int> Range(int begin, int end, int step = 1);
const std::vector<int> Range(int end); // [0, end)
template <typename PointT>
void DrawPointCloud(const pcl::PointCloud<PointT>& cloud, const Color& color,
const std::string& id, PCLVisualizer* const viewer,
@ -80,4 +86,14 @@ void RemoveClosedPoints(const pcl::PointCloud<PointT>& cloud_in,
});
}
template <typename PointT>
void VoxelDownSample(const pcl::PointCloud<PointT>& cloud_in,
pcl::PointCloud<PointT>* const cloud_out,
double voxel_size) {
pcl::VoxelGrid<PointT> filter;
filter.setInputCloud(cloud_in.makeShared());
filter.setLeafSize(voxel_size, voxel_size, voxel_size);
filter.filter(*cloud_out);
}
} // namespace oh_my_loam

20
configs/config.yaml
View File

@ -1,10 +1,18 @@
# global configs
lidar: VPL16
log_to_file: true
log_path: /home/liufei/catkin_ws/src/oh_my_loam/.log
vis: false
log_to_file: false
log_path: /home/lf/catkin_ws/src/oh_my_loam/.log
vis: true
# configs for feature points extractor
feature_extractor_config:
min_scan_point_num: 66
vis: true
extractor_config:
min_point_num: 66
vis: true
sharp_corner_point_num: 2
corner_point_num: 20
flat_surf_point_num: 4
surf_point_num: 1000
corner_point_curvature_thres: 0.5
surf_point_curvature_thres: 0.5
neighbor_point_dist_thres: 0.05
downsample_voxel_size: 0.2

5
main.cc
View File

@ -21,7 +21,7 @@ int main(int argc, char* argv[]) {
// logging
g3::InitG3Logging(log_to_file, "oh_my_loam_" + lidar, log_path);
AINFO << "Lidar: " << lidar;
AUSER << "LOAM start..., lidar = " << lidar;
// SLAM system
OhMyLoam slam;
@ -44,7 +44,6 @@ void PointCloudHandler(const sensor_msgs::PointCloud2ConstPtr& msg,
OhMyLoam* const slam) {
PointCloud cloud;
pcl::fromROSMsg(*msg, cloud);
ADEBUG << "Point num = " << cloud.size()
<< ", ts = " << LOG_TIMESTAMP(msg->header.stamp.toSec());
AINFO << "Timestamp = " << LOG_TIMESTAMP(msg->header.stamp.toSec());
slam->Run(cloud, 0.0);
}

2
src/CMakeLists.txt
View File

@ -1,4 +1,4 @@
add_subdirectory(feature_points_extractor)
add_subdirectory(extractor)
add_subdirectory(visualizer)
aux_source_directory(. SRC_FILES)

3
src/extractor/CMakeLists.txt Normal file
View File

@ -0,0 +1,3 @@
aux_source_directory(. SRC_FILES)
add_library(extractor SHARED ${SRC_FILES})

228
src/extractor/base_extractor.cc Normal file
View File

@ -0,0 +1,228 @@
#include "base_extractor.h"
#include <cmath>
namespace oh_my_loam {
namespace {
const int kScanSegNum = 6;
const double kTwoPi = 2 * M_PI;
} // namespace
bool Extractor::Init(const YAML::Node& config) {
config_ = config;
is_vis_ = Config::Instance()->Get<bool>("vis") && config_["vis"].as<bool>();
AINFO << "Extraction visualizer: " << (is_vis_ ? "ON" : "OFF");
if (is_vis_) visualizer_.reset(new ExtractorVisualizer);
return true;
}
void Extractor::Extract(const PointCloud& cloud, FeaturePoints* const feature) {
if (cloud.size() < config_["min_point_num"].as<size_t>()) {
AWARN << "Too few points ( < " << config_["min_point_num"].as<int>()
<< " ) after remove: " << cloud.size();
return;
}
TicToc timer;
// split point cloud int scans
std::vector<IPointCloud> scans;
SplitScan(cloud, &scans);
double time_split = timer.toc();
// compute curvature for each point in each scan
for (auto& scan : scans) {
ComputePointCurvature(&scan);
}
double time_curv = timer.toc();
// assign type for each point in each scan, five types: FLAT, LESS_FLAT,
// NORMAL, LESS_SHARP, SHARP
for (auto& scan : scans) {
AssignPointType(&scan);
}
double time_assign = timer.toc();
// store points into feature point clouds according to their type
std::ostringstream oss;
oss << "Feature point num: ";
for (const auto& scan : scans) {
FeaturePoints scan_feature;
StoreToFeaturePoints(scan, &scan_feature);
feature->Add(scan_feature);
oss << scan.size() << ":" << scan_feature.sharp_corner_pts->size() << ":"
<< scan_feature.less_sharp_corner_pts->size() << ":"
<< scan_feature.flat_surf_pts->size() << ":"
<< scan_feature.less_flat_surf_pts->size() << " ";
}
ADEBUG << oss.str();
double time_store = timer.toc();
AINFO << "Time elapsed (ms): scan_split = " << std::setprecision(3)
<< time_split << ", curvature_compute = " << time_curv - time_split
<< ", type_assign = " << time_assign - time_curv
<< ", store = " << time_store - time_assign
<< ", TOTAL = " << time_store;
if (is_vis_) Visualize(cloud, *feature);
}
void Extractor::SplitScan(const PointCloud& cloud,
std::vector<IPointCloud>* 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 Extractor::ComputePointCurvature(IPointCloud* const scan,
bool remove_nan) const {
if (scan->size() < 20) return;
auto& pts = scan->points;
for (size_t i = 5; i < pts.size() - 5; ++i) {
float dx = 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 dy = 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 dz = 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 = std::sqrt(dx * dx + dy * dy + dz * dz);
}
RemovePointsIf<IPoint>(*scan, scan, [](const IPoint& pt) {
return !std::isfinite(pt.curvature);
});
}
void Extractor::AssignPointType(IPointCloud* const scan) const {
int pt_num = scan->size();
ACHECK(pt_num >= kScanSegNum);
int seg_pt_num = (pt_num - 1) / kScanSegNum + 1;
std::vector<bool> picked(pt_num, false);
std::vector<int> indices = Range(pt_num);
int sharp_corner_point_num = config_["sharp_corner_point_num"].as<int>();
int corner_point_num = config_["corner_point_num"].as<int>();
int flat_surf_point_num = config_["flat_surf_point_num"].as<int>();
int surf_point_num = config_["surf_point_num"].as<int>();
float corner_point_curvature_thres =
config_["corner_point_curvature_thres"].as<float>();
float surf_point_curvature_thres =
config_["surf_point_curvature_thres"].as<float>();
for (int seg = 0; seg < kScanSegNum; ++seg) {
int s = seg * seg_pt_num;
int e = std::min((seg + 1) * seg_pt_num, pt_num);
// sort by curvature for each segment: large -> small
std::sort(indices.begin() + s, indices.begin() + e, [&](int i, int j) {
return scan->at(i).curvature > scan->at(j).curvature;
});
// pick corner points
int corner_pt_picked_num = 0;
for (int i = s; i < e; ++i) {
size_t ix = indices[i];
if (!picked.at(ix) &&
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;
} else if (corner_pt_picked_num <= corner_point_num) {
scan->at(ix).type = PointType::LESS_SHARP;
} else {
break;
}
picked.at(ix) = true;
SetNeighborsPicked(*scan, ix, &picked);
}
}
// pick surface points
int surf_pt_picked_num = 0;
for (int i = e - 1; i >= s; --i) {
size_t ix = indices[i];
if (!picked.at(ix) &&
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;
} else if (surf_pt_picked_num <= surf_point_num) {
scan->at(ix).type = PointType::LESS_FLAT;
} else {
break;
}
picked.at(ix) = true;
SetNeighborsPicked(*scan, ix, &picked);
}
}
}
}
void Extractor::SetNeighborsPicked(const IPointCloud& scan, size_t ix,
std::vector<bool>* const picked) const {
auto DistSqure = [&](int i, int j) -> float {
float dx = scan.at(i).x - scan.at(j).x;
float dy = scan.at(i).y - scan.at(j).y;
float dz = scan.at(i).z - scan.at(j).z;
return dx * dx + dy * dy + dz * dz;
};
float neighbor_point_dist_thres =
config_["neighbor_point_dist_thres"].as<float>();
for (size_t i = 1; i <= 5; ++i) {
if (ix < i) break;
if (picked->at(ix - i)) continue;
if (DistSqure(ix - i, ix - i + 1) > neighbor_point_dist_thres) break;
picked->at(ix - i) = true;
}
for (size_t i = 1; i <= 5; ++i) {
if (ix + i >= scan.size()) break;
if (picked->at(ix + i)) continue;
if (DistSqure(ix + i, ix + i - 1) > neighbor_point_dist_thres) break;
picked->at(ix + i) = true;
}
}
void Extractor::StoreToFeaturePoints(const IPointCloud& scan,
FeaturePoints* const feature) const {
for (const auto& pt : scan.points) {
switch (pt.type) {
case PointType::FLAT:
feature->flat_surf_pts->points.emplace_back(pt.x, pt.y, pt.z);
// no break: FLAT points are also LESS_FLAT
case PointType::LESS_FLAT:
feature->less_flat_surf_pts->points.emplace_back(pt.x, pt.y, pt.z);
break;
case PointType::SHARP:
feature->sharp_corner_pts->points.emplace_back(pt.x, pt.y, pt.z);
// no break: SHARP points are also LESS_SHARP
case PointType::LESS_SHARP:
feature->less_sharp_corner_pts->points.emplace_back(pt.x, pt.y, pt.z);
break;
default:
// all the rest are also LESS_FLAT
feature->less_flat_surf_pts->points.emplace_back(pt.x, pt.y, pt.z);
break;
}
}
PointCloudPtr filtered_less_flat_surf_pts(new PointCloud);
VoxelDownSample(*feature->less_flat_surf_pts,
filtered_less_flat_surf_pts.get(),
config_["downsample_voxel_size"].as<double>());
feature->less_flat_surf_pts = filtered_less_flat_surf_pts;
}
void Extractor::Visualize(const PointCloud& cloud,
const FeaturePoints& feature_pts, double timestamp) {
std::shared_ptr<ExtractorVisFrame> frame(new ExtractorVisFrame);
frame->timestamp = timestamp;
frame->cloud = cloud.makeShared();
frame->feature_pts = feature_pts;
visualizer_->Render(frame);
}
} // namespace oh_my_loam

50
src/extractor/base_extractor.h Normal file
View File

@ -0,0 +1,50 @@
#pragma once
#include "feature_points.h"
#include "visualizer/extractor_visualizer.h"
namespace oh_my_loam {
class Extractor {
public:
Extractor() = default;
virtual ~Extractor() = default;
bool Init(const YAML::Node& config);
void Extract(const PointCloud& cloud, FeaturePoints* const feature);
int num_scans() const { return num_scans_; }
protected:
virtual int GetScanID(const Point& pt) const = 0;
int num_scans_ = 0;
YAML::Node config_;
private:
void Visualize(const PointCloud& cloud, const FeaturePoints& feature_pts,
double timestamp = std::nan(""));
void SplitScan(const PointCloud& cloud,
std::vector<IPointCloud>* const scans) const;
void ComputePointCurvature(IPointCloud* const scan,
bool remove_nan = true) const;
void AssignPointType(IPointCloud* const scan) const;
void SetNeighborsPicked(const IPointCloud& scan, size_t ix,
std::vector<bool>* const picked) const;
void StoreToFeaturePoints(const IPointCloud& scan,
FeaturePoints* const feature) const;
bool is_vis_ = false;
std::unique_ptr<ExtractorVisualizer> visualizer_{nullptr};
DISALLOW_COPY_AND_ASSIGN(Extractor);
};
} // namespace oh_my_loam

4
src/feature_points_extractor/feature_points_extractor_VLP16.cc → src/extractor/extractor_VLP16.cc
View File

@ -1,8 +1,8 @@
#include "feature_points_extractor_VLP16.h"
#include "extractor_VLP16.h"
namespace oh_my_loam {
int FeaturePointsExtractorVLP16::GetScanID(const Point& pt) const {
int ExtractorVLP16::GetScanID(const Point& pt) const {
static int i = 0;
double omega = std::atan2(pt.z, Distance(pt)) * 180 * M_1_PI + 15.0;
if (i++ < 10) {

16
src/extractor/extractor_VLP16.h Normal file
View File

@ -0,0 +1,16 @@
#pragma once
#include "base_extractor.h"
namespace oh_my_loam {
// for VLP-16
class ExtractorVLP16 : public Extractor {
public:
ExtractorVLP16() { num_scans_ = 16; }
private:
int GetScanID(const Point& pt) const override;
};
} // namespace oh_my_loam

28
src/extractor/feature_points.h Normal file
View File

@ -0,0 +1,28 @@
#pragma once
#include "common.h"
namespace oh_my_loam {
struct FeaturePoints {
PointCloudPtr sharp_corner_pts;
PointCloudPtr less_sharp_corner_pts;
PointCloudPtr flat_surf_pts;
PointCloudPtr less_flat_surf_pts;
FeaturePoints() {
sharp_corner_pts.reset(new PointCloud);
less_sharp_corner_pts.reset(new PointCloud);
flat_surf_pts.reset(new PointCloud);
less_flat_surf_pts.reset(new PointCloud);
}
void Add(const FeaturePoints& rhs) {
*sharp_corner_pts += *rhs.sharp_corner_pts;
*less_sharp_corner_pts += *rhs.less_sharp_corner_pts;
*flat_surf_pts += *rhs.flat_surf_pts;
*less_flat_surf_pts += *rhs.less_flat_surf_pts;
}
};
} // namespace oh_my_loam

3
src/feature_points_extractor/CMakeLists.txt
View File

@ -1,3 +0,0 @@
aux_source_directory(. SRC_FILES)
add_library(feature_points_extractor SHARED ${SRC_FILES})

127
src/feature_points_extractor/base_feature_points_extractor.cc
View File

@ -1,127 +0,0 @@
#include "base_feature_points_extractor.h"
#include <cmath>
namespace oh_my_loam {
namespace {
const double kPointMinDist = 0.1;
const int kScanSegNum = 6;
const double kTwoPi = 2 * M_PI;
const int kMinPtsNum = 100;
} // namespace
bool FeaturePointsExtractor::Init(const YAML::Node& config) {
config_ = config;
is_vis_ = Config::Instance()->Get<bool>("vis") && config_["vis"].as<bool>();
AINFO << "Feature points extraction visualizer: " << (is_vis_ ? "ON" : "OFF");
if (is_vis_) visualizer_.reset(new FeaturePointsVisualizer);
return true;
}
void FeaturePointsExtractor::Extract(const PointCloud& cloud_in,
FeaturePoints* const feature) {
PointCloudPtr cloud(new PointCloud);
RemoveNaNPoint<Point>(cloud_in, cloud.get());
RemoveClosedPoints<Point>(*cloud, cloud.get(), kPointMinDist);
ADEBUG << "AFTER REMOVE, point num = " << cloud->size();
if (cloud->size() < kMinPtsNum) {
return;
}
std::vector<IPointCloud> scans;
SplitScan(*cloud, &scans);
std::ostringstream oss;
for (auto& scan : scans) {
oss << scan.size() << ":";
ComputePointCurvature(&scan);
RemovePointsIf<IPoint>(scan, &scan, [](const IPoint& pt) {
return !std::isfinite(pt.curvature);
});
oss << scan.size() << " ";
AssignPointType(&scan);
}
ADEBUG << oss.str();
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;
}
}
}
if (is_vis_) Visualize(cloud_in, *feature);
}
void FeaturePointsExtractor::SplitScan(
const PointCloud& cloud, std::vector<IPointCloud>* 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 {
if (scan->size() < 20) return;
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 = std::sqrt(diffX * diffX + diffY * diffY + diffZ * diffZ);
}
}
void FeaturePointsExtractor::AssignPointType(IPointCloud* const scan) const {
// int pt_num = scan->size();
// int pt_num_seg = (pt_num - 1) / kScanSegNum + 1;
// std::vector<bool> picked(pt_num, false);
// for (int i = 0; i < kScanSegNum; ++i) {
// int begin = i * pt_num_seg;
// int end = std::max((i + 1) * pt_num_seg, pt_num);
// }
}
void FeaturePointsExtractor::Visualize(const PointCloud& cloud,
const FeaturePoints& feature_pts,
double timestamp) {
std::shared_ptr<FeaturePointsVisFrame> frame(new FeaturePointsVisFrame);
frame->timestamp = timestamp;
frame->cloud = cloud.makeShared();
frame->feature_pts = feature_pts;
visualizer_->Render(frame);
}
} // namespace oh_my_loam

42
src/feature_points_extractor/base_feature_points_extractor.h
View File

@ -1,42 +0,0 @@
#pragma once
#include "feature_points.h"
#include "visualizer/feature_points_visualizer.h"
namespace oh_my_loam {
class FeaturePointsExtractor {
public:
FeaturePointsExtractor() = default;
virtual ~FeaturePointsExtractor() = default;
bool Init(const YAML::Node& config);
void Extract(const PointCloud& cloud_in, FeaturePoints* const feature);
int num_scans() const { return num_scans_; }
protected:
virtual int GetScanID(const Point& pt) const = 0;
void Visualize(const PointCloud& cloud, const FeaturePoints& feature_pts,
double timestamp = std::nan(""));
void SplitScan(const PointCloud& cloud,
std::vector<IPointCloud>* const scans) const;
void ComputePointCurvature(IPointCloud* const scan) const;
void AssignPointType(IPointCloud* const scan) const;
int num_scans_ = 0;
bool is_vis_ = false;
std::unique_ptr<FeaturePointsVisualizer> visualizer_{nullptr};
YAML::Node config_;
DISALLOW_COPY_AND_ASSIGN(FeaturePointsExtractor);
};
} // namespace oh_my_loam

23
src/feature_points_extractor/feature_points.h
View File

@ -1,23 +0,0 @@
#pragma once
#include "common.h"
namespace oh_my_loam {
struct FeaturePoints {
IPointCloudPtr feature_pts; // all feature points
IPointCloudPtr sharp_corner_pts;
IPointCloudPtr less_sharp_corner_pts;
IPointCloudPtr flat_surf_pts;
IPointCloudPtr less_flat_surf_pts;
FeaturePoints() {
feature_pts.reset(new IPointCloud);
sharp_corner_pts.reset(new IPointCloud);
less_sharp_corner_pts.reset(new IPointCloud);
flat_surf_pts.reset(new IPointCloud);
less_flat_surf_pts.reset(new IPointCloud);
}
};
} // namespace oh_my_loam

16
src/feature_points_extractor/feature_points_extractor_VLP16.h
View File

@ -1,16 +0,0 @@
#pragma once
#include "base_feature_points_extractor.h"
namespace oh_my_loam {
// for VLP-16
class FeaturePointsExtractorVLP16 : public FeaturePointsExtractor {
public:
FeaturePointsExtractorVLP16() { num_scans_ = 16; }
private:
int GetScanID(const Point& pt) const override;
};
} // namespace oh_my_loam

29
src/oh_my_loam.cc
View File

@ -1,21 +1,36 @@
#include "oh_my_loam.h"
#include "feature_points_extractor/feature_points_extractor_VLP16.h"
#include "extractor/extractor_VLP16.h"
namespace oh_my_loam {
namespace {
const double kPointMinDist = 0.1;
}
bool OhMyLoam::Init() {
YAML::Node config = Config::Instance()->config();
feature_extractor_.reset(new FeaturePointsExtractorVLP16);
if (!feature_extractor_->Init(config["feature_extractor_config"])) {
AERROR << "Failed to initialize feature points extractor";
extractor_.reset(new ExtractorVLP16);
if (!extractor_->Init(config["extractor_config"])) {
AERROR << "Failed to initialize extractor";
return false;
}
return true;
}
void OhMyLoam::Run(const PointCloud& cloud, double timestamp) {
FeaturePoints feature_pts;
feature_extractor_->Extract(cloud, &feature_pts);
void OhMyLoam::Run(const PointCloud& cloud_in, double timestamp) {
PointCloudPtr cloud(new PointCloud);
RemoveOutliers(cloud_in, cloud.get());
ADEBUG << "After remove, point num: " << cloud_in.size() << " -> "
<< cloud->size();
FeaturePoints feature_points;
extractor_->Extract(*cloud, &feature_points);
}
void OhMyLoam::RemoveOutliers(const PointCloud& cloud_in,
PointCloud* const cloud_out) const {
RemoveNaNPoint<Point>(cloud_in, cloud_out);
RemoveClosedPoints<Point>(*cloud_out, cloud_out, kPointMinDist);
}
} // namespace oh_my_loam

10
src/oh_my_loam.h
View File

@ -3,7 +3,7 @@
#include <yaml-cpp/yaml.h>
#include "common.h"
#include "feature_points_extractor/base_feature_points_extractor.h"
#include "extractor/base_extractor.h"
namespace oh_my_loam {
@ -13,10 +13,14 @@ class OhMyLoam {
bool Init();
void Run(const PointCloud& cloud, double timestamp);
void Run(const PointCloud& cloud_in, double timestamp);
private:
std::unique_ptr<FeaturePointsExtractor> feature_extractor_{nullptr};
std::unique_ptr<Extractor> extractor_{nullptr};
// remove outliers: nan and very close points
void RemoveOutliers(const PointCloud& cloud_in,
PointCloud* const cloud_out) const;
DISALLOW_COPY_AND_ASSIGN(OhMyLoam)
};

38
src/visualizer/extractor_visualizer.cc Normal file
View File

@ -0,0 +1,38 @@
#include "extractor_visualizer.h"
namespace oh_my_loam {
void ExtractorVisualizer::Draw() {
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 less_flat_surf_pts
std::string id = "less_flat_surf_pts";
DrawPointCloud(*frame.feature_pts.less_flat_surf_pts, CYAN, id,
viewer_.get(), 5);
rendered_cloud_ids_.push_back(id);
}
{ // add flat_surf_pts
std::string id = "flat_surf_pts";
DrawPointCloud(*frame.feature_pts.flat_surf_pts, BLUE, id, viewer_.get(),
7);
rendered_cloud_ids_.push_back(id);
}
{ // add less_sharp_corner_pts
std::string id = "less_sharp_corner_pts";
DrawPointCloud(*frame.feature_pts.less_sharp_corner_pts, PURPLE, id,
viewer_.get(), 5);
rendered_cloud_ids_.push_back(id);
}
{ // add sharp_corner_pts
std::string id = "sharp_corner_pts";
DrawPointCloud(*frame.feature_pts.sharp_corner_pts, RED, id, viewer_.get(),
7);
rendered_cloud_ids_.push_back(id);
}
};
} // namespace oh_my_loam

22
src/visualizer/extractor_visualizer.h Normal file
View File

@ -0,0 +1,22 @@
#pragma once
#include "base_visualizer.h"
#include "extractor/feature_points.h"
namespace oh_my_loam {
struct ExtractorVisFrame : public VisFrame {
FeaturePoints feature_pts;
};
class ExtractorVisualizer : public Visualizer<ExtractorVisFrame> {
public:
explicit ExtractorVisualizer(const std::string &name = "ExtractorVisualizer",
size_t max_history_size = 10)
: Visualizer<ExtractorVisFrame>(name, max_history_size) {}
private:
void Draw() override;
};
} // namespace oh_my_loam

43
src/visualizer/feature_points_visualizer.cc
View File

@ -1,43 +0,0 @@
#include "feature_points_visualizer.h"
namespace oh_my_loam {
void FeaturePointsVisualizer::Draw() {
auto frame = GetCurrentFrame();
{ // add raw point cloud
std::string id = "raw point cloud";
DrawPointCloud<Point>(*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, "curvature", id,
viewer_.get());
rendered_cloud_ids_.push_back(id);
}
{ // add flat_surf_pts
std::string id = "flat_surf_pts";
DrawPointCloud(*frame.feature_pts.flat_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_pts.less_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_pts.less_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);
}
};
} // namespace oh_my_loam

23
src/visualizer/feature_points_visualizer.h
View File

@ -1,23 +0,0 @@
#pragma once
#include "base_visualizer.h"
#include "feature_points_extractor/feature_points.h"
namespace oh_my_loam {
struct FeaturePointsVisFrame : public VisFrame {
FeaturePoints feature_pts;
};
class FeaturePointsVisualizer : public Visualizer<FeaturePointsVisFrame> {
public:
explicit FeaturePointsVisualizer(
const std::string &name = "FeaturePointsVisualizer",
size_t max_history_size = 10)
: Visualizer<FeaturePointsVisFrame>(name, max_history_size) {}
private:
void Draw() override;
};
} // namespace oh_my_loam