no_convergence needs to be solved

common/config/yaml_config.h

@@ -29,7 +29,7 @@ class YAMLConfig {
 
   template <typename T>
   static const std::vector<T> GetSeq(const YAML::Node &node) {
-    ACHECK(node.IsSequence()) << "Not sequence.";
+    ACHECK(node.IsSequence());
     std::vector<T> seq;
     for (auto it = node.begin(); it != node.end(); ++it) {
       seq.push_back(it->as<T>());
@@ -39,7 +39,7 @@ class YAMLConfig {
 
   template <typename TK, typename TV>
   static const std::map<TK, TV> GetMap(const YAML::Node &node) {
-    ACHECK(node.IsMap()) << "Not sequence.";
+    ACHECK(node.IsMap());
     std::map<TK, TV> map;
     for (auto it = node.begin(); it != node.end(); ++it) {
       map.insert({it->first.as<TK>(), it->second.as<TV>()});

common/log/log.cc

@@ -5,9 +5,8 @@ namespace common {
 
 void InitG3Logging(bool log_to_file, const std::string &prefix,
                    const std::string &path) {
-  static std::unique_ptr<g3::LogWorker> worker;
+  static std::unique_ptr<g3::LogWorker> worker(
-  if (worker != nullptr) return;
+      g3::LogWorker::createLogWorker());
-  worker = std::move(g3::LogWorker::createLogWorker());
   worker->addSink(std::make_unique<g3::CustomSink>(),
                   &g3::CustomSink::StdLogMessage);
   if (log_to_file) {
@@ -27,3 +26,51 @@ void InitG3Logging(bool log_to_file, const std::string &prefix,
 }
 
 }  // namespace common
+namespace g3 {
+
+CustomSink::~CustomSink() {
+  std::ostringstream oss;
+  oss << "\ng3log " << (log_to_file_ ? "FileSink" : "StdSink")
+      << " shutdown at: ";
+  auto now = std::chrono::system_clock::now();
+  oss << g3::localtime_formatted(now, "%Y%m%d %H:%M:%S.%f3");
+  if (log_to_file_) {
+    (*ofs_) << oss.str() << std::endl;
+  } else {
+    std::clog << oss.str() << std::endl;
+  }
+};
+
+std::string CustomSink::FormatedMessage(const g3::LogMessage &msg) const {
+  std::ostringstream oss;
+  oss << "[" << msg.level()[0] << msg.timestamp("%Y%m%d %H:%M:%S.%f3") << " "
+      << msg.file() << ":" << msg.line() << "] " << msg.message();
+  return oss.str();
+}
+
+std::string CustomSink::ColorFormatedMessage(const g3::LogMessage &msg) const {
+  std::ostringstream oss;
+  oss << GetColorCode(msg._level) << FormatedMessage(msg) << "\033[m";
+  return oss.str();
+}
+
+std::string CustomSink::GetColorCode(const LEVELS &level) const {
+  if (level.value == WARNING.value) {
+    return "\033[33m";  // yellow
+  }
+  if (level.value == DEBUG.value) {
+    return "\033[32m";  // green
+  }
+  if (level.value == ERROR.value) {
+    return "\033[31m";  // red
+  }
+  if (level.value == USER.value) {
+    return "\033[1m\033[34m";  // bold blue
+  }
+  if (g3::internal::wasFatal(level)) {
+    return "\033[1m\033[31m";  // red
+  }
+  return "\033[97m";  // white
+}
+
+}  // namespace g3

common/log/log.h

@@ -8,25 +8,31 @@
 #include <iostream>
 
 // 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
+// conflicts LOG macro in glog.
 #define G3LOG(level) \
-  if (!g3::logLevel(level)) { \
+  if (g3::logLevel(level)) INTERNAL_LOG_MESSAGE(level).stream()
-  } else                      \
+
-  INTERNAL_LOG_MESSAGE(level).stream()
+#define VARIABLE_WITH_LINE_NUM(var_name, line) var_name##line
+#define COUNT_VAR_WITH_LINE_NUM VARIABLE_WITH_LINE_NUM(_count_var_, __LINE__)
+
+#define G3LOG_EVERY(level, n)                                      \
+  static size_t COUNT_VAR_WITH_LINE_NUM = 0;                       \
+  if ((COUNT_VAR_WITH_LINE_NUM)++ % static_cast<size_t>(n) == 0 && \
+      g3::logLevel(level))                                         \
+    INTERNAL_LOG_MESSAGE(level).stream();
 
 #define G3LOG_IF(level, boolean_expression)        \
-  if (false == (boolean_expression) || !g3::logLevel(level)) { \
+  if ((boolean_expression) && g3::logLevel(level)) \
-  } else                                                       \
   INTERNAL_LOG_MESSAGE(level).stream()
 
 #define G3CHECK(boolean_expression) \
-  if (true == (boolean_expression)) { \
+  if (!(boolean_expression))        \
-  } else                              \
   INTERNAL_CONTRACT_MESSAGE(#boolean_expression).stream()
 
 const LEVELS ERROR{WARNING.value + 100, "ERROR"};
 const LEVELS USER(ERROR.value + 100, "USER");
 
+// LOG
 #define ADEBUG G3LOG(DEBUG)
 #define AINFO G3LOG(INFO)
 #define AWARN G3LOG(WARNING)
@@ -43,6 +49,13 @@ const LEVELS USER(ERROR.value + 100, "USER");
 #define AFATAL_IF(cond) G3LOG_IF(FATAL, cond)
 #define ACHECK(cond) G3CHECK(cond)
 
+// LOG_EVERY
+#define ADEBUG_EVERY(n) G3LOG_EVERY(DEBUG, n)
+#define AINFO_EVERY(n) G3LOG_EVERY(INFO, n)
+#define AWARN_EVERY(n) G3LOG_EVERY(WARNING, n)
+#define AERROR_EVERY(n) G3LOG_EVERY(ERROR, n)
+#define AUSER_EVERY(n) G3LOG_EVERY(USER, n)
+
 namespace common {
 void InitG3Logging(bool log_to_file = false, const std::string &prefix = "",
                    const std::string &path = "./");
@@ -58,26 +71,15 @@ class CustomSink {
     ofs_.reset(new std::ofstream(log_file_name));
   }
 
-  ~CustomSink() {
+  ~CustomSink();
-    std::ostringstream oss;
-    oss << "\ng3log " << (log_to_file_ ? "FileSink" : "StdSink")
-        << " shutdown at: ";
-    auto now = std::chrono::system_clock::now();
-    oss << g3::localtime_formatted(now, "%Y%m%d %H:%M:%S.%f3");
-    if (log_to_file_) {
-      (*ofs_) << oss.str() << std::endl;
-    } else {
-      std::clog << oss.str() << std::endl;
-    }
-  };
 
-  void StdLogMessage(g3::LogMessageMover logEntry) {
+  void StdLogMessage(g3::LogMessageMover log_entry) {
-    std::clog << ColorFormatedMessage(logEntry.get()) << std::endl;
+    std::clog << ColorFormatedMessage(log_entry.get()) << std::endl;
   }
 
-  void FileLogMessage(g3::LogMessageMover logEntry) {
+  void FileLogMessage(g3::LogMessageMover log_entry) {
     if (log_to_file_) {
-      (*ofs_) << FormatedMessage(logEntry.get()) << std::endl;
+      (*ofs_) << FormatedMessage(log_entry.get()) << std::endl;
     }
   }
 
@@ -86,37 +88,11 @@ class CustomSink {
   bool log_to_file_{false};
   std::unique_ptr<std::ofstream> ofs_{nullptr};
 
-  std::string FormatedMessage(const g3::LogMessage &msg) const {
+  std::string FormatedMessage(const g3::LogMessage &msg) const;
-    std::ostringstream oss;
-    oss << "[" << msg.level()[0] << msg.timestamp("%Y%m%d %H:%M:%S.%f3") << " "
-        << msg.file() << ":" << msg.line() << "] " << msg.message();
-    return oss.str();
-  }
 
-  std::string ColorFormatedMessage(const g3::LogMessage &msg) const {
+  std::string ColorFormatedMessage(const g3::LogMessage &msg) const;
-    std::ostringstream oss;
-    oss << GetColorCode(msg._level) << FormatedMessage(msg) << "\033[m";
-    return oss.str();
-  }
 
-  std::string GetColorCode(const LEVELS &level) const {
+  std::string GetColorCode(const LEVELS &level) const;
-    if (level.value == WARNING.value) {
-      return "\033[33m";  // yellow
-    }
-    if (level.value == DEBUG.value) {
-      return "\033[32m";  // green
-    }
-    if (level.value == ERROR.value) {
-      return "\033[31m";  // red
-    }
-    if (level.value == USER.value) {
-      return "\033[1m\033[34m";  // bold blue
-    }
-    if (g3::internal::wasFatal(level)) {
-      return "\033[1m\033[31m";  // red
-    }
-    return "\033[97m";  // white
-  }
 };
 
 }  // namespace g3

common/math/fitting.h

@@ -15,7 +15,7 @@ Eigen::Vector3d FitLine2D(const pcl::PointCloud<PT> &cloud,
   for (const auto &p : cloud) data.row(i++) << p.x, p.y;
   Eigen::RowVector2f centroid = data.colwise().mean();
   Eigen::MatrixX2f data_centered = data.rowwise() - centroid;
-  Eigen::JacobiSVD<Eigen::MatrixX2f> svd(data_centered, Eigen::ComputeThinV);
+  Eigen::JacobiSVD<Eigen::MatrixX2f> svd(data_centered, Eigen::ComputeFullV);
   Eigen::Vector2f normal = svd.matrixV().col(1);
   float c = -centroid * normal;
   if (score) *score = svd.singularValues()[0] / svd.singularValues()[1];
@@ -34,7 +34,7 @@ Eigen::Matrix<double, 6, 1> FitLine3D(const pcl::PointCloud<PT> &cloud,
   for (const auto &p : cloud) data.row(i++) << p.x, p.y, p.z;
   Eigen::RowVector3f centroid = data.colwise().mean();
   Eigen::MatrixX3f data_centered = data.rowwise() - centroid;
-  Eigen::JacobiSVD<Eigen::MatrixX3f> svd(data_centered, Eigen::ComputeThinV);
+  Eigen::JacobiSVD<Eigen::MatrixX3f> svd(data_centered, Eigen::ComputeFullV);
   Eigen::Vector3f direction = svd.matrixV().col(0);
   Eigen::Matrix<double, 6, 1> line_coeffs;
   line_coeffs.topRows(3) = centroid.transpose().cast<double>();
@@ -55,7 +55,7 @@ Eigen::Vector4d FitPlane(const pcl::PointCloud<PT> &cloud,
   for (const auto &p : cloud) data.row(i++) << p.x, p.y, p.z;
   Eigen::RowVector3f centroid = data.colwise().mean();
   Eigen::MatrixX3f data_centered = data.rowwise() - centroid;
-  Eigen::JacobiSVD<Eigen::MatrixX3f> svd(data_centered, Eigen::ComputeThinV);
+  Eigen::JacobiSVD<Eigen::MatrixX3f> svd(data_centered, Eigen::ComputeFullV);
   Eigen::Vector3f normal = svd.matrixV().col(2);
   float d = -centroid * normal;
   if (score) {

main.cc

@@ -21,17 +21,14 @@ int main(int argc, char *argv[]) {
   bool log_to_file = YAMLConfig::Instance()->Get<bool>("log_to_file");
   std::string log_path = YAMLConfig::Instance()->Get<std::string>("log_path");
   std::string lidar = YAMLConfig::Instance()->Get<std::string>("lidar");
-
   // logging
   InitG3Logging(log_to_file, "oh_my_loam_" + lidar, log_path);
   AUSER << "LOAM start..., lidar = " << lidar;
-
   // SLAM system
   OhMyLoam slam;
   if (!slam.Init()) {
     AFATAL << "Failed to initilize slam system.";
   }
-
   // ros
   ros::init(argc, argv, "oh_my_loam");
   ros::NodeHandle nh;

oh_my_loam/configs/config.yaml

@@ -2,11 +2,11 @@
 lidar: VPL16
 log_to_file: false
 log_path: /data/log/oh_my_loam
-vis: true
+vis: false
 
 # configs for extractor
 extractor_config:
-  vis: false
+  vis: true
   verbose: false
   min_point_num: 66
   scan_seg_num: 12
@@ -20,11 +20,11 @@ extractor_config:
 
 # configs for odometer
 odometer_config:
-  vis: true
+  vis: false
-  verbose: false
+  verbose: true
   nearby_scan_num: 1
   min_correspondence_num: 10
-  icp_iter_num: 2
+  icp_iter_num: 4
   solve_iter_num: 4
   corn_match_dist_sq_th: 16.0
   surf_match_dist_sq_th: 16.0
@@ -34,9 +34,13 @@ mapper_config:
   vis: false
   verbose: false
   map_shape: [3, 21, 21]
-  map_step: 50 # meter
+  map_step: 50
   submap_shape: [1, 5, 5]
+  icp_iter_num: 2
+  solve_iter_num: 4
+  min_correspondence_num: 10
   nearest_neighbor_k: 5
   neighbor_point_dist_sq_th: 1.0
   min_line_fit_score: 2
   min_plane_fit_score: 2
+  downsample_voxel_size: 0.6

oh_my_loam/extractor/extractor.cc

@@ -14,7 +14,7 @@ const double kEps = 1e-6;
 bool Extractor::Init() {
   const auto &config = common::YAMLConfig::Instance()->config();
   config_ = config["extractor_config"];
-  is_vis_ = config["vis"].as<bool>() && config_["vis"].as<bool>();
+  is_vis_ = config_["vis"].as<bool>();
   verbose_ = config_["verbose"].as<bool>();
   AINFO << "Extraction visualizer: " << (is_vis_ ? "ON" : "OFF");
   if (is_vis_) visualizer_.reset(new ExtractorVisualizer);

oh_my_loam/mapper/map.cc

@@ -108,6 +108,13 @@ 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];
+  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<int> &surr_shapes) const {
   TPointCloudPtr cloud_all(new TPointCloud);
@@ -128,6 +135,7 @@ void Map::AddPoints(const TPointCloudConstPtr &cloud,
   std::set<Index, Index::Comp> index_set;
   for (const auto &point : *cloud) {
     Index index = GetIndex(point);
+    if (!IsIndexValid(index)) continue;
     this->at(index)->push_back(point);
     if (indices) index_set.insert(index);
   }

oh_my_loam/mapper/map.h

@@ -48,6 +48,8 @@ class Map {
 
   Index GetIndex(const TPoint &point) const;
 
+  bool IsIndexValid(const Index &index) const;
+
   TPointCloudPtr GetSurrPoints(const TPoint &point,
                                const std::vector<int> &surr_shapes) const;
 

oh_my_loam/mapper/mapper.cc

@@ -36,11 +36,11 @@ void Mapper::Process(double timestamp, const TPointCloudConstPtr &cloud_corn,
                      const common::Pose3d &pose_curr2odom,
                      common::Pose3d *const pose_curr2map) {
   if (GetState() == UN_INIT) {
-    corn_map_->AddPoints(cloud_corn);
-    surf_map_->AddPoints(cloud_surf);
     pose_curr2map->SetIdentity();
     pose_odom2map_.SetIdentity();
+    UpdateMap(*pose_curr2map, cloud_corn, cloud_surf);
     SetState(DONE);
+    AINFO << "Mapper initialized...";
     return;
   }
   if (GetState() == DONE) {
@@ -55,6 +55,7 @@ void Mapper::Process(double timestamp, const TPointCloudConstPtr &cloud_corn,
 void Mapper::Run(const TPointCloudConstPtr &cloud_corn,
                  const TPointCloudConstPtr &cloud_surf,
                  const common::Pose3d &pose_curr2odom) {
+  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(),
@@ -62,14 +63,24 @@ void Mapper::Run(const TPointCloudConstPtr &cloud_corn,
   AdjustMap(cnt);
   TPointCloudPtr cloud_corn_map = corn_map_->GetSurrPoints(cnt, submap_shape_);
   TPointCloudPtr cloud_surf_map = corn_map_->GetSurrPoints(cnt, submap_shape_);
+  for (int i = 0; i < config_["icp_iter_num"].as<int>(); ++i) {
     pcl::KdTreeFLANN<TPoint> kdtree_corn;
     kdtree_corn.setInputCloud(cloud_corn_map);
     pcl::KdTreeFLANN<TPoint> kdtree_surf;
     kdtree_surf.setInputCloud(cloud_surf_map);
     std::vector<PointLinePair> pl_pairs;
-  std::vector<PointPlaneCoeffPair> pp_pairs;
     MatchCorn(kdtree_corn, cloud_corn_map, pose_curr2map, &pl_pairs);
+    std::vector<PointPlaneCoeffPair> pp_pairs;
     MatchSurf(kdtree_surf, cloud_surf_map, pose_curr2map, &pp_pairs);
+    AINFO_IF(verbose_) << "Iter " << i
+                       << ": matched num: point2line = " << pl_pairs.size()
+                       << ", point2plane = " << pp_pairs.size();
+    if (pl_pairs.size() + pp_pairs.size() <
+        config_["min_correspondence_num"].as<size_t>()) {
+      AWARN << "Too less correspondence: " << pl_pairs.size() << " + "
+            << pp_pairs.size();
+      continue;
+    }
     PoseSolver solver(pose_curr2map);
     for (const auto &pair : pl_pairs) {
       solver.AddPointLinePair(pair, 0.0);
@@ -77,12 +88,18 @@ void Mapper::Run(const TPointCloudConstPtr &cloud_corn,
     for (const auto &pair : pp_pairs) {
       solver.AddPointPlaneCoeffPair(pair, 0.0);
     }
-  if (!solver.Solve(5, false, &pose_curr2map)) {
+    if (!solver.Solve(config_["solve_iter_num"].as<int>(), verbose_,
-    AWARN << "Solve: no_convergence";
+                      &pose_curr2map)) {
+      AWARN << "Mapping solve: no_convergence";
     }
+    AINFO_IF(verbose_) << "Odometer::ICP: iter_" << i << ": "
+                       << BLOCK_TIMER_STOP_FMT;
+  }
+  UpdateMap(pose_curr2map, cloud_corn, cloud_surf);
+  SetState(DONE);  // be careful with deadlock
   std::lock_guard<std::mutex> lock(state_mutex_);
   pose_odom2map_ = pose_curr2map * pose_curr2odom.Inv();
-  SetState(DONE);
+  AUSER << "Mapper::Run: " << BLOCK_TIMER_STOP_FMT;
 }
 
 void Mapper::MatchCorn(const pcl::KdTreeFLANN<TPoint> &kdtree,
@@ -173,18 +190,36 @@ void Mapper::AdjustMap(const TPoint &center) {
   }
 }
 
+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) {
+    TPointCloudPtr cloud_trans(new TPointCloud);
+    common::TransformPointCloud(pose_curr2map, *cloud, cloud_trans.get());
+    std::vector<Index> indices;
+    map->AddPoints(cloud_trans, &indices);
+    map->Downsample(indices, config_["downsample_voxel_size"].as<double>());
+  };
+  std::lock_guard<std::mutex> lock(map_mutex_);
+  update_map(cloud_corn, corn_map_.get());
+  update_map(cloud_surf, surf_map_.get());
+}
+
 TPointCloudPtr Mapper::GetMapCloudCorn() const {
+  std::lock_guard<std::mutex> lock(map_mutex_);
   return corn_map_->GetAllPoints();
 }
 
 TPointCloudPtr Mapper::GetMapCloudSurf() const {
+  std::lock_guard<std::mutex> lock(map_mutex_);
   return surf_map_->GetAllPoints();
 }
 
 TPointCloudPtr Mapper::GetMapCloud() const {
   TPointCloudPtr map_points(new TPointCloud);
-  *map_points += *GetMapCloudCorn();
+  std::lock_guard<std::mutex> lock(map_mutex_);
-  *map_points += *GetMapCloudSurf();
+  *map_points += *corn_map_->GetAllPoints();
+  *map_points += *surf_map_->GetAllPoints();
   return map_points;
 }
 

oh_my_loam/mapper/mapper.h

@@ -56,6 +56,10 @@ class Mapper {
 
   void AdjustMap(const TPoint &center);
 
+  void UpdateMap(const common::Pose3d &pose_curr2map,
+                 const TPointCloudConstPtr &cloud_corn = nullptr,
+                 const TPointCloudConstPtr &cloud_surf = nullptr);
+
   void MatchCorn(const TPointCloudConstPtr &src,
                  const TCTPointCloudConstPtr &tgt,
                  std::vector<PointLinePair> *const pairs) const;
@@ -63,22 +67,21 @@ class Mapper {
   void MatchSurf(const TPointCloudConstPtr &src,
                  const TCTPointCloudConstPtr &tgt,
                  std::vector<PointLinePair> *const pairs) const;
-
+  // map
-  YAML::Node config_;
+  mutable std::mutex map_mutex_;
-
-  std::vector<int> map_shape_, submap_shape_;
-  double map_step_;
   std::unique_ptr<Map> corn_map_;
   std::unique_ptr<Map> surf_map_;
-
+  // state
   mutable std::mutex state_mutex_;
   State state_ = UN_INIT;
   common::Pose3d pose_odom2map_;
-
+  // thread to run mapping
   mutable std::unique_ptr<std::thread> thread_{nullptr};
-
+  // configs
+  YAML::Node config_;
+  std::vector<int> map_shape_, submap_shape_;
+  double map_step_;
   bool is_vis_ = false;
-
   bool verbose_ = false;
 
   DISALLOW_COPY_AND_ASSIGN(Mapper)

oh_my_loam/odometer/odometer.cc

@@ -11,7 +11,7 @@ namespace oh_my_loam {
 bool Odometer::Init() {
   const auto &config = common::YAMLConfig::Instance()->config();
   config_ = config["odometer_config"];
-  is_vis_ = config["vis"].as<bool>() && config_["vis"].as<bool>();
+  is_vis_ = config_["vis"].as<bool>();
   verbose_ = config_["verbose"].as<bool>();
   AINFO << "Odometry visualizer: " << (is_vis_ ? "ON" : "OFF");
   if (is_vis_) visualizer_.reset(new OdometerVisualizer);
@@ -30,7 +30,7 @@ void Odometer::Process(double timestamp, const std::vector<Feature> &features,
     pose_curr2last_.SetIdentity();
     pose_curr2world_.SetIdentity();
     pose_out->SetIdentity();
-    AINFO << "Odometer initialized....";
+    AINFO << "Odometer initialized...";
     return;
   }
   BLOCK_TIMER_START;
@@ -61,12 +61,12 @@ void Odometer::Process(double timestamp, const std::vector<Feature> &features,
     }
     bool is_converge = solver.Solve(config_["solve_iter_num"].as<int>(),
                                     verbose_, &pose_curr2last_);
-    AWARN_IF(!is_converge) << "Solve: no_convergence";
+    AWARN_IF(!is_converge) << "Odometry solve: no_convergence";
     AINFO_IF(verbose_) << "Odometer::ICP: iter_" << i << ": "
                        << BLOCK_TIMER_STOP_FMT;
   }
-  *pose_out = *pose_out * pose_curr2last_;
+  pose_curr2world_ = pose_curr2world_ * pose_curr2last_;
-  pose_curr2world_ = *pose_out;
+  *pose_out = pose_curr2world_;
   AINFO_IF(verbose_) << "Pose increase: " << pose_curr2last_.ToString();
   // mush called before calling UpdatePre
   if (is_vis_) Visualize(pl_pairs, pp_pairs);

oh_my_loam/oh_my_loam.cc

@@ -9,11 +9,12 @@
 namespace oh_my_loam {
 
 namespace {
-const double kPointMinDist = 0.1;
+const double kPointMinDist = 0.5;
 }  // namespace
 
 bool OhMyLoam::Init() {
   config_ = common::YAMLConfig::Instance()->config();
+  is_vis_ = config_["vis"].as<bool>();
   extractor_.reset(new ExtractorVLP16);
   if (!extractor_->Init()) {
     AERROR << "Failed to initialize extractor";
@@ -29,6 +30,7 @@ bool OhMyLoam::Init() {
     AERROR << "Failed to initialize mapper";
     return false;
   }
+  if (is_vis_) visualizer_.reset(new OhmyloamVisualizer);
   return true;
 }
 
@@ -53,11 +55,27 @@ void OhMyLoam::Run(double timestamp,
   mapper_->Process(timestamp, cloud_corn, cloud_surf, pose_curr2odom,
                    &pose_curr2map);
   poses_curr2odom_.push_back(pose_curr2odom);
-  poses_curr2world_.push_back(pose_curr2map);
+  poses_curr2map_.push_back(pose_curr2map);
-  if (is_vis_) Visualize(timestamp);
+  if (is_vis_) {
+    Visualize(pose_curr2odom, pose_curr2map, cloud_corn, cloud_surf, timestamp);
+  }
 }
 
-void OhMyLoam::Visualize(double timestamp) {}
+void OhMyLoam::Visualize(const common::Pose3d &pose_curr2odom,
+                         const common::Pose3d &pose_curr2map,
+                         const TPointCloudConstPtr &cloud_corn,
+                         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;
+  frame->cloud_surf = cloud_surf;
+  frame->pose_odom = pose_curr2odom;
+  frame->pose_map = pose_curr2map;
+  visualizer_->Render(frame);
+}
 
 void OhMyLoam::RemoveOutliers(const common::PointCloud &cloud_in,
                               common::PointCloud *const cloud_out) const {

oh_my_loam/oh_my_loam.h

@@ -6,6 +6,7 @@
 #include "oh_my_loam/extractor/extractor.h"
 #include "oh_my_loam/mapper/mapper.h"
 #include "oh_my_loam/odometer/odometer.h"
+#include "oh_my_loam/visualizer/ohmyloam_visualizer.h"
 
 namespace oh_my_loam {
 
@@ -20,7 +21,10 @@ class OhMyLoam {
  private:
   void Reset();
 
-  void Visualize(double timestamp = 0.0);
+  void Visualize(const common::Pose3d &pose_curr2odom,
+                 const common::Pose3d &pose_curr2map,
+                 const TPointCloudConstPtr &cloud_corn,
+                 const TPointCloudConstPtr &cloud_surf, double timestamp = 0.0);
 
   std::unique_ptr<Extractor> extractor_{nullptr};
 
@@ -33,11 +37,13 @@ class OhMyLoam {
                       common::PointCloud *const cloud_out) const;
 
   std::vector<common::Pose3d> poses_curr2odom_;
-  std::vector<common::Pose3d> poses_curr2world_;
+  std::vector<common::Pose3d> poses_curr2map_;
 
   YAML::Node config_;
   bool is_vis_ = false;
 
+  std::unique_ptr<OhmyloamVisualizer> visualizer_{nullptr};
+
   DISALLOW_COPY_AND_ASSIGN(OhMyLoam);
 };
 

oh_my_loam/visualizer/odometer_visualizer.h

@@ -38,7 +38,7 @@ class OdometerVisualizer : public common::LidarVisualizer {
   bool corn_connect_ = false;
   bool surf_connect_ = false;
 
-  std::deque<common::Pose3d> poses_;
+  std::vector<common::Pose3d> poses_;
 };
 
 }  // namespace oh_my_loam

oh_my_loam/visualizer/ohmyloam_visualizer.cc

@@ -1,38 +1,73 @@
 #include "ohmyloam_visualizer.h"
 
 #include "common/color/color.h"
+#include "common/pcl/pcl_utils.h"
 
 namespace oh_my_loam {
 
 namespace {
 using namespace common;
-#define LIGHT_BLUE common::Color(0, 0, 80)
+#define LIGHT_BLUE common::Color(0, 0, 150)
-#define LIGHT_YELLOW common::Color(80, 80, 0)
+#define LIGHT_YELLOW common::Color(150, 150, 0)
 }  // namespace
 
 void OhmyloamVisualizer::Draw() {
   auto frame = GetCurrentFrame<OhmyloamVisFrame>();
-  DrawPointCloud<TPoint>(frame.cloud_map_corn, LIGHT_YELLOW, "cloud_map_corn");
+  DrawPointCloud<TPoint>(frame.cloud_map_corn, GRAY, "cloud_map_corn");
-  DrawPointCloud<TPoint>(frame.cloud_map_surf, LIGHT_BLUE, "cloud_map_surf");
+  DrawPointCloud<TPoint>(frame.cloud_map_surf, GRAY, "cloud_map_surf");
-
+  if (!trans_) {
     DrawPointCloud<TPoint>(frame.cloud_corn, RED, "cloud_corn");
     DrawPointCloud<TPoint>(frame.cloud_surf, CYAN, "cloud_surf");
-
+  } else {
-  DrawTrajectory(frame.poses);
+    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<TPoint>(cloud_corn_trans, RED, "cloud_corn");
+    DrawPointCloud<TPoint>(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(std::vector<common::Pose3d> &poses) {
+void OhmyloamVisualizer::DrawTrajectory(
-  std::vector<Pose3d> poses_n;
+    const std::vector<common::Pose3d> &poses, const std::string &id,
-  poses_n.reserve((poses.size()));
+    const common::Color &color) {
-  Pose3d pose_inv = poses.back().Inv();
+  for (size_t i = 0; i < poses.size() - 1; ++i) {
-  for (const auto &pose : poses) {
+    Eigen::Vector3f p1 = poses[i].t_vec().cast<float>();
-    poses_n.emplace_back(pose_inv * pose);
+    Eigen::Vector3f p2 = poses[i + 1].t_vec().cast<float>();
-  };
+    AddLine<Point>({p1.x(), p1.y(), p1.z()}, {p2.x(), p2.y(), p2.z()}, color,
-  for (size_t i = 0; i < poses_n.size() - 1; ++i) {
+                   id + std::to_string(i), viewer_.get());
-    Eigen::Vector3f p1 = poses_n[i].t_vec().cast<float>();
+  }
-    Eigen::Vector3f p2 = poses_n[i + 1].t_vec().cast<float>();
+}
-    AddLine<Point>({p1.x(), p1.y(), p1.z()}, {p2.x(), p2.y(), p2.z()}, PINK,
+
-                   "trajectory" + std::to_string(i), 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() == "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;
   }
 }
 

oh_my_loam/visualizer/ohmyloam_visualizer.h

@@ -11,7 +11,8 @@ struct OhmyloamVisFrame : public common::LidarVisFrame {
   TPointCloudConstPtr cloud_map_surf;
   TPointCloudConstPtr cloud_corn;  // current
   TPointCloudConstPtr cloud_surf;  // current
-  std::vector<common::Pose3d> poses;
+  common::Pose3d pose_odom;
+  common::Pose3d pose_map;
 };
 
 class OhmyloamVisualizer : public common::LidarVisualizer {
@@ -23,7 +24,15 @@ class OhmyloamVisualizer : public common::LidarVisualizer {
  private:
   void Draw() override;
 
-  void DrawTrajectory(std::vector<common::Pose3d> &poses);
+  void DrawTrajectory(const std::vector<common::Pose3d> &poses,
+                      const std::string &id, const common::Color &color);
+
+  void KeyboardEventCallback(
+      const pcl::visualization::KeyboardEvent &event) override;
+
+  std::vector<common::Pose3d> poses_odom_;
+  std::vector<common::Pose3d> poses_map_;
+  bool trans_ = true;
 };
 
 }  // namespace oh_my_loam