diff --git a/oh_my_loam/configs/config.yaml b/oh_my_loam/configs/config.yaml index 5ea2fd1..a0f3bce 100644 --- a/oh_my_loam/configs/config.yaml +++ b/oh_my_loam/configs/config.yaml @@ -32,7 +32,7 @@ odometer_config: # configs for mapper mapper_config: vis: false - verbose: true + verbose: false map_shape: [3, 21, 21] map_step: 50 submap_shape: [1, 5, 5] diff --git a/oh_my_loam/extractor/extractor.cc b/oh_my_loam/extractor/extractor.cc index 1bdf52e..b0166b2 100644 --- a/oh_my_loam/extractor/extractor.cc +++ b/oh_my_loam/extractor/extractor.cc @@ -75,7 +75,8 @@ void Extractor::SplitScan(const common::PointCloud &cloud, half_passed = true; yaw_diff += kTwoPi; } - double time = std::min(yaw_diff / kTwoPi, 1 - kEps) + scan_id; + // double time = std::min(yaw_diff / kTwoPi, 1 - kEps) + scan_id; + double time = 0.999999 + scan_id; scans->at(scan_id).push_back( {pt.x, pt.y, pt.z, static_cast(time), std::nanf("")}); } diff --git a/oh_my_loam/mapper/mapper.cc b/oh_my_loam/mapper/mapper.cc index 48de0c2..85cec18 100644 --- a/oh_my_loam/mapper/mapper.cc +++ b/oh_my_loam/mapper/mapper.cc @@ -1,10 +1,12 @@ #include "mapper.h" +#include #include #include "common/log/log.h" #include "common/math/fitting.h" #include "common/pcl/pcl_utils.h" +#include "oh_my_loam/visualizer/mapper_visualizer.h" namespace oh_my_loam { @@ -24,6 +26,7 @@ bool Mapper::Init() { map_step_ = config_["map_step"].as(); corn_map_.reset(new Map(map_shape_, map_step_)); surf_map_.reset(new Map(map_shape_, map_step_)); + if (is_vis_) visualizer_.reset(MapperVisualizer); return true; } @@ -63,15 +66,13 @@ void Mapper::Run(const TPointCloudConstPtr &cloud_corn, // AdjustMap(cnt); TPointCloudPtr cloud_corn_map = corn_map_->GetSurrPoints(cnt, submap_shape_); TPointCloudPtr cloud_surf_map = surf_map_->GetSurrPoints(cnt, submap_shape_); + kdtree_corn_.setInputCloud(cloud_corn_map); + kdtree_surf_.setInputCloud(cloud_surf_map); for (int i = 0; i < config_["icp_iter_num"].as(); ++i) { - pcl::KdTreeFLANN kdtree_corn; - kdtree_corn.setInputCloud(cloud_corn_map); - pcl::KdTreeFLANN kdtree_surf; - kdtree_surf.setInputCloud(cloud_surf_map); std::vector pl_pairs; - MatchCorn(kdtree_corn, cloud_corn_map, pose_curr2map, &pl_pairs); + MatchCorn(cloud_corn_map, pose_curr2map, &pl_pairs); std::vector pp_pairs; - MatchSurf(kdtree_surf, cloud_surf_map, pose_curr2map, &pp_pairs); + MatchSurf(cloud_surf_map, pose_curr2map, &pp_pairs); AINFO_IF(verbose_) << "Iter " << i << ": matched num: point2line = " << pl_pairs.size() << ", point2plane = " << pp_pairs.size(); @@ -102,8 +103,7 @@ void Mapper::Run(const TPointCloudConstPtr &cloud_corn, AUSER << "Mapper::Run: " << BLOCK_TIMER_STOP_FMT; } -void Mapper::MatchCorn(const pcl::KdTreeFLANN &kdtree, - const TPointCloudConstPtr &cloud_curr, +void Mapper::MatchCorn(const TPointCloudConstPtr &cloud_curr, const common::Pose3d &pose_curr2map, std::vector *const pairs) const { std::vector indices; @@ -113,8 +113,8 @@ void Mapper::MatchCorn(const pcl::KdTreeFLANN &kdtree, config_["neighbor_point_dist_sq_th"].as(); for (const auto &pt : *cloud_curr) { TPoint pt_query = common::TransformPoint(pose_curr2map, pt); - if (kdtree.nearestKSearch(pt_query, nearest_neighbor_k, indices, dists) != - nearest_neighbor_k) { + if (kdtree_corn_.nearestKSearch(pt_query, nearest_neighbor_k, indices, + dists) != nearest_neighbor_k) { continue; } if (dists.back() > neighbor_point_dist_sq_th) continue; @@ -127,8 +127,7 @@ void Mapper::MatchCorn(const pcl::KdTreeFLANN &kdtree, } } -void Mapper::MatchSurf(const pcl::KdTreeFLANN &kdtree, - const TPointCloudConstPtr &cloud_curr, +void Mapper::MatchSurf(const TPointCloudConstPtr &cloud_curr, const common::Pose3d &pose_curr2map, std::vector *const pairs) const { std::vector indices; @@ -138,8 +137,8 @@ void Mapper::MatchSurf(const pcl::KdTreeFLANN &kdtree, config_["neighbor_point_dist_sq_th"].as(); for (const auto &pt : *cloud_curr) { TPoint pt_query = common::TransformPoint(pose_curr2map, pt); - if (kdtree.nearestKSearch(pt_query, nearest_neighbor_k, indices, dists) != - nearest_neighbor_k) { + if (kdtree_surf_.nearestKSearch(pt_query, nearest_neighbor_k, indices, + dists) != nearest_neighbor_k) { continue; } if (dists.back() > neighbor_point_dist_sq_th) continue; @@ -230,4 +229,17 @@ void Mapper::SetState(State state) { state_ = state; } +void Mapper::Visualize(const std::vector &pl_pairs, + const std::vector &pp_pairs, + const common::Pose3d &pose_curr2odom, + const common::Pose3d &pose_curr2map, double timestamp) { + std::shared_ptr frame(new MapperVisFrame); + frame->timestamp = timestamp; + frame->cloud_corn = kdtree_corn_.getInputCloud()->makeShared(); + frame->cloud_surf = kdtree_surf_.getInputCloud()->makeShared(); + frame->pl_pairs = pl_pairs; + frame->pp_pairs = pp_pairs; + frame->pose_curr2world +} + } // namespace oh_my_loam \ No newline at end of file diff --git a/oh_my_loam/mapper/mapper.h b/oh_my_loam/mapper/mapper.h index 1b243c6..4409488 100644 --- a/oh_my_loam/mapper/mapper.h +++ b/oh_my_loam/mapper/mapper.h @@ -10,7 +10,9 @@ #include "oh_my_loam/base/types.h" #include "oh_my_loam/base/utils.h" #include "oh_my_loam/mapper/map.h" +#include "oh_my_loam/solver/cost_function.h" #include "oh_my_loam/solver/solver.h" +#include "oh_my_loam/visualizer/mapper_visualizer.h" namespace oh_my_loam { @@ -40,13 +42,11 @@ class Mapper { const TPointCloudConstPtr &cloud_surf, const common::Pose3d &pose_init); - void MatchCorn(const pcl::KdTreeFLANN &kdtree, - const TPointCloudConstPtr &cloud_curr, + void MatchCorn(const TPointCloudConstPtr &cloud_curr, const common::Pose3d &pose_curr2map, std::vector *const pairs) const; - void MatchSurf(const pcl::KdTreeFLANN &kdtree, - const TPointCloudConstPtr &cloud_curr, + void MatchSurf(const TPointCloudConstPtr &cloud_curr, const common::Pose3d &pose_curr2map, std::vector *const pairs) const; @@ -60,10 +60,17 @@ class Mapper { const TPointCloudConstPtr &cloud_corn = nullptr, const TPointCloudConstPtr &cloud_surf = nullptr); + void Visualize(const std::vector &pl_pairs, + const std::vector &pp_pairs, + const common::Pose3d &pose_curr2odom, + const common::Pose3d &pose_curr2map, double timestamp = 0.0); + // map mutable std::mutex map_mutex_; std::unique_ptr corn_map_; std::unique_ptr surf_map_; + pcl::KdTreeFLANN kdtree_corn_; + pcl::KdTreeFLANN kdtree_surf_; // state mutable std::mutex state_mutex_; State state_ = UN_INIT; @@ -77,6 +84,8 @@ class Mapper { bool is_vis_ = false; bool verbose_ = false; + std::unique_ptr visualizer_{nullptr}; + DISALLOW_COPY_AND_ASSIGN(Mapper) }; diff --git a/oh_my_loam/odometer/odometer.cc b/oh_my_loam/odometer/odometer.cc index 6ff6a53..919e494 100644 --- a/oh_my_loam/odometer/odometer.cc +++ b/oh_my_loam/odometer/odometer.cc @@ -54,10 +54,10 @@ void Odometer::Process(double timestamp, const std::vector &features, } PoseSolver solver(pose_curr2last_); for (const auto &pair : pl_pairs) { - solver.AddPointLinePair(pair, GetTime(pair.pt)); + solver.AddPointLinePair(pair, 1.0); } for (const auto &pair : pp_pairs) { - solver.AddPointPlanePair(pair, GetTime(pair.pt)); + solver.AddPointPlanePair(pair, 1.0); } bool is_converge = solver.Solve(config_["solve_iter_num"].as(), verbose_, &pose_curr2last_); @@ -190,8 +190,8 @@ void Odometer::Visualize(const std::vector &pl_pairs, double timestamp) const { std::shared_ptr frame(new OdometerVisFrame); frame->timestamp = timestamp; - frame->cloud_corn = kdtree_corn_.getInputCloud(); - frame->cloud_surf = kdtree_surf_.getInputCloud(); + frame->cloud_corn = kdtree_corn_.getInputCloud()->makeShared(); + frame->cloud_surf = kdtree_surf_.getInputCloud()->makeShared(); frame->pl_pairs = pl_pairs; frame->pp_pairs = pp_pairs; frame->pose_curr2last = pose_curr2last_; diff --git a/oh_my_loam/visualizer/mapper_visualizer.cc b/oh_my_loam/visualizer/mapper_visualizer.cc new file mode 100644 index 0000000..a837620 --- /dev/null +++ b/oh_my_loam/visualizer/mapper_visualizer.cc @@ -0,0 +1,116 @@ +#include "mapper_visualizer.h" + +#include "common/color/color.h" + +namespace oh_my_loam { + +namespace { +using namespace common; +} // namespace + +void MapperVisualizer::Draw() { + auto frame = GetCurrentFrame(); + DrawPointCloud(frame.cloud_corn, GRAY, "cloud_corn"); + DrawPointCloud(frame.cloud_surf, GRAY, "cloud_surf"); + + DrawCorn(frame.pose_curr2odom, frame.pl_pairs); + DrawSurf(frame.pose_curr2odom, frame.pp_pairs); + + poses_.push_back(frame.pose_curr2map); + DrawTrajectory(); +} + +void MapperVisualizer::DrawCorn(const Pose3d &pose_curr2odom, + const Pose3d &pose_curr2map, + const std::vector &pairs) { + TPointCloudPtr src(new TPointCloud); + TPointCloudPtr tgt(new TPointCloud); + src->resize(pairs.size()); + tgt->resize(pairs.size() * 2); + for (size_t i = 0; i < pairs.size(); ++i) { + const auto &pair = pairs[i]; + src->at(i) = pair.pt; + if (trans_) TransformToStart(pose, pair.pt, &src->points[i]); + } + DrawPointCloud(tgt, YELLOW, "tgt_corn", 4); + DrawPointCloud(src, RED, "src_corn", 4); + for (size_t i = 0; i < src->size(); ++i) { + const auto &p = src->at(i), &p1 = tgt->at(2 * i), &p2 = tgt->at(2 * i + 1); + common::AddLine(p, p1, WHITE, "corn_line1_" + std::to_string(i), + viewer_.get()); + common::AddLine(p, p2, WHITE, "corn_line2_" + std::to_string(i), + viewer_.get()); + } +} + +void MapperVisualizer::DrawSurf(const Pose3d &pose_curr2odom, + const Pose3d &pose_curr2map, + const std::vector &pairs) { + TPointCloudPtr src(new TPointCloud); + TPointCloudPtr tgt(new TPointCloud); + src->resize(pairs.size()); + tgt->resize(pairs.size() * 3); + for (size_t i = 0; i < pairs.size(); ++i) { + const auto &pair = pairs[i]; + src->at(i) = pair.pt; + if (trans_) TransformToStart(pose, pair.pt, &src->points[i]); + // tgt->at(3 * i) = pair.plane.pt1; + // tgt->at(3 * i + 1) = pair.plane.pt2; + // tgt->at(3 * i + 2) = pair.plane.pt3; + } + DrawPointCloud(tgt, BLUE, "tgt_surf", 4); + DrawPointCloud(src, CYAN, "src_surf", 4); + for (size_t i = 0; i < src->size(); ++i) { + const auto &p = src->at(i), &p1 = tgt->at(3 * i), &p2 = tgt->at(3 * i + 1), + &p3 = tgt->at(3 * i + 2); + AddLine(p, p1, WHITE, "surf_line1_" + std::to_string(i), + viewer_.get()); + AddLine(p, p2, WHITE, "surf_line2_" + std::to_string(i), + viewer_.get()); + AddLine(p, p3, WHITE, "surf_line3_" + std::to_string(i), + viewer_.get()); + } +} + +void MapperVisualizer::DrawTrajectory() { + std::vector poses_n; + poses_n.reserve((poses_.size())); + Pose3d pose_inv = poses_.back().Inv(); + for (const auto &pose : poses_) { + poses_n.emplace_back(pose_inv * pose); + }; + for (size_t i = 0; i < poses_n.size() - 1; ++i) { + Eigen::Vector3f p1 = poses_n[i].t_vec().cast(); + Eigen::Vector3f p2 = poses_n[i + 1].t_vec().cast(); + AddLine({p1.x(), p1.y(), p1.z()}, {p2.x(), p2.y(), p2.z()}, PINK, + "trajectory" + std::to_string(i), viewer_.get()); + } +} + +void MapperVisualizer::KeyboardEventCallback( + const pcl::visualization::KeyboardEvent &event) { + if (event.getKeySym() == "p" && event.keyDown()) { + std::lock_guard lock(mutex_); + ++curr_frame_iter_; + if (curr_frame_iter_ == history_frames_.end()) { + --curr_frame_iter_; + } + is_updated_ = true; + } else if (event.getKeySym() == "n" && event.keyDown()) { + std::lock_guard lock(mutex_); + if (curr_frame_iter_ != history_frames_.begin()) { + --curr_frame_iter_; + } + is_updated_ = true; + } else if (event.getKeySym() == "t" && event.keyDown()) { + trans_ = !trans_; + is_updated_ = true; + } else if (event.getKeySym() == "r" && event.keyDown()) { + viewer_->setCameraPosition(0, 0, 200, 0, 0, 0, 1, 0, 0, 0); + viewer_->setSize(2500, 1500); + trans_ = true; + is_updated_ = true; + } +} + +} // namespace oh_my_loam diff --git a/oh_my_loam/visualizer/mapper_visualizer.h b/oh_my_loam/visualizer/mapper_visualizer.h new file mode 100644 index 0000000..034cf12 --- /dev/null +++ b/oh_my_loam/visualizer/mapper_visualizer.h @@ -0,0 +1,42 @@ +#pragma once + +#include "common/visualizer/lidar_visualizer.h" +#include "oh_my_loam/solver/cost_function.h" + +namespace oh_my_loam { + +struct MapperVisFrame : public common::LidarVisFrame { + TPointCloudConstPtr cloud_surf; + TPointCloudConstPtr cloud_corn; + std::vector pl_pairs; + std::vector pp_pairs; + common::Pose3d pose_curr2odom; + common::Pose3d pose_curr2map; +}; + +class MapperVisualizer : public common::LidarVisualizer { + public: + explicit MapperVisualizer(const std::string &name = "MapperVisualizer", + size_t max_history_size = 10) + : common::LidarVisualizer(name, max_history_size) {} + + private: + void Draw() override; + + void DrawCorn(const common::Pose3d &pose, + const std::vector &pairs); + + void DrawSurf(const common::Pose3d &pose, + const std::vector &pairs); + + void DrawTrajectory(); + + void KeyboardEventCallback( + const pcl::visualization::KeyboardEvent &event) override; + + bool trans_ = true; + + std::vector poses_; +}; + +} // namespace oh_my_loam