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

re-organize code structure

main
feixyz10 2020-10-16 18:08:31 +08:00 committed by feixyz
parent 2263ab2ae7
commit 5830e46fed
30 changed files with 329 additions and 247 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

9
CMakeLists.txt
View File

@ -44,13 +44,16 @@ catkin_package(
add_subdirectory(common)
add_subdirectory(src)
add_executable(oh_my_loam main.cc)
target_link_libraries(oh_my_loam
add_executable(main main.cc)
target_link_libraries(main
${catkin_LIBRARIES}
${PCL_LIBRARIES}
${CERES_LIBRARIES}
${G3LOG_LIBRARIES}
${YAML_CPP_LIBRARIES}
g3log
OhMyLoam
common
oh_my_loam
feature_points_extractor
visualizer
)

3
common/CMakeLists.txt
View File

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

3
common/common.h
View File

@ -1,7 +1,8 @@
#pragma once
#include "filter.h"
#include "config.h"
#include "log.h"
#include "macros.h"
#include "tic_toc.h"
#include "types.h"
#include "utils.h"

10
common/config.h
View File

@ -5,9 +5,9 @@
#include <string>
#include "log.h"
#include "micros.h"
#include "macros.h"
namespace oh_my_slam {
namespace oh_my_loam {
class Config {
public:
@ -17,7 +17,7 @@ class Config {
}
template <typename T>
const T& Get(const std::string& key) const {
const T Get(const std::string& key) const {
AFATAL_IF(!config_) << "No config exists: please call SetConfigFile.";
return (*config_)[key].as<T>();
}
@ -30,7 +30,7 @@ class Config {
private:
std::shared_ptr<YAML::Node> config_{nullptr};
DECLARE_SINGLETON(Config);
DECLARE_SINGLETON(Config)
};
} // namespace oh_my_slam
} // namespace oh_my_loam

44
common/filter.h
View File

@ -1,44 +0,0 @@
#pragma once
#include "utils.h"
namespace oh_my_loam {
template <typename PointT>
void RemovePointsIf(const pcl::PointCloud<PointT>& cloud_in,
pcl::PointCloud<PointT>* const cloud_out,
std::function<bool(const PointT&)> cond) {
if (&cloud_in != cloud_out) {
cloud_out->header = cloud_in.header;
cloud_out->points.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];
if (cond(pt)) continue;
cloud_out->points[j++] = pt;
}
cloud_out->points.resize(j);
cloud_out->height = 1;
cloud_out->width = static_cast<uint32_t>(j);
cloud_out->is_dense = true;
}
template <typename PointT>
void RemoveNaNPoint(const pcl::PointCloud<PointT>& cloud_in,
pcl::PointCloud<PointT>* const cloud_out) {
RemovePointsIf<PointT>(cloud_in, cloud_out,
[](const PointT& pt) { return !IsFinite(pt); });
}
template <typename PointT>
void RemoveClosedPoints(const pcl::PointCloud<PointT>& cloud_in,
pcl::PointCloud<PointT>* const cloud_out,
double min_dist = 0.1) {
RemovePointsIf<PointT>(cloud_in, cloud_out, [&](const PointT& pt) {
return DistanceSqure(pt) < min_dist * min_dist;
});
}
} // namespace oh_my_loam

25
common/log.cc Normal file
View File

@ -0,0 +1,25 @@
#include "log.h"
namespace g3 {
void InitG3Logging(bool log_to_file, const std::string& prefix,
const std::string& path) {
static std::unique_ptr<g3::LogWorker> worker;
if (worker != nullptr) return;
worker = std::move(g3::LogWorker::createLogWorker());
worker->addSink(std::make_unique<g3::CustomSink>(),
&g3::CustomSink::StdLogMessage);
if (log_to_file) {
std::ostringstream oss;
oss << path;
if (*path.rbegin() != '/') oss << '/';
if (prefix.empty()) oss << "g3log";
oss << prefix << ".";
auto now = std::chrono::system_clock::now();
oss << g3::localtime_formatted(now, "%Y%m%d-%H%M%S");
oss << ".log";
worker->addSink(std::make_unique<g3::CustomSink>(oss.str()),
&g3::CustomSink::FileLogMessage);
}
g3::initializeLogging(worker.get());
}
}

33
common/log.h
View File

@ -1,17 +1,23 @@
#pragma once
#include <chrono>
#include <fstream>
#include <g3log/g3log.hpp>
#include <g3log/logworker.hpp>
#include <iostream>
const LEVELS ERROR{WARNING.value + 100, "ERROR"};
#define ADEBUG LOG(DEBUG) << "[DEBUG] "
#define AINFO LOG(INFO)
#define AWARN LOG(WARNING)
#define AERROR LOG(ERROR)
#define AFATAL LOG(FATAL)
// LOG_IF
#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 AFATAL_IF(cond) LOG_IF(FATAL, cond)
#define ACHECK(cond) CHECK(cond)
@ -19,10 +25,12 @@ namespace g3 {
class CustomSink {
public:
CustomSink() = default;
explicit CustomSink(const std::string& log_file_name)
: log_file_name_(log_file_name), log_to_file_(true) {
ofs_.reset(new std::ofstream(log_file_name));
}
~CustomSink() {
std::ostringstream oss;
oss << "\ng3log " << (log_to_file_ ? "FileSink" : "StdSink")
@ -71,6 +79,9 @@ class CustomSink {
if (level.value == DEBUG.value) {
return 32; // green
}
if (level.value == ERROR.value) {
return 31; // red
}
if (g3::internal::wasFatal(level)) {
return 31; // red
}
@ -78,25 +89,7 @@ class CustomSink {
}
};
template <bool LogToFile>
void InitG3Logging(const std::string& prefix, const std::string& path) {
static std::unique_ptr<g3::LogWorker> worker;
if (worker != nullptr) return;
worker = std::move(g3::LogWorker::createLogWorker());
worker->addSink(std::make_unique<g3::CustomSink>(),
&g3::CustomSink::StdLogMessage);
if (LogToFile) {
std::ostringstream oss;
oss << path;
if (*path.rbegin() != '/') oss << '/';
oss << prefix << ".";
auto now = std::chrono::system_clock::now();
oss << g3::localtime_formatted(now, "%Y%m%d-%H%M%S");
oss << ".log";
worker->addSink(std::make_unique<g3::CustomSink>(oss.str()),
&g3::CustomSink::FileLogMessage);
}
g3::initializeLogging(worker.get());
}
void InitG3Logging(bool log_to_file = false, const std::string& prefix = "",
const std::string& path = "./");
} // namespace g3

9
common/micros.h → common/macros.h
View File

@ -3,10 +3,14 @@
#include <memory>
#include <mutex>
#define DISALLOW_COPY_AND_ASSIGN(classname) \
classname(const classname &) = delete; \
classname &operator=(const classname &) = delete;
// adapted form baidu apollo cyber/common/macros.h, it is thread safe
#define DECLARE_SINGLETON(classname) \
public: \
static classname* Instance() { \
static classname *Instance() { \
static std::unique_ptr<classname> instance = nullptr; \
if (!instance) { \
static std::once_flag flag; \
@ -18,5 +22,4 @@
\
private: \
classname() = default; \
classname(const classname&) = delete; \
classname& operator=(const classname&) = delete;
DISALLOW_COPY_AND_ASSIGN(classname)

10
common/utils.cc Normal file
View File

@ -0,0 +1,10 @@
#include "utils.h"
namespace oh_my_loam {
double NormalizeAngle(double ang) {
const double& two_pi = 2 * M_PI;
return ang - two_pi * std::floor((ang + M_PI) / two_pi);
}
} // oh_my_loam

42
common/utils.h
View File

@ -20,10 +20,7 @@ inline double IsFinite(const PointT& pt) {
}
// normalize an angle to [-pi, pi)
inline double NormalizeAngle(double ang) {
const double& two_pi = 2 * M_PI;
return ang - two_pi * std::floor((ang + M_PI) / two_pi);
}
double NormalizeAngle(double ang);
template <typename PointT>
void DrawPointCloud(const pcl::PointCloud<PointT>& cloud, const Color& color,
@ -46,4 +43,41 @@ void DrawPointCloud(const pcl::PointCloud<PointT>& cloud,
pcl::visualization::PCL_VISUALIZER_POINT_SIZE, pt_size, id);
}
template <typename PointT>
void RemovePointsIf(const pcl::PointCloud<PointT>& cloud_in,
pcl::PointCloud<PointT>* const cloud_out,
std::function<bool(const PointT&)> cond) {
if (&cloud_in != cloud_out) {
cloud_out->header = cloud_in.header;
cloud_out->points.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];
if (cond(pt)) continue;
cloud_out->points[j++] = pt;
}
cloud_out->points.resize(j);
cloud_out->height = 1;
cloud_out->width = static_cast<uint32_t>(j);
cloud_out->is_dense = true;
}
template <typename PointT>
void RemoveNaNPoint(const pcl::PointCloud<PointT>& cloud_in,
pcl::PointCloud<PointT>* const cloud_out) {
RemovePointsIf<PointT>(cloud_in, cloud_out,
[](const PointT& pt) { return !IsFinite(pt); });
}
template <typename PointT>
void RemoveClosedPoints(const pcl::PointCloud<PointT>& cloud_in,
pcl::PointCloud<PointT>* const cloud_out,
double min_dist = 0.1) {
RemovePointsIf<PointT>(cloud_in, cloud_out, [&](const PointT& pt) {
return DistanceSqure(pt) < min_dist * min_dist;
});
}
} // namespace oh_my_loam

4
config/config.yaml
View File

@ -1,4 +0,0 @@
lidar: VPL16
feature_points_extractor_config:
min_scan_point_num: 66

10
configs/config.yaml Normal file
View File

@ -0,0 +1,10 @@
# global configs
lidar: VPL16
log_to_file: true
log_path: /home/liufei/catkin_ws/src/oh_my_loam/.log
vis: false
# configs for feature points extractor
feature_extractor_config:
min_scan_point_num: 66
vis: true

35
main.cc
View File

@ -1,27 +1,38 @@
#include <pcl_conversions/pcl_conversions.h>
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <yaml-cpp/yaml.h>
#include <functional>
#include "log.h"
#include "oh_my_loam.h"
#include "common/common.h"
#include "src/oh_my_loam.h"
using namespace oh_my_loam;
void PointCloudHandler(const sensor_msgs::PointCloud2ConstPtr& msg,
oh_my_loam::OhMyLoam* const slam);
OhMyLoam* const slam);
int main(int argc, char* argv[]) {
// configurations
YAML::Node config = YAML::LoadFile("./config/config.yaml");
// config
Config::Instance()->SetConfigFile("configs/config.yaml");
bool log_to_file = Config::Instance()->Get<bool>("log_to_file");
std::string log_path = Config::Instance()->Get<std::string>("log_path");
std::string lidar = Config::Instance()->Get<std::string>("lidar");
// logging
g3::InitG3Logging<true>("oh_my_loam", ".log");
AWARN << config["lidar"].as<std::string>();
g3::InitG3Logging(log_to_file, "oh_my_loam_" + lidar, log_path);
AINFO << "Lidar: " << lidar;
// SLAM system
oh_my_loam::OhMyLoam slam;
slam.Init(config["feature_points_extractor_config"]);
OhMyLoam slam;
if (!slam.Init()) {
AFATAL << "Failed to initilize slam system.";
}
ADEBUG << "DEBUG";
AINFO << "INFO";
AWARN << "WARN";
AERROR << "ERROR";
AFATAL << "FATAL";
// ros
ros::init(argc, argv, "oh_my_loam");
@ -35,8 +46,8 @@ int main(int argc, char* argv[]) {
}
void PointCloudHandler(const sensor_msgs::PointCloud2ConstPtr& msg,
oh_my_loam::OhMyLoam* const slam) {
oh_my_loam::PointCloud cloud;
OhMyLoam* const slam) {
PointCloud cloud;
pcl::fromROSMsg(*msg, cloud);
AINFO << "Point num = " << cloud.size()
<< ", ts = " << msg->header.stamp.toSec();

BIN
oh_my_loam Executable file
View File

Binary file not shown.

8
src/CMakeLists.txt
View File

@ -1,7 +1,7 @@
include_directories(
${PROJECT_SOURCE_DIR}/common
)
add_subdirectory(feature_points_extractor)
add_subdirectory(visualizer)
aux_source_directory(. SRC_FILES)
add_library(OhMyLoam SHARED ${SRC_FILES})
add_library(oh_my_loam SHARED ${SRC_FILES})

26
src/feature_extractor_VLP16.h
View File

@ -1,26 +0,0 @@
#pragma once
#include "feature_extractor_base.h"
namespace oh_my_loam {
// for VLP-16
class FeatureExtractorVLP16 : public FeaturePointsExtractor {
public:
FeatureExtractorVLP16() { num_scans_ = 16; }
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) {
AWARN << "OMEGA: " << std::atan2(pt.z, Distance(pt)) * 180 * M_1_PI + 15.0
<< " id = " << static_cast<int>(std::round(omega / 2.0) + 0.01)
<< " z = " << pt.z << " "
<< " d = " << Distance(pt) << std::endl;
}
return static_cast<int>(std::round(omega / 2.0) + 1.e-5);
}
};
} // namespace oh_my_loam

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

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

44
src/feature_extractor_base.cc → src/feature_points_extractor/base_feature_points_extractor.cc
View File

@ -1,9 +1,7 @@
#include "feature_extractor_base.h"
#include "base_feature_points_extractor.h"
#include <cmath>
#include "filter.h"
namespace oh_my_loam {
namespace {
@ -15,11 +13,14 @@ const int kMinPtsNum = 100;
bool FeaturePointsExtractor::Init(const YAML::Node& config) {
config_ = config;
is_vis_ = Config::Instance()->Get<bool>("vis") && config_["vis"].as<bool>();
AINFO << "Feature points visualization: " << (is_vis_ ? "ON" : "OFF");
if (is_vis_) visualizer_.reset(new FeaturePointsVisualizer);
return true;
}
void FeaturePointsExtractor::Extract(const PointCloud& cloud_in,
FeaturePoints* const feature) const {
FeaturePoints* const feature) {
PointCloudPtr cloud(new PointCloud);
AINFO << "BEFORE REMOVE, num = " << cloud_in.size();
RemoveNaNPoint<Point>(cloud_in, cloud.get());
@ -42,26 +43,27 @@ void FeaturePointsExtractor::Extract(const PointCloud& cloud_in,
}
AWARN << oss.str();
for (const auto& scan : scans) {
feature->feature_pts += scan;
*feature->feature_pts += scan;
for (const auto& pt : scan.points) {
switch (pt.Type()) {
case PointType::FLAT:
feature->flat_surf_pts.points.emplace_back(pt);
feature->flat_surf_pts->points.emplace_back(pt);
break;
case PointType::LESS_FLAT:
feature->less_flat_surf_pts.points.emplace_back(pt);
feature->less_flat_surf_pts->points.emplace_back(pt);
break;
case PointType::LESS_SHARP:
feature->less_sharp_corner_pts.points.emplace_back(pt);
feature->less_sharp_corner_pts->points.emplace_back(pt);
break;
case PointType::SHARP:
feature->sharp_corner_pts.points.emplace_back(pt);
feature->sharp_corner_pts->points.emplace_back(pt);
break;
default:
break;
}
}
}
if (is_vis_) Visualize(cloud_in, *feature);
}
void FeaturePointsExtractor::SplitScan(
@ -104,13 +106,23 @@ void FeaturePointsExtractor::ComputePointCurvature(
}
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);
}
// 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

25
src/feature_extractor_base.h → src/feature_points_extractor/base_feature_points_extractor.h
View File

@ -1,32 +1,26 @@
#pragma once
#include "common.h"
#include "feature_points.h"
#include "visualizer/feature_points_visualizer.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;
bool Init(const YAML::Node& config);
void Extract(const PointCloud& cloud_in, FeaturePoints* const feature) const;
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;
@ -35,7 +29,14 @@ class FeaturePointsExtractor {
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 Normal file
View File

@ -0,0 +1,23 @@
#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

17
src/feature_points_extractor/feature_points_extractor_VLP16.cc Normal file
View File

@ -0,0 +1,17 @@
#include "feature_points_extractor_VLP16.h"
namespace oh_my_loam {
int FeaturePointsExtractorVLP16::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) {
AWARN << "OMEGA: " << std::atan2(pt.z, Distance(pt)) * 180 * M_1_PI + 15.0
<< " id = " << static_cast<int>(std::round(omega / 2.0) + 0.01)
<< " z = " << pt.z << " "
<< " d = " << Distance(pt) << std::endl;
}
return static_cast<int>(std::round(omega / 2.0) + 1.e-5);
};
} // namespace oh_my_loam

16
src/feature_points_extractor/feature_points_extractor_VLP16.h Normal file
View File

@ -0,0 +1,16 @@
#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

28
src/oh_my_loam.cc
View File

@ -1,31 +1,21 @@
#include "oh_my_loam.h"
#include "feature_points_extractor/feature_points_extractor_VLP16.h"
namespace oh_my_loam {
bool OhMyLoam::Init(const YAML::Node& config) {
is_vis_ = true;
config_ = config;
feature_extractor_.reset(new FeatureExtractorVLP16);
if (is_vis_) {
visualizer_.reset(new FeaturePointsVisualizer);
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";
return false;
}
return true;
}
void OhMyLoam::Run(const PointCloud& cloud, double timestamp) {
std::shared_ptr<FeaturePoints> 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<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);
FeaturePoints feature_pts;
feature_extractor_->Extract(cloud, &feature_pts);
}
} // namespace oh_my_loam

17
src/oh_my_loam.h
View File

@ -3,31 +3,22 @@
#include <yaml-cpp/yaml.h>
#include "common.h"
#include "feature_extractor_VLP16.h"
#include "visualizer_feature_points.h"
#include "feature_points_extractor/base_feature_points_extractor.h"
namespace oh_my_loam {
class OhMyLoam {
public:
OhMyLoam() = default;
~OhMyLoam() = default;
OhMyLoam(const OhMyLoam&) = delete;
OhMyLoam& operator=(const OhMyLoam&) = delete;
bool Init(const YAML::Node& config);
bool Init();
void Run(const PointCloud& cloud, double timestamp);
private:
void Visualize(const PointCloud& cloud,
const std::shared_ptr<const FeaturePoints>& feature_pts,
double timestamp = std::nanf(""));
std::unique_ptr<FeaturePointsExtractor> feature_extractor_{nullptr};
bool is_vis_ = false;
std::unique_ptr<FeaturePointsExtractor> feature_extractor_ = nullptr;
std::unique_ptr<FeaturePointsVisualizer> visualizer_ = nullptr;
YAML::Node config_;
DISALLOW_COPY_AND_ASSIGN(OhMyLoam)
};
} // namespace oh_my_loam

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

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

0
src/visualizer_base.h → src/visualizer/base_visualizer.h
View File

43
src/visualizer/feature_points_visualizer.cc Normal file
View File

@ -0,0 +1,43 @@
#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 Normal file
View File

@ -0,0 +1,23 @@
#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

59
src/visualizer_feature_points.h
View File

@ -1,59 +0,0 @@
#pragma once
#include "feature_extractor_base.h"
#include "visualizer_base.h"
namespace oh_my_loam {
struct FeaturePointsVisFrame : public VisFrame {
std::shared_ptr<const FeaturePoints> feature_pts;
};
class FeaturePointsVisualizer : public Visualizer<FeaturePointsVisFrame> {
public:
explicit FeaturePointsVisualizer(
const std::string &name = "FeaturePointsVisFrame",
size_t max_history_size = 10)
: Visualizer<FeaturePointsVisFrame>(name, max_history_size) {}
protected:
void Draw() override final {
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<IPoint>(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