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) project(oh_my_loam)
set(CMAKE_CXX_FLAGS "-std=c++17 -Wall -lpthread") set(CMAKE_CXX_FLAGS "-std=c++17 -Wall -lpthread")
@ -26,7 +26,6 @@ find_package(catkin REQUIRED COMPONENTS
include_directories(SYSTEM include_directories(SYSTEM
${catkin_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS}
${PCL_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS}
${CERES_INCLUDE_DIRS}
${G3LOG_INCLUDE_DIRS} ${G3LOG_INCLUDE_DIRS}
) )
@ -53,7 +52,6 @@ target_link_libraries(main
${PCL_LIBRARIES} ${PCL_LIBRARIES}
${G3LOG_LIBRARIES} ${G3LOG_LIBRARIES}
${YAML_CPP_LIBRARIES} ${YAML_CPP_LIBRARIES}
g3log
common common
oh_my_loam oh_my_loam
extractor 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>(), worker->addSink(std::make_unique<g3::CustomSink>(),
&g3::CustomSink::StdLogMessage); &g3::CustomSink::StdLogMessage);
if (log_to_file) { if (log_to_file) {
std::filesystem::create_directories(path);
std::ostringstream oss; std::ostringstream oss;
oss << path; oss << path;
if (*path.rbegin() != '/') oss << '/'; if (*path.rbegin() != '/') oss << '/';

1
common/log.h
View File

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

4
configs/config.yaml
View File

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

27
src/visualizer/base_visualizer.h
View File

@ -60,7 +60,7 @@ class Visualizer {
is_updated_ = true; is_updated_ = true;
} }
std::string Name() { return name_; } std::string Name() const { return name_; }
protected: protected:
void Run() { 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() { * virtual void Draw() {
* auto frame = GetCurrentFrame<VisFrame>(); * auto frame = GetCurrentFrame<VisFrame>();
* { // Update cloud * AddPointCloud(frame.cloud, {255, 255, 255}, "point cloud");
* std::string id = "point cloud";
* DrawPointCloud(*frame.cloud, {255, 255, 255}, id, viewer_.get());
* rendered_cloud_ids_.push_back(id);
* }
* } * }
*/ */
virtual void Draw() = 0; virtual void Draw() = 0;
@ -134,6 +129,22 @@ class Visualizer {
return *static_cast<FrameT *>((*curr_frame_iter_).get()); 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 // visualizer name
std::string name_; std::string name_;
size_t max_history_size_; size_t max_history_size_;

38
src/visualizer/extractor_visualizer.cc
View File

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

60
src/visualizer/odometry_visualizer.cc
View File

@ -4,45 +4,31 @@ namespace oh_my_loam {
void OdometryVisualizer::Draw() { void OdometryVisualizer::Draw() {
auto frame = GetCurrentFrame<OdometryVisFrame>(); auto frame = GetCurrentFrame<OdometryVisFrame>();
TPointCloud src_corn_pts, tgt_corn_pts; TPointCloudPtr src_corn_pts(new TPointCloud);
src_corn_pts.resize(frame.pl_pairs.size()); TPointCloudPtr tgt_corn_pts(new TPointCloud);
tgt_corn_pts.resize(frame.pl_pairs.size() * 2); 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) { for (size_t i = 0; i < frame.pl_pairs.size(); ++i) {
const auto& pair = frame.pl_pairs[i]; const auto& pair = frame.pl_pairs[i];
TransformToStart(frame.pose_curr2last, pair.pt, &src_corn_pts[i]); TransformToStart(frame.pose_curr2last, pair.pt, &src_corn_pts->points[i]);
tgt_corn_pts[2 * i] = pair.line.pt1; tgt_corn_pts->points[2 * i] = pair.line.pt1;
tgt_corn_pts[2 * i + 1] = pair.line.pt2; tgt_corn_pts->points[2 * i + 1] = pair.line.pt2;
} }
TPointCloud src_surf_pts, tgt_surf_pts; TPointCloudPtr src_surf_pts(new TPointCloud);
src_surf_pts.resize(frame.pp_pairs.size()); TPointCloudPtr tgt_surf_pts(new TPointCloud);
tgt_surf_pts.resize(frame.pp_pairs.size() * 3); 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) { for (size_t i = 0; i < frame.pp_pairs.size(); ++i) {
const auto& pair = frame.pp_pairs[i]; const auto& pair = frame.pp_pairs[i];
TransformToStart(frame.pose_curr2last, pair.pt, &src_surf_pts[i]); TransformToStart(frame.pose_curr2last, pair.pt, &src_surf_pts->points[i]);
tgt_surf_pts[3 * i] = pair.plane.pt1; tgt_surf_pts->points[3 * i] = pair.plane.pt1;
tgt_surf_pts[3 * i + 1] = pair.plane.pt2; tgt_surf_pts->points[3 * i + 1] = pair.plane.pt2;
tgt_surf_pts[3 * i + 2] = pair.plane.pt3; tgt_surf_pts->points[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);
} }
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; std::vector<Pose3D> poses_n;
poses_n.reserve((poses_.size())); poses_n.reserve((poses_.size()));
Pose3D pose_inv = frame.pose_curr2world.Inv(); Pose3D pose_inv = frame.pose_curr2world.Inv();
@ -53,11 +39,11 @@ void OdometryVisualizer::Draw() {
Eigen::Vector3f p1 = poses_n[i].p().cast<float>(); Eigen::Vector3f p1 = poses_n[i].p().cast<float>();
if (i < poses_n.size() - 1) { if (i < poses_n.size() - 1) {
Eigen::Vector3f p2 = poses_n[i + 1].p().cast<float>(); 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()); WHITE, "line" + std::to_string(i), viewer_.get());
} else { } 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()); "line" + std::to_string(i), viewer_.get());
} }
} }
poses_.push_back(frame.pose_curr2world); poses_.push_back(frame.pose_curr2world);

36
src/visualizer/utils.h
View File

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