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

adjust visualizer api

main
feixyz10 2020-12-31 20:59:08 +08:00 committed by feixyz
parent 7584eee1f9
commit cf4c361145
8 changed files with 74 additions and 97 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
CMakeLists.txt
View File

@ -1,4 +1,4 @@
cmake_minimum_required(VERSION 2.8.3)
cmake_minimum_required(VERSION 3.15)
project(oh_my_loam)
set(CMAKE_CXX_FLAGS "-std=c++17 -Wall -lpthread")
@ -26,7 +26,6 @@ find_package(catkin REQUIRED COMPONENTS
include_directories(SYSTEM
${catkin_INCLUDE_DIRS}
${PCL_INCLUDE_DIRS}
${CERES_INCLUDE_DIRS}
${G3LOG_INCLUDE_DIRS}
)
@ -53,7 +52,6 @@ target_link_libraries(main
${PCL_LIBRARIES}
${G3LOG_LIBRARIES}
${YAML_CPP_LIBRARIES}
g3log
common
oh_my_loam
extractor

1
common/log.cc
View File

@ -9,6 +9,7 @@ void InitG3Logging(bool log_to_file, const std::string& prefix,
worker->addSink(std::make_unique<g3::CustomSink>(),
&g3::CustomSink::StdLogMessage);
if (log_to_file) {
std::filesystem::create_directories(path);
std::ostringstream oss;
oss << path;
if (*path.rbegin() != '/') oss << '/';

1
common/log.h
View File

@ -1,6 +1,7 @@
#pragma once
#include <chrono>
#include <filesystem>
#include <fstream>
#include <g3log/g3log.hpp>
#include <g3log/logworker.hpp>

4
configs/config.yaml
View File

@ -1,10 +1,10 @@
# global configs
lidar: VPL16
log_to_file: false
log_path: /home/lf/catkin_ws/src/oh_my_loam/.log
log_path: /data/log/oh_my_loam
vis: true
# configs for feature points extractor
# configs for extractor
extractor_config:
min_point_num: 66
vis: false

27
src/visualizer/base_visualizer.h
View File

@ -60,7 +60,7 @@ class Visualizer {
is_updated_ = true;
}
std::string Name() { return name_; }
std::string Name() const { return name_; }
protected:
void Run() {
@ -75,15 +75,10 @@ class Visualizer {
}
/**
* @brief Draw objects. This method should be overrided for customization.
*
* @brief Draw objects, pure virtual function. Example code:
* virtual void Draw() {
* auto frame = GetCurrentFrame<VisFrame>();
* { // Update cloud
* std::string id = "point cloud";
* DrawPointCloud(*frame.cloud, {255, 255, 255}, id, viewer_.get());
* rendered_cloud_ids_.push_back(id);
* }
* AddPointCloud(frame.cloud, {255, 255, 255}, "point cloud");
* }
*/
virtual void Draw() = 0;
@ -134,6 +129,22 @@ class Visualizer {
return *static_cast<FrameT *>((*curr_frame_iter_).get());
}
template <typename PointType>
void DrawPointCloud(
const typename pcl::PointCloud<PointType>::ConstPtr &cloud,
const Color &color, const std::string &id, int point_size = 3) {
AddPointCloud<PointType>(cloud, color, id, viewer_.get(), point_size);
rendered_cloud_ids_.push_back(id);
}
template <typename PointType>
void DrawPointCloud(
const typename pcl::PointCloud<PointType>::ConstPtr &cloud,
const std::string &field, const std::string &id, int point_size = 3) {
AddPointCloud<PointType>(cloud, field, id, viewer_.get(), point_size);
rendered_cloud_ids_.push_back(id);
}
// visualizer name
std::string name_;
size_t max_history_size_;

38
src/visualizer/extractor_visualizer.cc
View File

@ -4,35 +4,15 @@ namespace oh_my_loam {
void ExtractorVisualizer::Draw() {
auto frame = GetCurrentFrame<ExtractorVisFrame>();
{ // 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);
}
DrawPointCloud<Point>(frame.cloud, WHITE, "raw_point_cloud");
DrawPointCloud<TPoint>(frame.feature_pts.less_flat_surf_pts, CYAN,
"less_flat_surf_pts");
DrawPointCloud<TPoint>(frame.feature_pts.flat_surf_pts, BLUE,
"flat_surf_pts");
DrawPointCloud<TPoint>(frame.feature_pts.less_sharp_corner_pts, PURPLE,
"less_sharp_corner_pts");
DrawPointCloud<TPoint>(frame.feature_pts.sharp_corner_pts, RED,
"sharp_corner_pts");
};
} // namespace oh_my_loam

56
src/visualizer/odometry_visualizer.cc
View File

@ -4,45 +4,31 @@ namespace oh_my_loam {
void OdometryVisualizer::Draw() {
auto frame = GetCurrentFrame<OdometryVisFrame>();
TPointCloud src_corn_pts, tgt_corn_pts;
src_corn_pts.resize(frame.pl_pairs.size());
tgt_corn_pts.resize(frame.pl_pairs.size() * 2);
TPointCloudPtr src_corn_pts(new TPointCloud);
TPointCloudPtr tgt_corn_pts(new TPointCloud);
src_corn_pts->resize(frame.pl_pairs.size());
tgt_corn_pts->resize(frame.pl_pairs.size() * 2);
for (size_t i = 0; i < frame.pl_pairs.size(); ++i) {
const auto& pair = frame.pl_pairs[i];
TransformToStart(frame.pose_curr2last, pair.pt, &src_corn_pts[i]);
tgt_corn_pts[2 * i] = pair.line.pt1;
tgt_corn_pts[2 * i + 1] = pair.line.pt2;
TransformToStart(frame.pose_curr2last, pair.pt, &src_corn_pts->points[i]);
tgt_corn_pts->points[2 * i] = pair.line.pt1;
tgt_corn_pts->points[2 * i + 1] = pair.line.pt2;
}
TPointCloud src_surf_pts, tgt_surf_pts;
src_surf_pts.resize(frame.pp_pairs.size());
tgt_surf_pts.resize(frame.pp_pairs.size() * 3);
TPointCloudPtr src_surf_pts(new TPointCloud);
TPointCloudPtr tgt_surf_pts(new TPointCloud);
src_surf_pts->resize(frame.pp_pairs.size());
tgt_surf_pts->resize(frame.pp_pairs.size() * 3);
for (size_t i = 0; i < frame.pp_pairs.size(); ++i) {
const auto& pair = frame.pp_pairs[i];
TransformToStart(frame.pose_curr2last, pair.pt, &src_surf_pts[i]);
tgt_surf_pts[3 * i] = pair.plane.pt1;
tgt_surf_pts[3 * i + 1] = pair.plane.pt2;
tgt_surf_pts[3 * i + 2] = pair.plane.pt3;
}
{ // add src_corn_pts
std::string id = "src_corn_pts";
DrawPointCloud(src_corn_pts, CYAN, id, viewer_.get(), 7);
rendered_cloud_ids_.push_back(id);
}
{ // add tgt_corn_pts
std::string id = "tgt_corn_pts";
DrawPointCloud(tgt_corn_pts, BLUE, id, viewer_.get(), 4);
rendered_cloud_ids_.push_back(id);
}
{ // add src_surf_pts
std::string id = "src_surf_pts";
DrawPointCloud(src_surf_pts, PURPLE, id, viewer_.get(), 7);
rendered_cloud_ids_.push_back(id);
}
{ // add tgt_surf_pts
std::string id = "tgt_surf_pts";
DrawPointCloud(tgt_surf_pts, RED, id, viewer_.get(), 4);
rendered_cloud_ids_.push_back(id);
TransformToStart(frame.pose_curr2last, pair.pt, &src_surf_pts->points[i]);
tgt_surf_pts->points[3 * i] = pair.plane.pt1;
tgt_surf_pts->points[3 * i + 1] = pair.plane.pt2;
tgt_surf_pts->points[3 * i + 2] = pair.plane.pt3;
}
DrawPointCloud<TPoint>(src_corn_pts, CYAN, "src_corn_pts", 7);
DrawPointCloud<TPoint>(tgt_corn_pts, BLUE, "tgt_corn_pts", 4);
DrawPointCloud<TPoint>(src_surf_pts, PURPLE, "src_surf_pts", 7);
DrawPointCloud<TPoint>(tgt_surf_pts, RED, "tgt_surf_pts", 4);
std::vector<Pose3D> poses_n;
poses_n.reserve((poses_.size()));
Pose3D pose_inv = frame.pose_curr2world.Inv();
@ -53,10 +39,10 @@ void OdometryVisualizer::Draw() {
Eigen::Vector3f p1 = poses_n[i].p().cast<float>();
if (i < poses_n.size() - 1) {
Eigen::Vector3f p2 = poses_n[i + 1].p().cast<float>();
DrawLine(Point(p1.x(), p1.y(), p1.z()), Point(p2.x(), p2.y(), p2.z()),
AddLine(Point(p1.x(), p1.y(), p1.z()), Point(p2.x(), p2.y(), p2.z()),
WHITE, "line" + std::to_string(i), viewer_.get());
} else {
DrawLine(Point(p1.x(), p1.y(), p1.z()), Point(0, 0, 0), WHITE,
AddLine(Point(p1.x(), p1.y(), p1.z()), Point(0, 0, 0), WHITE,
"line" + std::to_string(i), viewer_.get());
}
}

32
src/visualizer/utils.h
View File

@ -7,29 +7,29 @@ using PCLVisualizer = pcl::visualization::PCLVisualizer;
#define PCLColorHandlerGenericField \
pcl::visualization::PointCloudColorHandlerGenericField
template <typename PointT>
void DrawPointCloud(const pcl::PointCloud<PointT>& cloud, const Color& color,
const std::string& id, PCLVisualizer* const viewer,
int pt_size = 3) {
PCLColorHandlerCustom<PointT> color_handler(cloud.makeShared(), color.r,
color.g, color.b);
viewer->addPointCloud<PointT>(cloud.makeShared(), color_handler, id);
template <typename PointType>
void AddPointCloud(const typename pcl::PointCloud<PointType>::ConstPtr& cloud,
const Color& color, const std::string& id,
PCLVisualizer* const viewer, int point_size = 3) {
PCLColorHandlerCustom<PointType> color_handler(cloud, color.r, color.g,
color.b);
viewer->addPointCloud<PointType>(cloud, color_handler, id);
viewer->setPointCloudRenderingProperties(
pcl::visualization::PCL_VISUALIZER_POINT_SIZE, pt_size, id);
pcl::visualization::PCL_VISUALIZER_POINT_SIZE, point_size, id);
}
template <typename PointT>
void DrawPointCloud(const pcl::PointCloud<PointT>& cloud,
template <typename PointType>
void AddPointCloud(const typename pcl::PointCloud<PointType>::ConstPtr& cloud,
const std::string& field, const std::string& id,
PCLVisualizer* const viewer, int pt_size = 3) {
PCLColorHandlerGenericField<PointT> color_handler(cloud.makeShared(), field);
viewer->addPointCloud<PointT>(cloud.makeShared(), color_handler, id);
PCLVisualizer* const viewer, int point_size = 3) {
PCLColorHandlerGenericField<PointType> color_handler(cloud, field);
viewer->addPointCloud<PointType>(cloud, color_handler, id);
viewer->setPointCloudRenderingProperties(
pcl::visualization::PCL_VISUALIZER_POINT_SIZE, pt_size, id);
pcl::visualization::PCL_VISUALIZER_POINT_SIZE, point_size, id);
}
template <typename PointT>
void DrawLine(const PointT& pt1, const PointT& pt2, const Color& color,
template <typename PointType>
void AddLine(const PointType& pt1, const PointType& pt2, const Color& color,
const std::string& id, PCLVisualizer* const viewer) {
viewer->addLine(pt1, pt2, color.r, color.g, color.b, id);
}