From 907125f775bb79757260ee7d4eeca5f27610f607 Mon Sep 17 00:00:00 2001 From: feixyz10 Date: Thu, 31 Dec 2020 20:59:08 +0800 Subject: [PATCH] adjust visualizer api --- src/visualizer/base_visualizer.h | 27 ++++++++---- src/visualizer/extractor_visualizer.cc | 38 ++++------------ src/visualizer/odometry_visualizer.cc | 60 ++++++++++---------------- src/visualizer/utils.h | 36 ++++++++-------- 4 files changed, 69 insertions(+), 92 deletions(-) diff --git a/src/visualizer/base_visualizer.h b/src/visualizer/base_visualizer.h index 5d1b087..36b3e96 100644 --- a/src/visualizer/base_visualizer.h +++ b/src/visualizer/base_visualizer.h @@ -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(); - * { // 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((*curr_frame_iter_).get()); } + template + void DrawPointCloud( + const typename pcl::PointCloud::ConstPtr &cloud, + const Color &color, const std::string &id, int point_size = 3) { + AddPointCloud(cloud, color, id, viewer_.get(), point_size); + rendered_cloud_ids_.push_back(id); + } + + template + void DrawPointCloud( + const typename pcl::PointCloud::ConstPtr &cloud, + const std::string &field, const std::string &id, int point_size = 3) { + AddPointCloud(cloud, field, id, viewer_.get(), point_size); + rendered_cloud_ids_.push_back(id); + } + // visualizer name std::string name_; size_t max_history_size_; diff --git a/src/visualizer/extractor_visualizer.cc b/src/visualizer/extractor_visualizer.cc index 1d076d1..d9f3571 100644 --- a/src/visualizer/extractor_visualizer.cc +++ b/src/visualizer/extractor_visualizer.cc @@ -4,35 +4,15 @@ 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); - } + DrawPointCloud(frame.cloud, WHITE, "raw_point_cloud"); + DrawPointCloud(frame.feature_pts.less_flat_surf_pts, CYAN, + "less_flat_surf_pts"); + DrawPointCloud(frame.feature_pts.flat_surf_pts, BLUE, + "flat_surf_pts"); + DrawPointCloud(frame.feature_pts.less_sharp_corner_pts, PURPLE, + "less_sharp_corner_pts"); + DrawPointCloud(frame.feature_pts.sharp_corner_pts, RED, + "sharp_corner_pts"); }; } // namespace oh_my_loam diff --git a/src/visualizer/odometry_visualizer.cc b/src/visualizer/odometry_visualizer.cc index bd35909..7788c62 100644 --- a/src/visualizer/odometry_visualizer.cc +++ b/src/visualizer/odometry_visualizer.cc @@ -4,45 +4,31 @@ namespace oh_my_loam { void OdometryVisualizer::Draw() { auto frame = GetCurrentFrame(); - 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(src_corn_pts, CYAN, "src_corn_pts", 7); + DrawPointCloud(tgt_corn_pts, BLUE, "tgt_corn_pts", 4); + DrawPointCloud(src_surf_pts, PURPLE, "src_surf_pts", 7); + DrawPointCloud(tgt_surf_pts, RED, "tgt_surf_pts", 4); std::vector poses_n; poses_n.reserve((poses_.size())); Pose3D pose_inv = frame.pose_curr2world.Inv(); @@ -53,11 +39,11 @@ void OdometryVisualizer::Draw() { Eigen::Vector3f p1 = poses_n[i].p().cast(); if (i < poses_n.size() - 1) { Eigen::Vector3f p2 = poses_n[i + 1].p().cast(); - DrawLine(Point(p1.x(), p1.y(), p1.z()), Point(p2.x(), p2.y(), p2.z()), - WHITE, "line" + std::to_string(i), viewer_.get()); + 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, - "line" + std::to_string(i), viewer_.get()); + AddLine(Point(p1.x(), p1.y(), p1.z()), Point(0, 0, 0), WHITE, + "line" + std::to_string(i), viewer_.get()); } } poses_.push_back(frame.pose_curr2world); diff --git a/src/visualizer/utils.h b/src/visualizer/utils.h index f92af59..30b1972 100644 --- a/src/visualizer/utils.h +++ b/src/visualizer/utils.h @@ -7,30 +7,30 @@ using PCLVisualizer = pcl::visualization::PCLVisualizer; #define PCLColorHandlerGenericField \ pcl::visualization::PointCloudColorHandlerGenericField -template -void DrawPointCloud(const pcl::PointCloud& cloud, const Color& color, - const std::string& id, PCLVisualizer* const viewer, - int pt_size = 3) { - PCLColorHandlerCustom color_handler(cloud.makeShared(), color.r, - color.g, color.b); - viewer->addPointCloud(cloud.makeShared(), color_handler, id); +template +void AddPointCloud(const typename pcl::PointCloud::ConstPtr& cloud, + const Color& color, const std::string& id, + PCLVisualizer* const viewer, int point_size = 3) { + PCLColorHandlerCustom color_handler(cloud, color.r, color.g, + color.b); + viewer->addPointCloud(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 -void DrawPointCloud(const pcl::PointCloud& cloud, - const std::string& field, const std::string& id, - PCLVisualizer* const viewer, int pt_size = 3) { - PCLColorHandlerGenericField color_handler(cloud.makeShared(), field); - viewer->addPointCloud(cloud.makeShared(), color_handler, id); +template +void AddPointCloud(const typename pcl::PointCloud::ConstPtr& cloud, + const std::string& field, const std::string& id, + PCLVisualizer* const viewer, int point_size = 3) { + PCLColorHandlerGenericField color_handler(cloud, field); + viewer->addPointCloud(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 -void DrawLine(const PointT& pt1, const PointT& pt2, const Color& color, - const std::string& id, PCLVisualizer* const viewer) { +template +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); }