From 33a74e5f65831dd5a073037ae76cfcf4136568cc Mon Sep 17 00:00:00 2001 From: feixyz10 Date: Tue, 3 Nov 2020 15:10:16 +0800 Subject: [PATCH] odometry visualizer: ok --- CMakeLists.txt | 2 +- common/log.h | 4 +- configs/config.yaml | 4 +- src/odometry/odometry.cc | 23 +++++++-- src/odometry/odometry.h | 13 ++++- src/visualizer/odometry_visualizer.cc | 72 +++++++++++++++++++-------- src/visualizer/odometry_visualizer.h | 10 +++- src/visualizer/utils.h | 6 +++ 8 files changed, 99 insertions(+), 35 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 5f51a41..a547f7d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -57,9 +57,9 @@ target_link_libraries(main common oh_my_loam extractor - visualizer odometry solver ${CERES_LIBRARIES} helper + visualizer ) diff --git a/common/log.h b/common/log.h index 21e5b18..9426df1 100644 --- a/common/log.h +++ b/common/log.h @@ -6,8 +6,8 @@ #include #include -// heere we define G3LOG instead of use LOG directly, since LOG macro in g3log -// is conflict with LOG macro in glog. Same for G3LOG_IF and G3CHECK +// here we define G3LOG instead of use LOG directly, since LOG macro in g3log +// conflicts LOG macro in glog. Same for G3LOG_IF and G3CHECK #define G3LOG(level) \ if (!g3::logLevel(level)) { \ } else \ diff --git a/configs/config.yaml b/configs/config.yaml index 5c65223..5163c34 100644 --- a/configs/config.yaml +++ b/configs/config.yaml @@ -7,7 +7,7 @@ vis: true # configs for feature points extractor extractor_config: min_point_num: 66 - vis: true + vis: false sharp_corner_point_num: 2 corner_point_num: 20 flat_surf_point_num: 4 @@ -20,4 +20,4 @@ extractor_config: odometry_config: icp_iter_num : 2 match_dist_sq_thresh: 1 - vis: false \ No newline at end of file + vis: true \ No newline at end of file diff --git a/src/odometry/odometry.cc b/src/odometry/odometry.cc index 5344207..5637ced 100644 --- a/src/odometry/odometry.cc +++ b/src/odometry/odometry.cc @@ -13,12 +13,15 @@ bool Odometry::Init(const YAML::Node& config) { config_ = config; kdtree_surf_pts_.reset(new pcl::KdTreeFLANN); kdtree_corn_pts_.reset(new pcl::KdTreeFLANN); + is_vis_ = Config::Instance()->Get("vis") && config_["vis"].as(); + AINFO << "Odometry visualizer: " << (is_vis_ ? "ON" : "OFF"); + if (is_vis_) visualizer_.reset(new OdometryVisualizer); return true; } void Odometry::Process(const FeaturePoints& feature, Pose3D* const pose) { - if (!is_initialized) { - is_initialized = true; + if (!is_initialized_) { + is_initialized_ = true; UpdatePre(feature); *pose = pose_curr2world_; AWARN << "Odometry initialized...."; @@ -27,10 +30,10 @@ void Odometry::Process(const FeaturePoints& feature, Pose3D* const pose) { TicToc timer_whole; double match_dist_sq_thresh = config_["match_dist_sq_thresh"].as(); AINFO << "Pose before: " << pose_curr2world_.ToString(); + std::vector pl_pairs; + std::vector pp_pairs; for (int i = 0; i < config_["icp_iter_num"].as(); ++i) { TicToc timer; - std::vector pl_pairs; - std::vector pp_pairs; MatchCornPoints(*feature.sharp_corner_pts, *corn_pts_pre_, &pl_pairs, match_dist_sq_thresh); double time_pl_match = timer.toc(); @@ -73,6 +76,7 @@ void Odometry::Process(const FeaturePoints& feature, Pose3D* const pose) { AWARN << "Pose after: " << pose_curr2world_.ToString(); UpdatePre(feature); AUSER << "Time elapsed (ms): whole odometry = " << timer_whole.toc(); + if (is_vis_) Visualize(feature, pl_pairs, pp_pairs); } void Odometry::MatchCornPoints(const TPointCloud& src, const TPointCloud& tgt, @@ -192,4 +196,15 @@ void Odometry::UpdatePre(const FeaturePoints& feature) { kdtree_corn_pts_->setInputCloud(corn_pts_pre_); } +void Odometry::Visualize(const FeaturePoints& feature, + const std::vector& pl_pairs, + const std::vector& pp_pairs) const { + std::shared_ptr frame(new OdometryVisFrame); + frame->pl_pairs = pl_pairs; + frame->pp_pairs = pp_pairs; + frame->pose_curr2last = pose_curr2last_; + frame->pose_curr2world = pose_curr2world_; + visualizer_->Render(frame); +} + } // namespace oh_my_loam diff --git a/src/odometry/odometry.h b/src/odometry/odometry.h index 77ee2e1..9414c06 100644 --- a/src/odometry/odometry.h +++ b/src/odometry/odometry.h @@ -1,6 +1,7 @@ #pragma once -#include +#include "extractor/feature_points.h" +#include "visualizer/odometry_visualizer.h" #include "common.h" #include "helper/helper.h" @@ -18,6 +19,7 @@ class Odometry { protected: void UpdatePre(const FeaturePoints& feature); + void MatchCornPoints(const TPointCloud& src, const TPointCloud& tgt, std::vector* const pairs, double dist_sq_thresh) const; @@ -26,6 +28,10 @@ class Odometry { std::vector* const pairs, double dist_sq_thresh) const; + void Visualize(const FeaturePoints& feature, + const std::vector& pl_pairs, + const std::vector& pp_pairs) const; + Pose3D pose_curr2world_; Pose3D pose_curr2last_; @@ -35,9 +41,12 @@ class Odometry { pcl::KdTreeFLANN::Ptr kdtree_surf_pts_{nullptr}; pcl::KdTreeFLANN::Ptr kdtree_corn_pts_{nullptr}; - bool is_initialized = false; + bool is_initialized_ = false; + bool is_vis_ = false; YAML::Node config_; + std::unique_ptr visualizer_{nullptr}; + DISALLOW_COPY_AND_ASSIGN(Odometry) }; diff --git a/src/visualizer/odometry_visualizer.cc b/src/visualizer/odometry_visualizer.cc index f03770d..bd35909 100644 --- a/src/visualizer/odometry_visualizer.cc +++ b/src/visualizer/odometry_visualizer.cc @@ -1,38 +1,66 @@ -#include "Odometry_visualizer.h" +#include "odometry_visualizer.h" namespace oh_my_loam { void OdometryVisualizer::Draw() { auto frame = GetCurrentFrame(); - { // add raw point cloud - std::string id = "raw point cloud"; - DrawPointCloud(*frame.cloud, WHITE, id, viewer_.get()); + TPointCloud src_corn_pts, tgt_corn_pts; + 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; + } + TPointCloud src_surf_pts, tgt_surf_pts; + 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 less_flat_surf_pts - std::string id = "less_flat_surf_pts"; - DrawPointCloud(*frame.feature_pts.less_flat_surf_pts, CYAN, id, - viewer_.get(), 5); + { // 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 flat_surf_pts - std::string id = "flat_surf_pts"; - DrawPointCloud(*frame.feature_pts.flat_surf_pts, BLUE, id, viewer_.get(), - 7); + { // 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 less_sharp_corner_pts - std::string id = "less_sharp_corner_pts"; - DrawPointCloud(*frame.feature_pts.less_sharp_corner_pts, PURPLE, id, - viewer_.get(), 5); + { // 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); } - { // 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); + std::vector poses_n; + poses_n.reserve((poses_.size())); + Pose3D pose_inv = frame.pose_curr2world.Inv(); + for (const auto& pose : poses_) { + poses_n.emplace_back(pose_inv * pose); + }; + for (size_t i = 0; i < poses_n.size(); ++i) { + 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()); + } else { + DrawLine(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); +} } // namespace oh_my_loam diff --git a/src/visualizer/odometry_visualizer.h b/src/visualizer/odometry_visualizer.h index ea081aa..c6b417c 100644 --- a/src/visualizer/odometry_visualizer.h +++ b/src/visualizer/odometry_visualizer.h @@ -1,12 +1,17 @@ #pragma once #include "base_visualizer.h" -#include "extractor/feature_points.h" +#include "helper/helper.h" namespace oh_my_loam { struct OdometryVisFrame : public VisFrame { - FeaturePoints feature_pts; + TPointCloudPtr surf_pts; + TPointCloudPtr corn_pts; + std::vector pl_pairs; + std::vector pp_pairs; + Pose3D pose_curr2last; + Pose3D pose_curr2world; }; class OdometryVisualizer : public Visualizer { @@ -17,6 +22,7 @@ class OdometryVisualizer : public Visualizer { private: void Draw() override; + std::deque poses_; }; } // namespace oh_my_loam diff --git a/src/visualizer/utils.h b/src/visualizer/utils.h index a8471ab..f92af59 100644 --- a/src/visualizer/utils.h +++ b/src/visualizer/utils.h @@ -28,4 +28,10 @@ void DrawPointCloud(const pcl::PointCloud& cloud, pcl::visualization::PCL_VISUALIZER_POINT_SIZE, pt_size, id); } +template +void DrawLine(const PointT& pt1, const PointT& pt2, const Color& color, + const std::string& id, PCLVisualizer* const viewer) { + viewer->addLine(pt1, pt2, color.r, color.g, color.b, id); +} + } // namespace oh_my_loam \ No newline at end of file