From e371c2499f8fbb7c980c1cfeef28a7b21688cf35 Mon Sep 17 00:00:00 2001 From: feixyz10 Date: Mon, 8 Feb 2021 20:16:45 +0800 Subject: [PATCH] it works!!! --- oh_my_loam/configs/config.yaml | 4 +- oh_my_loam/mapper/map.cc | 44 +++++++------- oh_my_loam/mapper/map.h | 18 +++--- oh_my_loam/mapper/mapper.cc | 36 ++++++------ oh_my_loam/mapper/mapper.h | 4 +- oh_my_loam/oh_my_loam.cc | 17 ++---- oh_my_loam/oh_my_loam.h | 18 +++--- oh_my_loam/visualizer/mapper_visualizer.cc | 8 +-- oh_my_loam/visualizer/mapper_visualizer.h | 2 - oh_my_loam/visualizer/odometer_visualizer.cc | 45 +------------- oh_my_loam/visualizer/odometer_visualizer.h | 7 +-- oh_my_loam/visualizer/ohmyloam_visualizer.cc | 62 +++----------------- oh_my_loam/visualizer/ohmyloam_visualizer.h | 17 ++---- 13 files changed, 81 insertions(+), 201 deletions(-) diff --git a/oh_my_loam/configs/config.yaml b/oh_my_loam/configs/config.yaml index f6db393..a0f3bce 100644 --- a/oh_my_loam/configs/config.yaml +++ b/oh_my_loam/configs/config.yaml @@ -2,7 +2,7 @@ lidar: VPL16 log_to_file: false log_path: /data/log/oh_my_loam -vis: false +vis: true # configs for extractor extractor_config: @@ -31,7 +31,7 @@ odometer_config: # configs for mapper mapper_config: - vis: true + vis: false verbose: false map_shape: [3, 21, 21] map_step: 50 diff --git a/oh_my_loam/mapper/map.cc b/oh_my_loam/mapper/map.cc index 7eb31d6..38df0ee 100644 --- a/oh_my_loam/mapper/map.cc +++ b/oh_my_loam/mapper/map.cc @@ -19,8 +19,8 @@ Map::Map(const std::vector &shape, const std::vector &step) { Map::Map(const std::vector &shape, double step) : Map(shape, {step, step, step}) {} -void Map::Clear() { - for (auto &grid : map_) grid.Clear(); +void Map::clear() { + for (auto &grid : map_) grid.clear(); } TPointCloudPtr &Map::at(int z_idx, int y_idx, int x_idx) { @@ -45,13 +45,13 @@ void Map::ShiftZ(int n) { for (int k = shape_[0] - 1; k >= -n; --k) { std::swap(this->at(k), this->at(k + n)); } - for (int k = 0; k < -n; ++k) this->at(k).Clear(); + for (int k = 0; k < -n; ++k) this->at(k).clear(); } else { for (int k = 0; k < shape_[0] - n; ++k) { std::swap(this->at(k), this->at(k + n)); } for (int k = shape_[0] - n; k < shape_[0]; ++k) { - this->at(k).Clear(); + this->at(k).clear(); } } center_[0] -= n; @@ -64,13 +64,13 @@ void Map::ShiftY(int n) { for (int j = shape_[1] - 1; j >= -n; --j) { std::swap(this->at(k, j), this->at(k, j + n)); } - for (int j = 0; j < -n; ++j) this->at(k, j).Clear(); + for (int j = 0; j < -n; ++j) this->at(k, j).clear(); } else { for (int j = 0; j < shape_[1] - n; ++j) { std::swap(this->at(k, j), this->at(k, j + n)); } for (int j = shape_[1] - n; j < shape_[1]; ++j) { - this->at(k, j).Clear(); + this->at(k, j).clear(); } } } @@ -108,17 +108,16 @@ Index Map::GetIndex(const TPoint &point) const { return index; } -bool Map::IsIndexValid(const Index &index) const { - int k = index.k + center_[0], j = index.j + center_[1], - i = index.i + center_[2]; +bool Map::CheckIndex(const Index &index) const { + int k = index.k, j = index.j, i = index.i; return k >= 0 && k < shape_[0] && j >= 0 && j < shape_[1] && i >= 0 && i < shape_[2]; } -TPointCloudPtr Map::GetSurrPoints(const TPoint &point, - const std::vector &surr_shapes) const { +TPointCloudPtr Map::GetSubmapPoints( + const TPoint &point, const std::vector &submap_shapes) const { TPointCloudPtr cloud(new TPointCloud); - for (const auto &index : GetSurrIndices(point, surr_shapes)) { + for (const auto &index : GetSubmapIndices(point, submap_shapes)) { *cloud += *this->at(index); } return cloud; @@ -135,7 +134,7 @@ void Map::AddPoints(const TPointCloudConstPtr &cloud, std::set index_set; for (const auto &point : *cloud) { Index index = GetIndex(point); - if (!IsIndexValid(index)) continue; + if (!CheckIndex(index)) continue; this->at(index)->push_back(point); if (indices) index_set.insert(index); } @@ -151,11 +150,9 @@ void Map::Downsample(double voxel_size) { void Map::Downsample(const std::vector &indices, double voxel_size) { for (const auto &index : indices) { - if (!IsIndexValid(index)) continue; - TPointCloudPtr cloud_down_sampled(new TPointCloud); - common::VoxelDownSample(this->at(index), cloud_down_sampled.get(), + if (!CheckIndex(index)) continue; + common::VoxelDownSample(this->at(index), this->at(index).get(), voxel_size); - this->at(index) = cloud_down_sampled; } } @@ -175,11 +172,12 @@ const Row &Map::at(int z_idx, int y_idx) const { return map_.at(z_idx).at(y_idx); } -std::vector Map::GetSurrIndices( - const TPoint &point, const std::vector &surr_shapes) const { +std::vector Map::GetSubmapIndices( + const TPoint &point, const std::vector &submap_shapes) const { std::vector indices; Index index = GetIndex(point); - int nz = surr_shapes[0] / 2, ny = surr_shapes[1] / 2, nx = surr_shapes[2] / 2; + int nz = submap_shapes[0] / 2, ny = submap_shapes[1] / 2, + nx = submap_shapes[2] / 2; for (int k = -nz; k <= nz; ++k) { int idx_k = index.k + k; if (idx_k < 0 || idx_k >= shape_[0]) continue; @@ -221,7 +219,7 @@ const TPointCloudPtr &Row::at(int idx) const { return row_.at(idx); } -void Row::Clear() { +void Row::clear() { for (auto &cloud : row_) cloud->clear(); } @@ -235,8 +233,8 @@ Grid::Grid(int m, int n) { for (int i = 0; i < m; ++i) grid_.emplace_back(n); } -void Grid::Clear() { - for (auto &row : grid_) row.Clear(); +void Grid::clear() { + for (auto &row : grid_) row.clear(); } Row &Grid::at(int idx) { diff --git a/oh_my_loam/mapper/map.h b/oh_my_loam/mapper/map.h index 2dd83c4..4101cb2 100644 --- a/oh_my_loam/mapper/map.h +++ b/oh_my_loam/mapper/map.h @@ -34,7 +34,7 @@ class Map { const TPointCloudPtr &at(const Index &index) const; - void Clear(); + void clear(); void ShiftZ(int n); @@ -42,16 +42,16 @@ class Map { void ShiftX(int n); - const std::vector Shape() const { + const std::vector shape() const { return std::vector(shape_, shape_ + 3); } Index GetIndex(const TPoint &point) const; - bool IsIndexValid(const Index &index) const; + bool CheckIndex(const Index &index) const; - TPointCloudPtr GetSurrPoints(const TPoint &point, - const std::vector &surr_shapes) const; + TPointCloudPtr GetSubmapPoints(const TPoint &point, + const std::vector &submap_shapes) const; TPointCloudPtr GetAllPoints() const; @@ -75,8 +75,8 @@ class Map { const TPointCloudPtr &at(int z_idx, int y_idx, int x_idx) const; - std::vector GetSurrIndices(const TPoint &point, - const std::vector &surr_shapes) const; + std::vector GetSubmapIndices( + const TPoint &point, const std::vector &submap_shapes) const; std::vector GetAllIndices() const; @@ -97,7 +97,7 @@ class Row { const TPointCloudPtr &at(int idx) const; - void Clear(); + void clear(); TPointCloudPtr GetAllPoints() const; @@ -109,7 +109,7 @@ class Grid { public: Grid(int m, int n); - void Clear(); + void clear(); Row &at(int idx); diff --git a/oh_my_loam/mapper/mapper.cc b/oh_my_loam/mapper/mapper.cc index 7ec292e..f1af48a 100644 --- a/oh_my_loam/mapper/mapper.cc +++ b/oh_my_loam/mapper/mapper.cc @@ -53,6 +53,7 @@ void Mapper::Process(double timestamp, const TPointCloudConstPtr &cloud_corn, } std::lock_guard lock(state_mutex_); *pose_curr2map = pose_odom2map_ * pose_curr2odom; + AINFO << "Pose_curr2map = " << pose_curr2map->ToString(); } void Mapper::Run(const TPointCloudConstPtr &cloud_corn, @@ -61,14 +62,15 @@ void Mapper::Run(const TPointCloudConstPtr &cloud_corn, BLOCK_TIMER_START; SetState(RUNNING); common::Pose3d pose_curr2map = pose_odom2map_ * pose_curr2odom; - TPoint cnt(pose_curr2map.t_vec().x(), pose_curr2map.t_vec().y(), - pose_curr2map.t_vec().z()); - // 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_); + TPoint t_vec(pose_curr2map.t_vec().x(), pose_curr2map.t_vec().y(), + pose_curr2map.t_vec().z()); + AdjustMap(t_vec); + TPointCloudPtr cloud_corn_map = + corn_map_->GetSubmapPoints(t_vec, submap_shape_); + TPointCloudPtr cloud_surf_map = + surf_map_->GetSubmapPoints(t_vec, submap_shape_); + kdtree_corn_.setInputCloud(cloud_corn_map); + kdtree_surf_.setInputCloud(cloud_surf_map); std::vector pl_pairs; std::vector pp_pairs; for (int i = 0; i < config_["icp_iter_num"].as(); ++i) { @@ -102,6 +104,7 @@ void Mapper::Run(const TPointCloudConstPtr &cloud_corn, UpdateMap(pose_curr2map, cloud_corn, cloud_surf); std::lock_guard lock(state_mutex_); pose_odom2map_ = pose_curr2map * pose_curr2odom.Inv(); + AINFO << "Pose_curr2map = " << pose_curr2map.ToString(); AUSER << "Mapper::Run: " << BLOCK_TIMER_STOP_FMT; if (is_vis_) Visualize(pl_pairs, pp_pairs, pose_curr2odom, pose_curr2map); state_ = DONE; // be careful with deadlock @@ -194,19 +197,16 @@ void Mapper::UpdateMap(const common::Pose3d &pose_curr2map, const TPointCloudConstPtr &cloud_corn, const TPointCloudConstPtr &cloud_surf) { auto update_map = [&](const TPointCloudConstPtr &cloud, - /*Map*const map*/ const TPointCloudPtr &cloud_map) { + Map *const map /* const TPointCloudPtr &cloud_map*/) { TPointCloudPtr cloud_trans(new TPointCloud); common::TransformPointCloud(pose_curr2map, *cloud, cloud_trans.get()); - *cloud_map += *cloud_trans; - // std::vector indices; - // map->AddPoints(cloud_trans, &indices); - // map->Downsample(indices, config_["downsample_voxel_size"].as()); - common::VoxelDownSample(cloud_map, cloud_map.get(), - config_["downsample_voxel_size"].as()); + std::vector indices; + map->AddPoints(cloud_trans, &indices); + map->Downsample(indices, config_["downsample_voxel_size"].as()); }; std::lock_guard lock(map_mutex_); - update_map(cloud_corn, cloud_corn_map_); - update_map(cloud_surf, cloud_surf_map_); + update_map(cloud_corn, corn_map_.get()); + update_map(cloud_surf, surf_map_.get()); } TPointCloudPtr Mapper::GetMapCloudCorn() const { @@ -245,8 +245,6 @@ void Mapper::Visualize(const std::vector &pl_pairs, frame->timestamp = timestamp; frame->cloud_corn = kdtree_corn_.getInputCloud()->makeShared(); frame->cloud_surf = kdtree_surf_.getInputCloud()->makeShared(); - AINFO << "Map point num: " << frame->cloud_corn->size() << ", " - << frame->cloud_surf->size(); frame->pl_pairs = pl_pairs; frame->pp_pairs = pp_pairs; frame->pose_curr2odom = pose_curr2odom; diff --git a/oh_my_loam/mapper/mapper.h b/oh_my_loam/mapper/mapper.h index 6914116..9372247 100644 --- a/oh_my_loam/mapper/mapper.h +++ b/oh_my_loam/mapper/mapper.h @@ -83,10 +83,8 @@ class Mapper { double map_step_; bool is_vis_ = false; bool verbose_ = false; - + // visualizer std::unique_ptr visualizer_{nullptr}; - TPointCloudPtr cloud_corn_map_{new TPointCloud}; - TPointCloudPtr cloud_surf_map_{new TPointCloud}; DISALLOW_COPY_AND_ASSIGN(Mapper) }; diff --git a/oh_my_loam/oh_my_loam.cc b/oh_my_loam/oh_my_loam.cc index 83fcf9e..67cd1dd 100644 --- a/oh_my_loam/oh_my_loam.cc +++ b/oh_my_loam/oh_my_loam.cc @@ -54,25 +54,20 @@ void OhMyLoam::Run(double timestamp, const auto &cloud_surf = odometer_->GetCloudSurf()->makeShared(); mapper_->Process(timestamp, cloud_corn, cloud_surf, pose_curr2odom, &pose_curr2map); - poses_curr2odom_.push_back(pose_curr2odom); - poses_curr2map_.push_back(pose_curr2map); if (is_vis_) { - Visualize(pose_curr2odom, pose_curr2map, cloud_corn, cloud_surf, timestamp); + Visualize(pose_curr2map, cloud_corn, cloud_surf, timestamp); } } -void OhMyLoam::Visualize(const common::Pose3d &pose_curr2odom, - const common::Pose3d &pose_curr2map, - const TPointCloudConstPtr &cloud_corn, - const TPointCloudConstPtr &cloud_surf, - double timestamp) { +void OhMyLoam::Visualize(const common::Pose3d &pose_curr2map, + const TPointCloudPtr &cloud_corn, + const TPointCloudPtr &cloud_surf, double timestamp) { std::shared_ptr frame(new OhmyloamVisFrame); frame->timestamp = timestamp; frame->cloud_map_corn = mapper_->GetMapCloudCorn(); frame->cloud_map_surf = mapper_->GetMapCloudSurf(); - frame->cloud_corn = cloud_corn; - frame->cloud_surf = cloud_surf; - frame->pose_odom = pose_curr2odom; + frame->cloud_corn = cloud_corn->makeShared(); + frame->cloud_surf = cloud_surf->makeShared(); frame->pose_map = pose_curr2map; visualizer_->Render(frame); } diff --git a/oh_my_loam/oh_my_loam.h b/oh_my_loam/oh_my_loam.h index c274a5a..8fd021a 100644 --- a/oh_my_loam/oh_my_loam.h +++ b/oh_my_loam/oh_my_loam.h @@ -21,10 +21,13 @@ class OhMyLoam { private: void Reset(); - void Visualize(const common::Pose3d &pose_curr2odom, - const common::Pose3d &pose_curr2map, - const TPointCloudConstPtr &cloud_corn, - const TPointCloudConstPtr &cloud_surf, double timestamp = 0.0); + void Visualize(const common::Pose3d &pose_curr2map, + const TPointCloudPtr &cloud_corn, + const TPointCloudPtr &cloud_surf, double timestamp = 0.0); + + // remove outliers: nan or very close points + void RemoveOutliers(const common::PointCloud &cloud_in, + common::PointCloud *const cloud_out) const; std::unique_ptr extractor_{nullptr}; @@ -32,13 +35,6 @@ class OhMyLoam { std::unique_ptr mapper_{nullptr}; - // remove outliers: nan or very close points - void RemoveOutliers(const common::PointCloud &cloud_in, - common::PointCloud *const cloud_out) const; - - std::vector poses_curr2odom_; - std::vector poses_curr2map_; - YAML::Node config_; bool is_vis_ = false; diff --git a/oh_my_loam/visualizer/mapper_visualizer.cc b/oh_my_loam/visualizer/mapper_visualizer.cc index 1405290..6051046 100644 --- a/oh_my_loam/visualizer/mapper_visualizer.cc +++ b/oh_my_loam/visualizer/mapper_visualizer.cc @@ -18,7 +18,8 @@ void MapperVisualizer::Draw() { traj_odom_.AddPose(frame.pose_curr2odom); traj_map_.AddPose(frame.pose_curr2map); - DrawTrajectory(); + AddTrajectory(traj_odom_, PINK, "traj_odmo", viewer_.get()); + AddTrajectory(traj_map_, PURPLE, "traj_map", viewer_.get()); } void MapperVisualizer::DrawCorn(const common::Pose3d &pose_odom, @@ -47,11 +48,6 @@ void MapperVisualizer::DrawSurf(const common::Pose3d &pose_odom, DrawPointCloud(src, CYAN, "src_surf", 4); } -void MapperVisualizer::DrawTrajectory() { - common::AddTrajectory(traj_odom_, PINK, "traj_odmo", viewer_.get()); - common::AddTrajectory(traj_map_, PURPLE, "traj_map", viewer_.get()); -} - void MapperVisualizer::KeyboardEventCallback( const pcl::visualization::KeyboardEvent &event) { if (event.getKeySym() == "p" && event.keyDown()) { diff --git a/oh_my_loam/visualizer/mapper_visualizer.h b/oh_my_loam/visualizer/mapper_visualizer.h index db9092f..4742ba8 100644 --- a/oh_my_loam/visualizer/mapper_visualizer.h +++ b/oh_my_loam/visualizer/mapper_visualizer.h @@ -30,8 +30,6 @@ class MapperVisualizer : public common::LidarVisualizer { void DrawSurf(const common::Pose3d &pose_odom, const common::Pose3d &pose_map, const std::vector &pairs); - void DrawTrajectory(); - void KeyboardEventCallback( const pcl::visualization::KeyboardEvent &event) override; diff --git a/oh_my_loam/visualizer/odometer_visualizer.cc b/oh_my_loam/visualizer/odometer_visualizer.cc index d9e629b..ab4f500 100644 --- a/oh_my_loam/visualizer/odometer_visualizer.cc +++ b/oh_my_loam/visualizer/odometer_visualizer.cc @@ -16,8 +16,8 @@ void OdometerVisualizer::Draw() { DrawCorn(frame.pose_curr2last, frame.pl_pairs); DrawSurf(frame.pose_curr2last, frame.pp_pairs); - poses_.push_back(frame.pose_curr2world); - DrawTrajectory(); + traj_.AddPose(frame.pose_curr2world); + AddTrajectory(traj_.Copy(true), PINK, "trajectory", viewer_.get()); } void OdometerVisualizer::DrawCorn(const Pose3d &pose, @@ -35,14 +35,6 @@ void OdometerVisualizer::DrawCorn(const Pose3d &pose, } DrawPointCloud(tgt, YELLOW, "tgt_corn", 4); DrawPointCloud(src, RED, "src_corn", 4); - if (!corn_connect_) return; - 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 OdometerVisualizer::DrawSurf(const Pose3d &pose, @@ -61,32 +53,6 @@ void OdometerVisualizer::DrawSurf(const Pose3d &pose, } DrawPointCloud(tgt, BLUE, "tgt_surf", 4); DrawPointCloud(src, CYAN, "src_surf", 4); - if (!surf_connect_) return; - 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 OdometerVisualizer::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 OdometerVisualizer::KeyboardEventCallback( @@ -107,17 +73,10 @@ void OdometerVisualizer::KeyboardEventCallback( } else if (event.getKeySym() == "t" && event.keyDown()) { trans_ = !trans_; is_updated_ = true; - } else if (event.getKeySym() == "c" && event.keyDown()) { - corn_connect_ = !corn_connect_; - is_updated_ = true; - } else if (event.getKeySym() == "s" && event.keyDown()) { - surf_connect_ = !surf_connect_; - 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; - corn_connect_ = surf_connect_ = false; is_updated_ = true; } } diff --git a/oh_my_loam/visualizer/odometer_visualizer.h b/oh_my_loam/visualizer/odometer_visualizer.h index 1eba9d3..8420bd9 100644 --- a/oh_my_loam/visualizer/odometer_visualizer.h +++ b/oh_my_loam/visualizer/odometer_visualizer.h @@ -1,5 +1,6 @@ #pragma once +#include "common/geometry/trajectory.h" #include "common/visualizer/lidar_visualizer.h" #include "oh_my_loam/solver/cost_function.h" @@ -29,16 +30,12 @@ class OdometerVisualizer : public common::LidarVisualizer { void DrawSurf(const common::Pose3d &pose, const std::vector &pairs); - void DrawTrajectory(); - void KeyboardEventCallback( const pcl::visualization::KeyboardEvent &event) override; bool trans_ = true; - bool corn_connect_ = false; - bool surf_connect_ = false; - std::vector poses_; + common::Trajectory traj_; }; } // namespace oh_my_loam diff --git a/oh_my_loam/visualizer/ohmyloam_visualizer.cc b/oh_my_loam/visualizer/ohmyloam_visualizer.cc index b45a9df..0fb8087 100644 --- a/oh_my_loam/visualizer/ohmyloam_visualizer.cc +++ b/oh_my_loam/visualizer/ohmyloam_visualizer.cc @@ -15,60 +15,14 @@ void OhmyloamVisualizer::Draw() { auto frame = GetCurrentFrame(); DrawPointCloud(frame.cloud_map_corn, GRAY, "cloud_map_corn"); DrawPointCloud(frame.cloud_map_surf, GRAY, "cloud_map_surf"); - if (!trans_) { - DrawPointCloud(frame.cloud_corn, RED, "cloud_corn"); - DrawPointCloud(frame.cloud_surf, CYAN, "cloud_surf"); - } else { - TPointCloudPtr cloud_corn_trans(new TPointCloud); - TPointCloudPtr cloud_surf_trans(new TPointCloud); - common::TransformPointCloud(frame.pose_map, *frame.cloud_corn, - cloud_corn_trans.get()); - common::TransformPointCloud(frame.pose_map, *frame.cloud_surf, - cloud_surf_trans.get()); - DrawPointCloud(cloud_corn_trans, RED, "cloud_corn"); - DrawPointCloud(cloud_surf_trans, CYAN, "cloud_surf"); - } - // poses_odom_.push_back(frame.pose_odom); - // DrawTrajectory(poses_odom_, "odom_traj", PINK); - poses_map_.push_back(frame.pose_map); - DrawTrajectory(poses_map_, "map_traj", WHITE); -} - -void OhmyloamVisualizer::DrawTrajectory( - const std::vector &poses, const std::string &id, - const common::Color &color) { - for (size_t i = 0; i < poses.size() - 1; ++i) { - Eigen::Vector3f p1 = poses[i].t_vec().cast(); - Eigen::Vector3f p2 = poses[i + 1].t_vec().cast(); - AddLine({p1.x(), p1.y(), p1.z()}, {p2.x(), p2.y(), p2.z()}, color, - id + std::to_string(i), viewer_.get()); - } -} - -void OhmyloamVisualizer::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; - } + TransformPointCloud(frame.pose_map, *frame.cloud_corn, + frame.cloud_corn.get()); + DrawPointCloud(frame.cloud_corn, "time", "cloud_corn"); + TransformPointCloud(frame.pose_map, *frame.cloud_surf, + frame.cloud_surf.get()); + DrawPointCloud(frame.cloud_surf, "time", "cloud_surf"); + traj_.AddPose(frame.pose_map); + AddTrajectory(traj_, PINK, "trajectory", viewer_.get()); } } // namespace oh_my_loam \ No newline at end of file diff --git a/oh_my_loam/visualizer/ohmyloam_visualizer.h b/oh_my_loam/visualizer/ohmyloam_visualizer.h index ff778e8..8af3db4 100644 --- a/oh_my_loam/visualizer/ohmyloam_visualizer.h +++ b/oh_my_loam/visualizer/ohmyloam_visualizer.h @@ -1,6 +1,6 @@ #pragma once -#include "common/geometry/pose3d.h" +#include "common/geometry/trajectory.h" #include "common/visualizer/lidar_visualizer.h" #include "oh_my_loam/base/types.h" @@ -9,9 +9,8 @@ namespace oh_my_loam { struct OhmyloamVisFrame : public common::LidarVisFrame { TPointCloudConstPtr cloud_map_corn; TPointCloudConstPtr cloud_map_surf; - TPointCloudConstPtr cloud_corn; // current - TPointCloudConstPtr cloud_surf; // current - common::Pose3d pose_odom; + TPointCloudPtr cloud_corn; // current + TPointCloudPtr cloud_surf; // current common::Pose3d pose_map; }; @@ -24,15 +23,7 @@ class OhmyloamVisualizer : public common::LidarVisualizer { private: void Draw() override; - void DrawTrajectory(const std::vector &poses, - const std::string &id, const common::Color &color); - - void KeyboardEventCallback( - const pcl::visualization::KeyboardEvent &event) override; - - std::vector poses_odom_; - std::vector poses_map_; - bool trans_ = true; + common::Trajectory traj_; }; } // namespace oh_my_loam