odometer ok, a nan bug needs to be located

common/geometry/pose3d.h

@@ -29,7 +29,7 @@ class Pose3d {
   Pose3d Inv() const {
     Eigen::Quaterniond r_inv = r_quat_.inverse();
     Eigen::Vector3d t_inv = r_inv * t_vec_;
-    return {r_inv, -t_inv};
+    return Pose3d(r_inv, -t_inv);
   }
 
   Pose3d operator*(const Pose3d &rhs) const {
@@ -54,16 +54,11 @@ class Pose3d {
     return r_quat_ * vec;
   }
 
-  // Spherical linear interpolation to `pose_to`
+  // Spherical linear interpolation to `pose_to`， t \belong [0,1]
   Pose3d Interpolate(const Pose3d &pose_to, double t) const {
-    Pose3d pose_dst = t > 0 ? pose_to : (*this) * pose_to.Inv() * (*this);
+    Eigen::Quaterniond r_interp = r_quat_.slerp(t, pose_to.r_quat_);
-    t = t > 0 ? t : -t;
+    Eigen::Vector3d t_interp = (pose_to.t_vec_ - t_vec_) * t + t_vec_;
-    int whole = std::floor(t);
+    return Pose3d(r_interp, t_interp);
-    double frac = t - whole;
-    Eigen::Quaterniond r_interp = r_quat_.slerp(frac, pose_dst.r_quat_);
-    while (whole--) r_interp *= pose_dst.r_quat_;
-    Eigen::Vector3d t_interp = (pose_dst.t_vec_ - t_vec_) * t + t_vec_;
-    return {r_interp, t_interp};
   }
 
   std::string ToString() const {

oh_my_loam/base/helper.cc

@@ -4,7 +4,7 @@ namespace oh_my_loam {
 
 void TransformToStart(const Pose3d &pose, const TPoint &pt_in,
                       TPoint *const pt_out) {
-  Pose3d pose_interp = Pose3d().Interpolate(pose, pt_in.time);
+  Pose3d pose_interp = Pose3d().Interpolate(pose, GetTime(pt_in));
   TransformPoint<TPoint>(pose_interp, pt_in, pt_out);
 }
 

oh_my_loam/base/helper.h

@@ -7,13 +7,21 @@ namespace oh_my_loam {
 
 using common::Pose3d;
 
+inline int GetScanId(const TPoint &pt) {
+  return static_cast<int>(pt.time);
+}
+
+inline float GetTime(const TPoint &pt) {
+  return pt.time - GetScanId(pt);
+}
+
 template <typename PT>
 void TransformPoint(const Pose3d &pose, const PT &pt_in, PT *const pt_out) {
   *pt_out = pt_in;
-  Eigen::Vector3d pnt = pose * Eigen::Vector3d(pt_in.x, pt_in.y, pt_in.z);
+  Eigen::Vector3d p = pose * Eigen::Vector3d(pt_in.x, pt_in.y, pt_in.z);
-  pt_out->x = pnt.x();
+  pt_out->x = p.x();
-  pt_out->y = pnt.y();
+  pt_out->y = p.y();
-  pt_out->z = pnt.z();
+  pt_out->z = p.z();
 }
 
 template <typename PT>

oh_my_loam/configs/config.yaml

@@ -7,24 +7,24 @@ vis: true
 # configs for extractor
 extractor_config:
   vis: false
-  verbose: true
+  verbose: false
   min_point_num: 66
   sharp_corner_point_num: 2
-  corner_point_num: 20
+  corner_point_num: 10
   flat_surf_point_num: 4
-  surf_point_num: 30
+  surf_point_num: 20
   corner_point_curvature_th: 0.5
   surf_point_curvature_th: 0.5
-  neighbor_point_dist_th: 0.1
+  neighbor_point_dist_sq_th: 0.1
-  downsample_voxel_size: 0.3
 
 # configs for odometer
 odometer_config:
   vis: true
   verbose: true
   icp_iter_num: 2
-  solve_iter_num: 5
+  solve_iter_num: 4
-  match_dist_sq_th: 1
+  corn_match_dist_sq_th: 1.0
+  surf_match_dist_sq_th: 1.0
 
 # configs for mapper
 mapper_config:

oh_my_loam/extractor/extractor.cc

@@ -9,6 +9,7 @@ namespace oh_my_loam {
 namespace {
 const int kScanSegNum = 6;
 const double kTwoPi = 2 * M_PI;
+const double kEps = 1e-6;
 }  // namespace
 
 bool Extractor::Init() {
@@ -70,7 +71,7 @@ void Extractor::SplitScan(const common::PointCloud &cloud,
       half_passed = true;
       yaw_start += kTwoPi;
     }
-    double time = std::min(yaw_diff / kTwoPi, 1.0) + scan_id;
+    double time = std::min(yaw_diff / kTwoPi, 1 - kEps) + scan_id;
     scans->at(scan_id).push_back(
         {pt.x, pt.y, pt.z, static_cast<float>(time), std::nanf("")});
   }
@@ -173,15 +174,10 @@ void Extractor::GenerateFeature(const TCTPointCloud &scan,
         feature->cloud_corner->push_back(point);
         break;
       default:
-        feature->cloud_surf->push_back(point);
+        // feature->cloud_surf->push_back(point);
         break;
     }
   }
-  TPointCloudPtr dowm_sampled(new TPointCloud);
-  common::VoxelDownSample<TPoint>(
-      feature->cloud_surf, dowm_sampled.get(),
-      config_["downsample_voxel_size"].as<double>());
-  feature->cloud_surf = dowm_sampled;
 }
 
 void Extractor::Visualize(const common::PointCloudConstPtr &cloud,
@@ -199,17 +195,18 @@ void Extractor::UpdateNeighborsPicked(const TCTPointCloud &scan, size_t ix,
   auto dist_sq = [&](size_t i, size_t j) -> double {
     return common::DistanceSqure<TCTPoint>(scan[i], scan[j]);
   };
-  double neighbor_point_dist_th = config_["neighbor_point_dist_th"].as<float>();
+  double neighbor_point_dist_sq_th =
+      config_["neighbor_point_dist_sq_th"].as<float>();
   for (size_t i = 1; i <= 5; ++i) {
     if (ix < i) break;
     if (picked->at(ix - i)) continue;
-    if (dist_sq(ix - i, ix - i + 1) > neighbor_point_dist_th) break;
+    if (dist_sq(ix - i, ix - i + 1) > neighbor_point_dist_sq_th) break;
     picked->at(ix - i) = true;
   }
   for (size_t i = 1; i <= 5; ++i) {
     if (ix + i >= scan.size()) break;
     if (picked->at(ix + i)) continue;
-    if (dist_sq(ix + i, ix + i - 1) > neighbor_point_dist_th) break;
+    if (dist_sq(ix + i, ix + i - 1) > neighbor_point_dist_sq_th) break;
     picked->at(ix + i) = true;
   }
 }

oh_my_loam/odometer/odometer.cc

@@ -13,12 +13,6 @@ namespace oh_my_loam {
 namespace {
 int kNearScanN = 2;
 size_t kMinMatchNum = 10;
-int GetScanId(const TPoint &point) {
-  return static_cast<int>(point.time);
-}
-float GetTime(const TPoint &point) {
-  return point.time - GetScanId(point);
-}
 }  // namespace
 
 bool Odometer::Init() {
@@ -64,10 +58,10 @@ void Odometer::Process(double timestamp, const std::vector<Feature> &features,
     }
     PoseSolver solver(pose_curr2last_);
     for (const auto &pair : pl_pairs) {
-      solver.AddPointLinePair(pair, pair.pt.time);
+      solver.AddPointLinePair(pair, GetTime(pair.pt));
     }
     for (const auto &pair : pp_pairs) {
-      solver.AddPointPlanePair(pair, pair.pt.time);
+      solver.AddPointPlanePair(pair, GetTime(pair.pt));
     }
     solver.Solve(config_["solve_iter_num"].as<int>(), verbose_,
                  &pose_curr2last_);
@@ -84,103 +78,81 @@ void Odometer::Process(double timestamp, const std::vector<Feature> &features,
 
 void Odometer::MatchCorn(const TPointCloud &src,
                          std::vector<PointLinePair> *const pairs) const {
-  double dist_sq_thresh = config_["match_dist_sq_th"].as<double>();
+  double dist_sq_thresh = config_["corn_match_dist_sq_th"].as<double>();
-  for (const auto &point : src) {
+  for (const auto &pt : src) {
-    TPoint query_pt = TransformToStart(pose_curr2last_, point);
+    TPoint query_pt = TransformToStart(pose_curr2last_, pt);
-    std::vector<int> indices(1);
+    std::vector<int> indices;
-    std::vector<float> dists(1);
+    std::vector<float> dists;
-    if (kdtree_corn_.nearestKSearch(query_pt, 1, indices, dists) != 1) {
+    if (kdtree_corn_.nearestKSearch(query_pt, 1, indices, dists) < 1) {
       continue;
     }
     if (dists[0] >= dist_sq_thresh) continue;
     TPoint pt1 = kdtree_corn_.getInputCloud()->at(indices[0]);
     int pt1_scan_id = GetScanId(pt1);
 
-    bool pt2_fount = false;
-    float min_pt2_dist_squre = dist_sq_thresh;
     TPoint pt2;
+    bool pt2_fount = false;
+    float min_pt2_dist_sq = dist_sq_thresh;
     int i_begin = std::max<int>(0, pt1_scan_id - kNearScanN);
     int i_end =
         std::min<int>(kdtrees_scan_corn_.size(), pt1_scan_id + kNearScanN + 1);
-    for (int i = i_begin; i < i_end && i != pt1_scan_id; ++i) {
+    for (int i = i_begin; i < i_end; ++i) {
+      if (i == pt1_scan_id) continue;
       const auto &kdtree = kdtrees_scan_corn_[i];
-      if (kdtree.nearestKSearch(query_pt, 1, indices, dists) != 0) {
+      if (kdtree.nearestKSearch(query_pt, 1, indices, dists) < 1) {
         continue;
       }
-      if (dists[0] < min_pt2_dist_squre) {
+      if (dists[0] < min_pt2_dist_sq) {
         pt2_fount = true;
         pt2 = kdtree.getInputCloud()->at(indices[0]);
-        min_pt2_dist_squre = dists[0];
+        min_pt2_dist_sq = dists[0];
       }
     }
     if (!pt2_fount) continue;
 
-    TPoint pt(point.x, point.y, point.z, GetTime(point));
-    pt1.time = GetTime(pt1);
-    pt2.time = GetTime(pt2);
     pairs->emplace_back(pt, pt1, pt2);
   }
 }
 
 void Odometer::MatchSurf(const TPointCloud &src,
                          std::vector<PointPlanePair> *const pairs) const {
-  double dist_sq_thresh = config_["match_dist_sq_th"].as<double>();
+  double dist_sq_thresh = config_["surf_match_dist_sq_th"].as<double>();
-  for (const auto &point : src) {
+  for (const auto &pt : src) {
-    TPoint query_pt = TransformToStart(pose_curr2last_, point);
+    TPoint query_pt = TransformToStart(pose_curr2last_, pt);
     std::vector<int> indices;
     std::vector<float> dists;
-    if (kdtree_surf_.nearestKSearch(query_pt, 2, indices, dists) != 2) {
+    if (kdtree_surf_.nearestKSearch(query_pt, 1, indices, dists) < 1) {
       continue;
     }
-    if (dists[0] >= dist_sq_thresh || dists[1] >= dist_sq_thresh) continue;
+    if (dists[0] >= dist_sq_thresh) continue;
     TPoint pt1 = kdtree_surf_.getInputCloud()->at(indices[0]);
     int pt1_scan_id = GetScanId(pt1);
 
-    TPoint pt2 = kdtree_surf_.getInputCloud()->at(indices[1]), pt3;
+    TPoint pt2;
-    bool pt2_found = true, pt3_found = false;
+    bool pt2_fount = false;
-    int pt2_scan_id = GetScanId(pt2);
+    float min_pt2_dist_sq = dist_sq_thresh;
-    if (pt2_scan_id != pt1_scan_id) {
+    int i_begin = std::max<int>(0, pt1_scan_id - kNearScanN);
-      pt2_found = false;
+    int i_end =
-      if (std::abs(pt2_scan_id - pt1_scan_id) <= kNearScanN) {
+        std::min<int>(kdtrees_scan_surf_.size(), pt1_scan_id + kNearScanN + 1);
-        pt3 = pt2;
+    for (int i = i_begin; i < i_end; ++i) {
-        pt3_found = true;
+      if (i == pt1_scan_id) continue;
+      const auto &kdtree = kdtrees_scan_surf_[i];
+      if (kdtree.nearestKSearch(query_pt, 1, indices, dists) < 1) {
+        continue;
+      }
+      if (dists[0] < min_pt2_dist_sq) {
+        pt2_fount = true;
+        pt2 = kdtree.getInputCloud()->at(indices[0]);
+        min_pt2_dist_sq = dists[0];
       }
     }
+    if (!pt2_fount) continue;
 
-    if (!pt2_found) {
+    const auto &kdtree = kdtrees_scan_surf_[pt1_scan_id];
-      const auto &kdtree = kdtrees_scan_surf_[pt1_scan_id];
+    if (kdtree.nearestKSearch(query_pt, 2, indices, dists) < 2) continue;
-      if (kdtree.nearestKSearch(query_pt, 2, indices, dists) == 2) {
+    if (dists[1] >= dist_sq_thresh) continue;
-        if (dists[1] < dist_sq_thresh) {
+    TPoint pt3 = kdtree.getInputCloud()->at(indices[1]);
-          pt2 = kdtree.getInputCloud()->at(indices[1]);
-          pt2_found = true;
-        }
-      }
-    }
-    if (!pt2_found) continue;
 
-    if (!pt3_found) {
-      float min_pt3_dist_squre = dist_sq_thresh;
-      int i_begin = std::max<int>(0, pt1_scan_id - kNearScanN);
-      int i_end = std::min<int>(kdtrees_scan_corn_.size(),
-                                pt1_scan_id + kNearScanN + 1);
-      for (int i = i_begin; i < i_end && i != pt1_scan_id; ++i) {
-        const auto &kdtree = kdtrees_scan_surf_[i];
-        if (kdtree.nearestKSearch(query_pt, 1, indices, dists) != 1) {
-          continue;
-        }
-        if (dists[0] < min_pt3_dist_squre) {
-          pt3 = kdtree.getInputCloud()->at(indices[0]);
-          pt3_found = true;
-          min_pt3_dist_squre = dists[0];
-        }
-      }
-    }
-    if (!pt3_found) continue;
-
-    TPoint pt(point.x, point.y, point.z, GetTime(point));
-    pt1.time = GetTime(pt1);
-    pt2.time = GetTime(pt2);
-    pt3.time = GetTime(pt3);
     pairs->emplace_back(pt, pt1, pt2, pt3);
   }
 }
@@ -188,12 +160,12 @@ void Odometer::MatchSurf(const TPointCloud &src,
 void Odometer::UpdatePre(const std::vector<Feature> &features) {
   kdtrees_scan_corn_.resize(features.size());
   kdtrees_scan_surf_.resize(features.size());
-  TPointCloudPtr corn_pre(new TPointCloud);
   TPointCloudPtr surf_pre(new TPointCloud);
-  TPointCloudPtr scan_corn_pre(new TPointCloud);
+  TPointCloudPtr corn_pre(new TPointCloud);
-  TPointCloudPtr scan_surf_pre(new TPointCloud);
   for (size_t i = 0; i < features.size(); ++i) {
     const auto &feature = features[i];
+    TPointCloudPtr scan_corn_pre(new TPointCloud);
+    TPointCloudPtr scan_surf_pre(new TPointCloud);
     scan_corn_pre->resize(feature.cloud_corner->size());
     scan_surf_pre->resize(feature.cloud_surf->size());
     for (size_t j = 0; j < feature.cloud_corner->size(); ++j) {

oh_my_loam/solver/cost_function.h

@@ -2,6 +2,9 @@
 
 #include <ceres/ceres.h>
 
+#include <eigen3/Eigen/Dense>
+#include <type_traits>
+
 #include "common/common.h"
 #include "oh_my_loam/base/helper.h"
 
@@ -13,7 +16,8 @@ class PointLineCostFunction {
       : pair_(pair), time_(time){};
 
   template <typename T>
-  bool operator()(const T *const q, const T *const p, T *residual) const;
+  bool operator()(const T *const r_quat, const T *const t_vec,
+                  T *residual) const;
 
   static ceres::CostFunction *Create(const PointLinePair &pair, double time) {
     return new ceres::AutoDiffCostFunction<PointLineCostFunction, 3, 4, 3>(
@@ -33,7 +37,8 @@ class PointPlaneCostFunction {
       : pair_(pair), time_(time){};
 
   template <typename T>
-  bool operator()(const T *const q, const T *const p, T *residual) const;
+  bool operator()(const T *const r_quat, const T *const t_vec,
+                  T *residual) const;
 
   static ceres::CostFunction *Create(const PointPlanePair &pair, double time) {
     return new ceres::AutoDiffCostFunction<PointPlaneCostFunction, 1, 4, 3>(
@@ -47,46 +52,56 @@ class PointPlaneCostFunction {
 };
 
 template <typename T>
-bool PointLineCostFunction::operator()(const T *const q, const T *const p,
+bool PointLineCostFunction::operator()(const T *const r_quat,
+                                       const T *const t_vec,
                                        T *residual) const {
-  const auto pt = pair_.pt, pt1 = pair_.line.pt1, pt2 = pair_.line.pt2;
+  const auto &pt = pair_.pt, &pt1 = pair_.line.pt1, &pt2 = pair_.line.pt2;
-  Eigen::Matrix<T, 3, 1> pnt(T(pt.x), T(pt.y), T(pt.z));
+  Eigen::Matrix<T, 3, 1> p(T(pt.x), T(pt.y), T(pt.z));
-  Eigen::Matrix<T, 3, 1> pnt1(T(pt1.x), T(pt1.y), T(pt1.z));
+  Eigen::Matrix<T, 3, 1> p1(T(pt1.x), T(pt1.y), T(pt1.z));
-  Eigen::Matrix<T, 3, 1> pnt2(T(pt2.x), T(pt2.y), T(pt2.z));
+  Eigen::Matrix<T, 3, 1> p2(T(pt2.x), T(pt2.y), T(pt2.z));
 
-  Eigen::Quaternion<T> r(q[3], q[0], q[1], q[2]);
+  Eigen::Quaternion<T> r(r_quat[3], r_quat[0], r_quat[1], r_quat[2]);
-  Eigen::Quaternion<T> q_identity{T(1), T(0), T(0), T(0)};
+  Eigen::Quaternion<T> r_interp =
-  r = q_identity.slerp(T(time_), r);
+      Eigen::Quaternion<T>::Identity().slerp(T(time_), r);
-  Eigen::Matrix<T, 3, 1> t(T(time_) * p[0], T(time_) * p[1], T(time_) * p[2]);
+  Eigen::Matrix<T, 3, 1> t(T(time_) * t_vec[0], T(time_) * t_vec[1],
-  Eigen::Matrix<T, 3, 1> pnt_n = r * pnt + t;
+                           T(time_) * t_vec[2]);
+  Eigen::Matrix<T, 3, 1> p0 = r_interp * p + t;
 
   // norm of cross product: triangle area x 2
-  Eigen::Matrix<T, 3, 1> area = (pnt_n - pnt1).cross(pnt_n - pnt2) * 0.5;
+  Eigen::Matrix<T, 3, 1> area = (p0 - p1).cross(p0 - p2) * 0.5;
-  T base_length = (pnt2 - pnt1).norm();
+  T base_length = (p2 - p1).norm();
   residual[0] = area[0] / base_length;
   residual[1] = area[1] / base_length;
   residual[2] = area[2] / base_length;
+  if constexpr (!std::is_arithmetic<T>::value) {
+    AERROR << p.transpose() << ", ";
+  }
   return true;
 }
 
 template <typename T>
-bool PointPlaneCostFunction::operator()(const T *const q, const T *const p,
+bool PointPlaneCostFunction::operator()(const T *const r_quat,
+                                        const T *const t_vec,
                                         T *residual) const {
   const auto &pt = pair_.pt, &pt1 = pair_.plane.pt1, &pt2 = pair_.plane.pt2,
              &pt3 = pair_.plane.pt3;
-  Eigen::Matrix<T, 3, 1> pnt(T(pt.x), T(pt.y), T(pt.z));
+  Eigen::Matrix<T, 3, 1> p(T(pt.x), T(pt.y), T(pt.z));
-  Eigen::Matrix<T, 3, 1> pnt1(T(pt1.x), T(pt1.y), T(pt1.z));
+  Eigen::Matrix<T, 3, 1> p1(T(pt1.x), T(pt1.y), T(pt1.z));
-  Eigen::Matrix<T, 3, 1> pnt2(T(pt2.x), T(pt2.y), T(pt2.z));
+  Eigen::Matrix<T, 3, 1> p2(T(pt2.x), T(pt2.y), T(pt2.z));
-  Eigen::Matrix<T, 3, 1> pnt3(T(pt3.x), T(pt3.y), T(pt3.z));
+  Eigen::Matrix<T, 3, 1> p3(T(pt3.x), T(pt3.y), T(pt3.z));
 
-  Eigen::Quaternion<T> r(q[3], q[0], q[1], q[2]);
+  Eigen::Quaternion<T> r(r_quat[3], r_quat[0], r_quat[1], r_quat[2]);
-  Eigen::Quaternion<T> q_identity{T(1), T(0), T(0), T(0)};
+  Eigen::Quaternion<T> r_interp =
-  r = q_identity.slerp(T(time_), r);
+      Eigen::Quaternion<T>::Identity().slerp(T(time_), r);
-  Eigen::Matrix<T, 3, 1> t(T(time_) * p[0], T(time_) * p[1], T(time_) * p[2]);
+  Eigen::Matrix<T, 3, 1> t(T(time_) * t_vec[0], T(time_) * t_vec[1],
-  Eigen::Matrix<T, 3, 1> pnt_n = r * pnt + t;
+                           T(time_) * t_vec[2]);
+  Eigen::Matrix<T, 3, 1> p0 = r_interp * p + t;
 
-  Eigen::Matrix<T, 3, 1> normal = (pnt2 - pnt1).cross(pnt3 - pnt1).normalized();
+  Eigen::Matrix<T, 3, 1> normal = (p2 - p1).cross(p3 - p1).normalized();
-  residual[0] = (pnt_n - pnt1).dot(normal);
+  residual[0] = (p0 - p1).dot(normal);
+  if constexpr (!std::is_arithmetic<T>::value) {
+    AERROR << "ppres" << residual[0].a;
+  }
   return true;
 }
 

oh_my_loam/solver/solver.cc

@@ -2,6 +2,7 @@
 
 #include <Eigen/src/Core/Matrix.h>
 #include <ceres/loss_function.h>
+#include <ceres/types.h>
 
 #include "common/log/log.h"
 
@@ -29,9 +30,10 @@ bool PoseSolver::Solve(int max_iter_num, bool verbose,
   ceres::Solver::Summary summary;
   ceres::Solve(options, &problem_, &summary);
   AINFO_IF(verbose) << summary.BriefReport();
-  AWARN_IF(!summary.IsSolutionUsable()) << "Solution may be unusable";
+  AWARN_IF(summary.termination_type != ceres::CONVERGENCE)
+      << "Solve: no convergence";
   if (pose) *pose = common::Pose3d(r_quat_, t_vec_);
-  return summary.IsSolutionUsable();
+  return summary.termination_type == ceres::CONVERGENCE;
 }
 
 void PoseSolver::AddPointLinePair(const PointLinePair &pair, double time) {

oh_my_loam/visualizer/odometer_visualizer.cc

@@ -1,5 +1,7 @@
 #include "odometer_visualizer.h"
 
+#include "common/color/color.h"
+
 namespace oh_my_loam {
 
 namespace {
@@ -8,31 +10,31 @@ using namespace common;
 
 void OdometerVisualizer::Draw() {
   auto frame = GetCurrentFrame<OdometerVisFrame>();
-  TPointCloudPtr src_corn_pts(new TPointCloud);
+  TPointCloudPtr src_corn(new TPointCloud);
-  TPointCloudPtr tgt_corn_pts(new TPointCloud);
+  TPointCloudPtr tgt_corn(new TPointCloud);
-  src_corn_pts->resize(frame.pl_pairs.size());
+  src_corn->resize(frame.pl_pairs.size());
-  tgt_corn_pts->resize(frame.pl_pairs.size() * 2);
+  tgt_corn->resize(frame.pl_pairs.size() * 2);
   for (size_t i = 0; i < frame.pl_pairs.size(); ++i) {
     const auto &pair = frame.pl_pairs[i];
-    TransformToStart(frame.pose_curr2last, pair.pt, &src_corn_pts->points[i]);
+    TransformToStart(frame.pose_curr2last, pair.pt, &src_corn->points[i]);
-    tgt_corn_pts->points[2 * i] = pair.line.pt1;
+    tgt_corn->points[2 * i] = pair.line.pt1;
-    tgt_corn_pts->points[2 * i + 1] = pair.line.pt2;
+    tgt_corn->points[2 * i + 1] = pair.line.pt2;
   }
-  TPointCloudPtr src_surf_pts(new TPointCloud);
+  TPointCloudPtr src_surf(new TPointCloud);
-  TPointCloudPtr tgt_surf_pts(new TPointCloud);
+  TPointCloudPtr tgt_surf(new TPointCloud);
-  src_surf_pts->resize(frame.pp_pairs.size());
+  src_surf->resize(frame.pp_pairs.size());
-  tgt_surf_pts->resize(frame.pp_pairs.size() * 3);
+  tgt_surf->resize(frame.pp_pairs.size() * 3);
   for (size_t i = 0; i < frame.pp_pairs.size(); ++i) {
     const auto &pair = frame.pp_pairs[i];
-    TransformToStart(frame.pose_curr2last, pair.pt, &src_surf_pts->points[i]);
+    TransformToStart(frame.pose_curr2last, pair.pt, &src_surf->points[i]);
-    tgt_surf_pts->points[3 * i] = pair.plane.pt1;
+    tgt_surf->points[3 * i] = pair.plane.pt1;
-    tgt_surf_pts->points[3 * i + 1] = pair.plane.pt2;
+    tgt_surf->points[3 * i + 1] = pair.plane.pt2;
-    tgt_surf_pts->points[3 * i + 2] = pair.plane.pt3;
+    tgt_surf->points[3 * i + 2] = pair.plane.pt3;
   }
-  DrawPointCloud<TPoint>(src_corn_pts, CYAN, "src_corn_pts", 7);
+  DrawPointCloud<TPoint>(tgt_corn, YELLOW, "tgt_corn", 4);
-  DrawPointCloud<TPoint>(tgt_corn_pts, BLUE, "tgt_corn_pts", 4);
+  DrawPointCloud<TPoint>(src_corn, RED, "src_corn", 4);
-  DrawPointCloud<TPoint>(src_surf_pts, PURPLE, "src_surf_pts", 7);
+  DrawPointCloud<TPoint>(tgt_surf, BLUE, "tgt_surf", 4);
-  DrawPointCloud<TPoint>(tgt_surf_pts, RED, "tgt_surf_pts", 4);
+  DrawPointCloud<TPoint>(src_surf, CYAN, "src_surf", 4);
   std::vector<Pose3d> poses_n;
   poses_n.reserve((poses_.size()));
   Pose3d pose_inv = frame.pose_curr2world.Inv();