12345qiupeng
/
oh_my_loam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

odometer ok, a nan bug needs to be located

main
feixyz10 2021-01-25 01:47:46 +08:00 committed by feixyz
parent 38b85ab8f2
commit f47143f01b
9 changed files with 142 additions and 151 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
common/geometry/pose3d.h
View File

@ -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);
t = t > 0 ? t : -t;
int whole = std::floor(t);
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};
Eigen::Quaterniond r_interp = r_quat_.slerp(t, pose_to.r_quat_);
Eigen::Vector3d t_interp = (pose_to.t_vec_ - t_vec_) * t + t_vec_;
return Pose3d(r_interp, t_interp);
}
std::string ToString() const {

2
oh_my_loam/base/helper.cc
View File

@ -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);
}

16
oh_my_loam/base/helper.h
View File

@ -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);
pt_out->x = pnt.x();
pt_out->y = pnt.y();
pt_out->z = pnt.z();
Eigen::Vector3d p = pose * Eigen::Vector3d(pt_in.x, pt_in.y, pt_in.z);
pt_out->x = p.x();
pt_out->y = p.y();
pt_out->z = p.z();
}
template <typename PT>

14
oh_my_loam/configs/config.yaml
View File

@ -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
downsample_voxel_size: 0.3
neighbor_point_dist_sq_th: 0.1
# configs for odometer
odometer_config:
vis: true
verbose: true
icp_iter_num: 2
solve_iter_num: 5
match_dist_sq_th: 1
solve_iter_num: 4
corn_match_dist_sq_th: 1.0
surf_match_dist_sq_th: 1.0
# configs for mapper
mapper_config:

17
oh_my_loam/extractor/extractor.cc
View File

@ -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;
}
}

110
oh_my_loam/odometer/odometer.cc
View File

@ -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>();
for (const auto &point : src) {
TPoint query_pt = TransformToStart(pose_curr2last_, point);
std::vector<int> indices(1);
std::vector<float> dists(1);
if (kdtree_corn_.nearestKSearch(query_pt, 1, indices, dists) != 1) {
double dist_sq_thresh = config_["corn_match_dist_sq_th"].as<double>();
for (const auto &pt : src) {
TPoint query_pt = TransformToStart(pose_curr2last_, pt);
std::vector<int> indices;
std::vector<float> dists;
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>();
for (const auto &point : src) {
TPoint query_pt = TransformToStart(pose_curr2last_, point);
double dist_sq_thresh = config_["surf_match_dist_sq_th"].as<double>();
for (const auto &pt : src) {
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;
bool pt2_found = true, pt3_found = false;
int pt2_scan_id = GetScanId(pt2);
if (pt2_scan_id != pt1_scan_id) {
pt2_found = false;
if (std::abs(pt2_scan_id - pt1_scan_id) <= kNearScanN) {
pt3 = pt2;
pt3_found = true;
}
}
if (!pt2_found) {
const auto &kdtree = kdtrees_scan_surf_[pt1_scan_id];
if (kdtree.nearestKSearch(query_pt, 2, indices, dists) == 2) {
if (dists[1] < dist_sq_thresh) {
pt2 = kdtree.getInputCloud()->at(indices[1]);
pt2_found = true;
}
}
}
if (!pt2_found) continue;
if (!pt3_found) {
float min_pt3_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) {
int i_end =
std::min<int>(kdtrees_scan_surf_.size(), pt1_scan_id + kNearScanN + 1);
for (int i = i_begin; i < i_end; ++i) {
if (i == pt1_scan_id) continue;
const auto &kdtree = kdtrees_scan_surf_[i];
if (kdtree.nearestKSearch(query_pt, 1, indices, dists) != 1) {
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 (dists[0] < min_pt2_dist_sq) {
pt2_fount = true;
pt2 = kdtree.getInputCloud()->at(indices[0]);
min_pt2_dist_sq = dists[0];
}
}
}
if (!pt3_found) continue;
if (!pt2_fount) continue;
const auto &kdtree = kdtrees_scan_surf_[pt1_scan_id];
if (kdtree.nearestKSearch(query_pt, 2, indices, dists) < 2) continue;
if (dists[1] >= dist_sq_thresh) continue;
TPoint pt3 = kdtree.getInputCloud()->at(indices[1]);
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 scan_surf_pre(new TPointCloud);
TPointCloudPtr corn_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) {

67
oh_my_loam/solver/cost_function.h
View File

@ -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;
Eigen::Matrix<T, 3, 1> pnt(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> pnt2(T(pt2.x), T(pt2.y), T(pt2.z));
const auto &pt = pair_.pt, &pt1 = pair_.line.pt1, &pt2 = pair_.line.pt2;
Eigen::Matrix<T, 3, 1> p(T(pt.x), T(pt.y), T(pt.z));
Eigen::Matrix<T, 3, 1> p1(T(pt1.x), T(pt1.y), T(pt1.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> q_identity{T(1), T(0), T(0), T(0)};
r = q_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> pnt_n = r * pnt + t;
Eigen::Quaternion<T> r(r_quat[3], r_quat[0], r_quat[1], r_quat[2]);
Eigen::Quaternion<T> r_interp =
Eigen::Quaternion<T>::Identity().slerp(T(time_), r);
Eigen::Matrix<T, 3, 1> t(T(time_) * t_vec[0], T(time_) * t_vec[1],
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;
T base_length = (pnt2 - pnt1).norm();
Eigen::Matrix<T, 3, 1> area = (p0 - p1).cross(p0 - p2) * 0.5;
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> pnt1(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> pnt3(T(pt3.x), T(pt3.y), T(pt3.z));
Eigen::Matrix<T, 3, 1> p(T(pt.x), T(pt.y), T(pt.z));
Eigen::Matrix<T, 3, 1> p1(T(pt1.x), T(pt1.y), T(pt1.z));
Eigen::Matrix<T, 3, 1> p2(T(pt2.x), T(pt2.y), T(pt2.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> q_identity{T(1), T(0), T(0), T(0)};
r = q_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> pnt_n = r * pnt + t;
Eigen::Quaternion<T> r(r_quat[3], r_quat[0], r_quat[1], r_quat[2]);
Eigen::Quaternion<T> r_interp =
Eigen::Quaternion<T>::Identity().slerp(T(time_), r);
Eigen::Matrix<T, 3, 1> t(T(time_) * t_vec[0], T(time_) * t_vec[1],
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();
residual[0] = (pnt_n - pnt1).dot(normal);
Eigen::Matrix<T, 3, 1> normal = (p2 - p1).cross(p3 - p1).normalized();
residual[0] = (p0 - p1).dot(normal);
if constexpr (!std::is_arithmetic<T>::value) {
AERROR << "ppres" << residual[0].a;
}
return true;
}

6
oh_my_loam/solver/solver.cc
View File

@ -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) {

40
oh_my_loam/visualizer/odometer_visualizer.cc
View File

@ -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 tgt_corn_pts(new TPointCloud);
src_corn_pts->resize(frame.pl_pairs.size());
tgt_corn_pts->resize(frame.pl_pairs.size() * 2);
TPointCloudPtr src_corn(new TPointCloud);
TPointCloudPtr tgt_corn(new TPointCloud);
src_corn->resize(frame.pl_pairs.size());
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]);
tgt_corn_pts->points[2 * i] = pair.line.pt1;
tgt_corn_pts->points[2 * i + 1] = pair.line.pt2;
TransformToStart(frame.pose_curr2last, pair.pt, &src_corn->points[i]);
tgt_corn->points[2 * i] = pair.line.pt1;
tgt_corn->points[2 * i + 1] = pair.line.pt2;
}
TPointCloudPtr src_surf_pts(new TPointCloud);
TPointCloudPtr tgt_surf_pts(new TPointCloud);
src_surf_pts->resize(frame.pp_pairs.size());
tgt_surf_pts->resize(frame.pp_pairs.size() * 3);
TPointCloudPtr src_surf(new TPointCloud);
TPointCloudPtr tgt_surf(new TPointCloud);
src_surf->resize(frame.pp_pairs.size());
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]);
tgt_surf_pts->points[3 * i] = pair.plane.pt1;
tgt_surf_pts->points[3 * i + 1] = pair.plane.pt2;
tgt_surf_pts->points[3 * i + 2] = pair.plane.pt3;
TransformToStart(frame.pose_curr2last, pair.pt, &src_surf->points[i]);
tgt_surf->points[3 * i] = pair.plane.pt1;
tgt_surf->points[3 * i + 1] = pair.plane.pt2;
tgt_surf->points[3 * i + 2] = pair.plane.pt3;
}
DrawPointCloud<TPoint>(src_corn_pts, CYAN, "src_corn_pts", 7);
DrawPointCloud<TPoint>(tgt_corn_pts, BLUE, "tgt_corn_pts", 4);
DrawPointCloud<TPoint>(src_surf_pts, PURPLE, "src_surf_pts", 7);
DrawPointCloud<TPoint>(tgt_surf_pts, RED, "tgt_surf_pts", 4);
DrawPointCloud<TPoint>(tgt_corn, YELLOW, "tgt_corn", 4);
DrawPointCloud<TPoint>(src_corn, RED, "src_corn", 4);
DrawPointCloud<TPoint>(tgt_surf, BLUE, "tgt_surf", 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();