mapper nice

common/geometry/trajectory.h

@@ -0,0 +1,64 @@
+#pragma once
+
+#include <memory>
+#include "common/geometry/pose3d.h"
+
+namespace common {
+
+class Trajectory {
+ public:
+  typedef std::shared_ptr<Trajectory> Ptr;
+  typedef std::shared_ptr<const Trajectory> ConstPtr;
+
+  Trajectory() = default;
+
+  explicit Trajectory(const std::vector<Pose3d> &poses) {
+    poses_ = poses;
+  }
+
+  size_t size() const {
+    return poses_.size();
+  }
+
+  bool empty() const {
+    return poses_.empty();
+  }
+
+  const Pose3d &at(size_t i) const {
+    return poses_.at(i);
+  }
+
+  void AddPose(const Pose3d &pose) {
+    poses_.push_back(pose);
+  }
+
+  Trajectory Copy(bool last_as_origin = false) const {
+    Trajectory traj;
+    if (empty()) return traj;
+    if (!last_as_origin) return *this;
+    const Pose3d &last_inv = poses_.back().Inv();
+    for (const auto &pose : poses_) {
+      traj.AddPose(last_inv * pose);
+    }
+    return traj;
+  }
+
+  std::vector<Eigen::Vector3d> GetPointSeq(bool last_as_origin = false) const {
+    std::vector<Eigen::Vector3d> point_seq;
+    if (empty()) return point_seq;
+    const Pose3d &last_inv = poses_.back().Inv();
+    for (const auto &pose : poses_) {
+      if (last_as_origin) {
+        point_seq.push_back((last_inv * pose).t_vec());
+      } else {
+        point_seq.push_back(pose.t_vec());
+      }
+    }
+    return point_seq;
+  };
+
+ private:
+  std::vector<Pose3d> poses_;
+};
+
+}  // namespace common

common/pcl/pcl_utils.h

@@ -98,7 +98,13 @@ void VoxelDownSample(const typename pcl::PointCloud<PT>::ConstPtr &cloud_in,
   pcl::VoxelGrid<PT> filter;
   filter.setInputCloud(cloud_in);
   filter.setLeafSize(voxel_size, voxel_size, voxel_size);
-  filter.filter(*cloud_out);
+  if (cloud_out == cloud_in.get()) {
+    pcl::PointCloud<PT> cloud_downsampled;
+    filter.filter(cloud_downsampled);
+    cloud_out->swap(cloud_downsampled);
+  } else {
+    filter.filter(*cloud_out);
+  }
 }
 
 }  // namespace common

common/visualizer/lidar_visualizer_utils.cc

@@ -1,3 +1,15 @@
 #include "lidar_visualizer_utils.h"
 
-namespace common {}  // namespace common
+namespace common {
+
+void AddTrajectory(const Trajectory &trajectory, const Color &color,
+                   const std::string &id, PCLVisualizer *const viewer) {
+  for (size_t i = 0; i < trajectory.size() - 1; ++i) {
+    Eigen::Vector3f p1 = trajectory.at(i).t_vec().cast<float>();
+    Eigen::Vector3f p2 = trajectory.at(i + 1).t_vec().cast<float>();
+    AddLine<Point>({p1.x(), p1.y(), p1.z()}, {p2.x(), p2.y(), p2.z()}, color,
+                   id + std::to_string(i), viewer);
+  }
+}
+
+}  // namespace common

common/visualizer/lidar_visualizer_utils.h

@@ -8,6 +8,7 @@
 #include <string>
 
 #include "common/color/color.h"
+#include "common/geometry/trajectory.h"
 #include "common/pcl/pcl_types.h"
 
 namespace common {
@@ -49,4 +50,7 @@ void AddSphere(const PT &center, double radius, const Color &color,
   viewer->addSphere(center, radius, color.r, color.g, color.b, id);
 }
 
+void AddTrajectory(const Trajectory &trajectory, const Color &color,
+                   const std::string &id, PCLVisualizer *const viewer);
+
 }  // namespace common

ggg.txt

@@ -0,0 +1,42 @@
+scan line number 16 
+line resolution 0.200000 plane resolution 0.400000 
+Mapping 10 Hz 
+... logging to /home/liufei/.ros/log/8dd723e0-69e0-11eb-931b-4cedfbbfbb36/roslaunch-liufei-fabu-4816.log
+Checking log directory for disk usage. This may take awhile.
+Press Ctrl-C to interrupt
+Done checking log file disk usage. Usage is <1GB.
+]2;/home/liufei/catkin_ws/src/A-LOAM/launch/aloam_velodyne_VLP_16.launch
+started roslaunch server http://liufei-fabu:38301/
+
+SUMMARY
+========
+
+PARAMETERS
+ * /mapping_line_resolution: 0.2
+ * /mapping_plane_resolution: 0.4
+ * /mapping_skip_frame: 1
+ * /minimum_range: 0.3
+ * /rosdistro: kinetic
+ * /rosversion: 1.12.16
+ * /scan_line: 16
+
+NODES
+  /
+    alaserMapping (aloam_velodyne/alaserMapping)
+    alaserOdometry (aloam_velodyne/alaserOdometry)
+    ascanRegistration (aloam_velodyne/ascanRegistration)
+    rviz (rviz/rviz)
+
+ROS_MASTER_URI=http://localhost:11311
+]2;/home/liufei/catkin_ws/src/A-LOAM/launch/aloam_velodyne_VLP_16.launch http://localhost:11311
+process[ascanRegistration-1]: started with pid [4854]
+process[alaserOdometry-2]: started with pid [4855]
+process[alaserMapping-3]: started with pid [4856]
+process[rviz-4]: started with pid [4857]
+[rviz-4] killing on exit
+[alaserMapping-3] killing on exit
+[alaserOdometry-2] killing on exit
+[ascanRegistration-1] killing on exit
+shutting down processing monitor...
+... shutting down processing monitor complete
+done

oh_my_loam/configs/config.yaml

@@ -2,7 +2,7 @@
 lidar: VPL16
 log_to_file: false
 log_path: /data/log/oh_my_loam
-vis: true
+vis: false
 
 # configs for extractor
 extractor_config:
@@ -31,7 +31,7 @@ odometer_config:
 
 # configs for mapper
 mapper_config:
-  vis: false
+  vis: true
   verbose: false
   map_shape: [3, 21, 21]
   map_step: 50

oh_my_loam/mapper/mapper.cc

@@ -26,7 +26,7 @@ bool Mapper::Init() {
   map_step_ = config_["map_step"].as<double>();
   corn_map_.reset(new Map(map_shape_, map_step_));
   surf_map_.reset(new Map(map_shape_, map_step_));
-  if (is_vis_) visualizer_.reset(MapperVisualizer);
+  if (is_vis_) visualizer_.reset(new MapperVisualizer);
   return true;
 }
 
@@ -64,15 +64,18 @@ void Mapper::Run(const TPointCloudConstPtr &cloud_corn,
   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_corn_map = corn_map_->GetSurrPoints(cnt,
-  TPointCloudPtr cloud_surf_map = surf_map_->GetSurrPoints(cnt, submap_shape_);
+  // submap_shape_); TPointCloudPtr cloud_surf_map =
-  kdtree_corn_.setInputCloud(cloud_corn_map);
+  // surf_map_->GetSurrPoints(cnt, submap_shape_);
-  kdtree_surf_.setInputCloud(cloud_surf_map);
+  kdtree_corn_.setInputCloud(cloud_corn_map_);
+  kdtree_surf_.setInputCloud(cloud_surf_map_);
+  std::vector<PointLineCoeffPair> pl_pairs;
+  std::vector<PointPlaneCoeffPair> pp_pairs;
   for (int i = 0; i < config_["icp_iter_num"].as<int>(); ++i) {
-    std::vector<PointLineCoeffPair> pl_pairs;
+    std::vector<PointLineCoeffPair>().swap(pl_pairs);
-    MatchCorn(cloud_corn_map, pose_curr2map, &pl_pairs);
+    std::vector<PointPlaneCoeffPair>().swap(pp_pairs);
-    std::vector<PointPlaneCoeffPair> pp_pairs;
+    MatchCorn(cloud_corn, pose_curr2map, &pl_pairs);
-    MatchSurf(cloud_surf_map, pose_curr2map, &pp_pairs);
+    MatchSurf(cloud_surf, pose_curr2map, &pp_pairs);
     AINFO_IF(verbose_) << "Iter " << i
                        << ": matched num: point2line = " << pl_pairs.size()
                        << ", point2plane = " << pp_pairs.size();
@@ -97,10 +100,11 @@ void Mapper::Run(const TPointCloudConstPtr &cloud_corn,
                        << 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();
   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
 }
 
 void Mapper::MatchCorn(const TPointCloudConstPtr &cloud_curr,
@@ -119,7 +123,7 @@ void Mapper::MatchCorn(const TPointCloudConstPtr &cloud_curr,
     }
     if (dists.back() > neighbor_point_dist_sq_th) continue;
     TPointCloud neighbor_pts;
-    pcl::copyPointCloud(*cloud_curr, indices, neighbor_pts);
+    pcl::copyPointCloud(*kdtree_corn_.getInputCloud(), indices, neighbor_pts);
     double fit_score = 0.0;
     LineCoeff coeff = common::FitLine3D(neighbor_pts, &fit_score);
     if (fit_score < config_["min_line_fit_score"].as<double>()) continue;
@@ -143,7 +147,7 @@ void Mapper::MatchSurf(const TPointCloudConstPtr &cloud_curr,
     }
     if (dists.back() > neighbor_point_dist_sq_th) continue;
     TPointCloud neighbor_pts;
-    pcl::copyPointCloud(*cloud_curr, indices, neighbor_pts);
+    pcl::copyPointCloud(*kdtree_surf_.getInputCloud(), indices, neighbor_pts);
     double fit_score = 0.0;
     Eigen::Vector4d coeff = common::FitPlane(neighbor_pts, &fit_score);
     if (fit_score < config_["min_plane_fit_score"].as<double>()) continue;
@@ -189,16 +193,20 @@ 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) {
+  auto update_map = [&](const TPointCloudConstPtr &cloud,
+                        /*Map*const map*/ const TPointCloudPtr &cloud_map) {
     TPointCloudPtr cloud_trans(new TPointCloud);
     common::TransformPointCloud(pose_curr2map, *cloud, cloud_trans.get());
-    std::vector<Index> indices;
+    *cloud_map += *cloud_trans;
-    map->AddPoints(cloud_trans, &indices);
+    // std::vector<Index> indices;
-    map->Downsample(indices, config_["downsample_voxel_size"].as<double>());
+    // map->AddPoints(cloud_trans, &indices);
+    // map->Downsample(indices, config_["downsample_voxel_size"].as<double>());
+    common::VoxelDownSample(cloud_map, cloud_map.get(),
+                            config_["downsample_voxel_size"].as<double>());
   };
   std::lock_guard<std::mutex> lock(map_mutex_);
-  update_map(cloud_corn, corn_map_.get());
+  update_map(cloud_corn, cloud_corn_map_);
-  update_map(cloud_surf, surf_map_.get());
+  update_map(cloud_surf, cloud_surf_map_);
 }
 
 TPointCloudPtr Mapper::GetMapCloudCorn() const {
@@ -237,9 +245,13 @@ void Mapper::Visualize(const std::vector<PointLineCoeffPair> &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_curr2world
+  frame->pose_curr2odom = pose_curr2odom;
+  frame->pose_curr2map = pose_curr2map;
+  visualizer_->Render(frame);
 }
 
 }  // namespace oh_my_loam

oh_my_loam/mapper/mapper.h

@@ -85,6 +85,8 @@ class Mapper {
   bool verbose_ = false;
 
   std::unique_ptr<MapperVisualizer> visualizer_{nullptr};
+  TPointCloudPtr cloud_corn_map_{new TPointCloud};
+  TPointCloudPtr cloud_surf_map_{new TPointCloud};
 
   DISALLOW_COPY_AND_ASSIGN(Mapper)
 };

oh_my_loam/oh_my_loam.cc

@@ -50,8 +50,8 @@ void OhMyLoam::Run(double timestamp,
   common::Pose3d pose_curr2odom;
   odometer_->Process(timestamp, features, &pose_curr2odom);
   common::Pose3d pose_curr2map;
-  const auto &cloud_corn = odometer_->GetCloudCorn();
+  const auto &cloud_corn = odometer_->GetCloudCorn()->makeShared();
-  const auto &cloud_surf = odometer_->GetCloudSurf();
+  const auto &cloud_surf = odometer_->GetCloudSurf()->makeShared();
   mapper_->Process(timestamp, cloud_corn, cloud_surf, pose_curr2odom,
                    &pose_curr2map);
   poses_curr2odom_.push_back(pose_curr2odom);

oh_my_loam/solver/cost_function.h

@@ -7,10 +7,6 @@
 
 namespace oh_my_loam {
 
-namespace {
-const double kEpsilon = 1.0e-6;
-}
-
 struct PointLinePair {
   TPoint pt;
   struct Line {
@@ -149,7 +145,7 @@ bool PointLineCostFunction::operator()(const T *const r_quat,
 
   Eigen::Quaternion<T> r(r_quat[3], r_quat[0], r_quat[1], r_quat[2]);
   Eigen::Matrix<T, 3, 1> t(t_vec[0], t_vec[1], t_vec[2]);
-  if (time_ <= 1.0 - kEpsilon) {
+  if (time_ <= 1.0 - 1.0e-6) {
     r = Eigen::Quaternion<T>::Identity().slerp(T(time_), r);
     t = T(time_) * t;
   }
@@ -177,7 +173,7 @@ bool PointPlaneCostFunction::operator()(const T *const r_quat,
 
   Eigen::Quaternion<T> r(r_quat[3], r_quat[0], r_quat[1], r_quat[2]);
   Eigen::Matrix<T, 3, 1> t(t_vec[0], t_vec[1], t_vec[2]);
-  if (time_ <= 1.0 - kEpsilon) {
+  if (time_ <= 1.0 - 1.0e-6) {
     r = Eigen::Quaternion<T>::Identity().slerp(T(time_), r);
     t = T(time_) * t;
   }
@@ -199,7 +195,7 @@ bool PointLineCoeffCostFunction::operator()(const T *const r_quat,
 
   Eigen::Quaternion<T> r(r_quat[3], r_quat[0], r_quat[1], r_quat[2]);
   Eigen::Matrix<T, 3, 1> t(t_vec[0], t_vec[1], t_vec[2]);
-  if (time_ <= 1.0 - kEpsilon) {
+  if (time_ <= 1.0 - 1.0e-6) {
     r = Eigen::Quaternion<T>::Identity().slerp(T(time_), r);
     t = T(time_) * t;
   }
@@ -221,7 +217,7 @@ bool PointPlaneCoeffCostFunction::operator()(const T *const r_quat,
 
   Eigen::Quaternion<T> r(r_quat[3], r_quat[0], r_quat[1], r_quat[2]);
   Eigen::Matrix<T, 3, 1> t(t_vec[0], t_vec[1], t_vec[2]);
-  if (time_ <= 1.0 - kEpsilon) {
+  if (time_ <= 1.0 - 1.0e-6) {
     r = Eigen::Quaternion<T>::Identity().slerp(T(time_), r);
     t = T(time_) * t;
   }

oh_my_loam/visualizer/mapper_visualizer.cc

@@ -13,78 +13,43 @@ void MapperVisualizer::Draw() {
   DrawPointCloud<TPoint>(frame.cloud_corn, GRAY, "cloud_corn");
   DrawPointCloud<TPoint>(frame.cloud_surf, GRAY, "cloud_surf");
 
-  DrawCorn(frame.pose_curr2odom, frame.pl_pairs);
+  DrawCorn(frame.pose_curr2odom, frame.pose_curr2map, frame.pl_pairs);
-  DrawSurf(frame.pose_curr2odom, frame.pp_pairs);
+  DrawSurf(frame.pose_curr2odom, frame.pose_curr2map, frame.pp_pairs);
 
-  poses_.push_back(frame.pose_curr2map);
+  traj_odom_.AddPose(frame.pose_curr2odom);
+  traj_map_.AddPose(frame.pose_curr2map);
   DrawTrajectory();
 }
 
-void MapperVisualizer::DrawCorn(const Pose3d &pose_curr2odom,
+void MapperVisualizer::DrawCorn(const common::Pose3d &pose_odom,
-                                const Pose3d &pose_curr2map,
+                                const common::Pose3d &pose_map,
                                 const std::vector<PointLineCoeffPair> &pairs) {
   TPointCloudPtr src(new TPointCloud);
-  TPointCloudPtr tgt(new TPointCloud);
   src->resize(pairs.size());
-  tgt->resize(pairs.size() * 2);
+  const auto &pose = trans_ ? pose_map : pose_odom;
   for (size_t i = 0; i < pairs.size(); ++i) {
     const auto &pair = pairs[i];
-    src->at(i) = pair.pt;
+    TransformToStart(pose, pair.pt, &src->at(i));
-    if (trans_) TransformToStart(pose, pair.pt, &src->points[i]);
   }
-  DrawPointCloud<TPoint>(tgt, YELLOW, "tgt_corn", 4);
   DrawPointCloud<TPoint>(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<TPoint>(p, p1, WHITE, "corn_line1_" + std::to_string(i),
-                            viewer_.get());
-    common::AddLine<TPoint>(p, p2, WHITE, "corn_line2_" + std::to_string(i),
-                            viewer_.get());
-  }
 }
 
-void MapperVisualizer::DrawSurf(const Pose3d &pose_curr2odom,
+void MapperVisualizer::DrawSurf(const common::Pose3d &pose_odom,
-                                const Pose3d &pose_curr2map,
+                                const common::Pose3d &pose_map,
                                 const std::vector<PointPlaneCoeffPair> &pairs) {
   TPointCloudPtr src(new TPointCloud);
-  TPointCloudPtr tgt(new TPointCloud);
   src->resize(pairs.size());
-  tgt->resize(pairs.size() * 3);
+  const auto &pose = trans_ ? pose_map : pose_odom;
   for (size_t i = 0; i < pairs.size(); ++i) {
     const auto &pair = pairs[i];
-    src->at(i) = pair.pt;
+    TransformToStart(pose, pair.pt, &src->at(i));
-    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<TPoint>(tgt, BLUE, "tgt_surf", 4);
   DrawPointCloud<TPoint>(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<TPoint>(p, p1, WHITE, "surf_line1_" + std::to_string(i),
-                    viewer_.get());
-    AddLine<TPoint>(p, p2, WHITE, "surf_line2_" + std::to_string(i),
-                    viewer_.get());
-    AddLine<TPoint>(p, p3, WHITE, "surf_line3_" + std::to_string(i),
-                    viewer_.get());
-  }
 }
 
 void MapperVisualizer::DrawTrajectory() {
-  std::vector<Pose3d> poses_n;
+  common::AddTrajectory(traj_odom_, PINK, "traj_odmo", viewer_.get());
-  poses_n.reserve((poses_.size()));
+  common::AddTrajectory(traj_map_, PURPLE, "traj_map", viewer_.get());
-  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<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 MapperVisualizer::KeyboardEventCallback(

oh_my_loam/visualizer/mapper_visualizer.h

@@ -1,13 +1,14 @@
 #pragma once
 
+#include "common/geometry/trajectory.h"
 #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;
+  TPointCloudConstPtr cloud_surf;
   std::vector<PointLineCoeffPair> pl_pairs;
   std::vector<PointPlaneCoeffPair> pp_pairs;
   common::Pose3d pose_curr2odom;
@@ -23,10 +24,10 @@ class MapperVisualizer : public common::LidarVisualizer {
  private:
   void Draw() override;
 
-  void DrawCorn(const common::Pose3d &pose,
+  void DrawCorn(const common::Pose3d &pose_odom, const common::Pose3d &pose_map,
                 const std::vector<PointLineCoeffPair> &pairs);
 
-  void DrawSurf(const common::Pose3d &pose,
+  void DrawSurf(const common::Pose3d &pose_odom, const common::Pose3d &pose_map,
                 const std::vector<PointPlaneCoeffPair> &pairs);
 
   void DrawTrajectory();
@@ -36,7 +37,8 @@ class MapperVisualizer : public common::LidarVisualizer {
 
   bool trans_ = true;
 
-  std::vector<common::Pose3d> poses_;
+  common::Trajectory traj_odom_;
+  common::Trajectory traj_map_;
 };
 
 }  // namespace oh_my_loam