fix bug in ohmyloamvisualizer

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

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<std::string>()));
+  }
   return true;
 }
 
@@ -60,14 +63,15 @@ void OhMyLoam::Run(double timestamp,
 }
 
 void OhMyLoam::Visualize(const common::Pose3d &pose_curr2map,
-                         const TPointCloudPtr &cloud_corn,
+                         const TPointCloudConstPtr &cloud_corn,
-                         const TPointCloudPtr &cloud_surf, double timestamp) {
+                         const TPointCloudConstPtr &cloud_surf,
+                         double timestamp) {
   std::shared_ptr<OhmyloamVisFrame> 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_corn = cloud_corn;
-  frame->cloud_surf = cloud_surf->makeShared();
+  frame->cloud_surf = cloud_surf;
   frame->pose_map = pose_curr2map;
   visualizer_->Render(frame);
 }

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 TPointCloudConstPtr &cloud_corn,
-                 const TPointCloudPtr &cloud_surf, double timestamp = 0.0);
+                 const TPointCloudConstPtr &cloud_surf, double timestamp = 0.0);
 
   // remove outliers: nan or very close points
   void RemoveOutliers(const common::PointCloud &cloud_in,

oh_my_loam/visualizer/ohmyloam_visualizer.cc

@@ -1,5 +1,6 @@
 #include "ohmyloam_visualizer.h"
 
+#include <pcl/io/pcd_io.h>
 #include "common/color/color.h"
 #include "common/pcl/pcl_utils.h"
 
@@ -13,16 +14,55 @@ using namespace common;
 
 void OhmyloamVisualizer::Draw() {
   auto frame = GetCurrentFrame<OhmyloamVisFrame>();
-  DrawPointCloud<TPoint>(frame.cloud_map_corn, GRAY, "cloud_map_corn");
+  DrawPointCloud<TPoint>(frame.cloud_map_corn, GRAY, "cloud_map_corn", 1);
-  DrawPointCloud<TPoint>(frame.cloud_map_surf, GRAY, "cloud_map_surf");
+  DrawPointCloud<TPoint>(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());
+                      cloud_corn_trans.get());
-  DrawPointCloud<TPoint>(frame.cloud_corn, "time", "cloud_corn");
+  DrawPointCloud<TPoint>(cloud_corn_trans, "time", "cloud_corn");
+  TPointCloudPtr cloud_surf_trans(new TPointCloud);
   TransformPointCloud(frame.pose_map, *frame.cloud_surf,
-                      frame.cloud_surf.get());
+                      cloud_surf_trans.get());
-  DrawPointCloud<TPoint>(frame.cloud_surf, "time", "cloud_surf");
+  DrawPointCloud<TPoint>(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<std::mutex> 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<std::mutex> 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<OhmyloamVisFrame>();
+    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

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
+  TPointCloudConstPtr cloud_corn;  // current
-  TPointCloudPtr cloud_surf;  // 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