diff --git a/oh_my_loam/configs/config.yaml b/oh_my_loam/configs/config.yaml index a0f3bce..147f537 100644 --- a/oh_my_loam/configs/config.yaml +++ b/oh_my_loam/configs/config.yaml @@ -3,6 +3,8 @@ lidar: VPL16 log_to_file: false log_path: /data/log/oh_my_loam vis: true +save_map: true +save_map_path: /data/log/oh_my_loam.pcd # configs for extractor extractor_config: @@ -33,7 +35,7 @@ odometer_config: mapper_config: vis: false verbose: false - map_shape: [3, 21, 21] + map_shape: [5, 21, 21] map_step: 50 submap_shape: [1, 5, 5] icp_iter_num: 2 diff --git a/oh_my_loam/oh_my_loam.cc b/oh_my_loam/oh_my_loam.cc index 67cd1dd..4a8238c 100644 --- a/oh_my_loam/oh_my_loam.cc +++ b/oh_my_loam/oh_my_loam.cc @@ -30,7 +30,10 @@ bool OhMyLoam::Init() { AERROR << "Failed to initialize mapper"; return false; } - if (is_vis_) visualizer_.reset(new OhmyloamVisualizer); + if (is_vis_) { + visualizer_.reset( + new OhmyloamVisualizer(config_["save_map_path"].as())); + } return true; } @@ -60,14 +63,15 @@ void OhMyLoam::Run(double timestamp, } void OhMyLoam::Visualize(const common::Pose3d &pose_curr2map, - const TPointCloudPtr &cloud_corn, - const TPointCloudPtr &cloud_surf, double timestamp) { + const TPointCloudConstPtr &cloud_corn, + const TPointCloudConstPtr &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->makeShared(); - frame->cloud_surf = cloud_surf->makeShared(); + frame->cloud_corn = cloud_corn; + frame->cloud_surf = cloud_surf; 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 8fd021a..c731496 100644 --- a/oh_my_loam/oh_my_loam.h +++ b/oh_my_loam/oh_my_loam.h @@ -22,8 +22,8 @@ class OhMyLoam { void Reset(); void Visualize(const common::Pose3d &pose_curr2map, - const TPointCloudPtr &cloud_corn, - const TPointCloudPtr &cloud_surf, double timestamp = 0.0); + const TPointCloudConstPtr &cloud_corn, + const TPointCloudConstPtr &cloud_surf, double timestamp = 0.0); // remove outliers: nan or very close points void RemoveOutliers(const common::PointCloud &cloud_in, diff --git a/oh_my_loam/visualizer/ohmyloam_visualizer.cc b/oh_my_loam/visualizer/ohmyloam_visualizer.cc index 0fb8087..5129dc4 100644 --- a/oh_my_loam/visualizer/ohmyloam_visualizer.cc +++ b/oh_my_loam/visualizer/ohmyloam_visualizer.cc @@ -1,5 +1,6 @@ #include "ohmyloam_visualizer.h" +#include #include "common/color/color.h" #include "common/pcl/pcl_utils.h" @@ -13,16 +14,55 @@ using namespace common; void OhmyloamVisualizer::Draw() { auto frame = GetCurrentFrame(); - DrawPointCloud(frame.cloud_map_corn, GRAY, "cloud_map_corn"); - DrawPointCloud(frame.cloud_map_surf, GRAY, "cloud_map_surf"); + DrawPointCloud(frame.cloud_map_corn, GRAY, "cloud_map_corn", 1); + DrawPointCloud(frame.cloud_map_surf, GRAY, "cloud_map_surf", 1); + TPointCloudPtr cloud_corn_trans(new TPointCloud); TransformPointCloud(frame.pose_map, *frame.cloud_corn, - frame.cloud_corn.get()); - DrawPointCloud(frame.cloud_corn, "time", "cloud_corn"); + cloud_corn_trans.get()); + DrawPointCloud(cloud_corn_trans, "time", "cloud_corn"); + TPointCloudPtr cloud_surf_trans(new TPointCloud); TransformPointCloud(frame.pose_map, *frame.cloud_surf, - frame.cloud_surf.get()); - DrawPointCloud(frame.cloud_surf, "time", "cloud_surf"); + cloud_surf_trans.get()); + DrawPointCloud(cloud_surf_trans, "time", "cloud_surf"); traj_.AddPose(frame.pose_map); AddTrajectory(traj_, PINK, "trajectory", 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() == "s" && event.keyDown()) { + auto frame = GetCurrentFrame(); + PointCloud cloud; + cloud.reserve(frame.cloud_map_surf->size() + frame.cloud_map_surf->size()); + for (const auto &p : *frame.cloud_map_corn) { + cloud.push_back({p.x, p.y, p.z}); + } + for (const auto p : *frame.cloud_map_surf) { + cloud.push_back({p.x, p.y, p.z}); + } + cloud.width = cloud.size(); + cloud.height = 1; + cloud.is_dense = false; + pcl::io::savePCDFileASCII(save_path_, cloud); + + } else if (event.getKeySym() == "r" && event.keyDown()) { + viewer_->setCameraPosition(0, 0, 200, 0, 0, 0, 1, 0, 0, 0); + viewer_->setSize(2500, 1500); + is_updated_ = true; + } +} + } // 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 8af3db4..fdd128d 100644 --- a/oh_my_loam/visualizer/ohmyloam_visualizer.h +++ b/oh_my_loam/visualizer/ohmyloam_visualizer.h @@ -9,21 +9,29 @@ namespace oh_my_loam { struct OhmyloamVisFrame : public common::LidarVisFrame { TPointCloudConstPtr cloud_map_corn; TPointCloudConstPtr cloud_map_surf; - TPointCloudPtr cloud_corn; // current - TPointCloudPtr cloud_surf; // current + TPointCloudConstPtr cloud_corn; // current + TPointCloudConstPtr cloud_surf; // current common::Pose3d pose_map; }; class OhmyloamVisualizer : public common::LidarVisualizer { public: - explicit OhmyloamVisualizer(const std::string &name = "OhmyloamVisualizer", + explicit OhmyloamVisualizer(const std::string &save_path, + const std::string &name = "OhmyloamVisualizer", size_t max_history_size = 10) - : common::LidarVisualizer(name, max_history_size) {} + : common::LidarVisualizer(name, max_history_size) { + save_path_ = save_path; + } private: void Draw() override; + void KeyboardEventCallback( + const pcl::visualization::KeyboardEvent &event) override; + common::Trajectory traj_; + + std::string save_path_; }; } // namespace oh_my_loam