diff --git a/common/geometry/pose3d.h b/common/geometry/pose3d.h index a3d8224..8b9211a 100644 --- a/common/geometry/pose3d.h +++ b/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); - 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 { diff --git a/oh_my_loam/base/helper.cc b/oh_my_loam/base/helper.cc index f9a972c..0bb6a7c 100644 --- a/oh_my_loam/base/helper.cc +++ b/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(pose_interp, pt_in, pt_out); } diff --git a/oh_my_loam/base/helper.h b/oh_my_loam/base/helper.h index 3aae436..beed983 100644 --- a/oh_my_loam/base/helper.h +++ b/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(pt.time); +} + +inline float GetTime(const TPoint &pt) { + return pt.time - GetScanId(pt); +} + template 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 diff --git a/oh_my_loam/configs/config.yaml b/oh_my_loam/configs/config.yaml index cf66638..6aa52fd 100644 --- a/oh_my_loam/configs/config.yaml +++ b/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 - 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: diff --git a/oh_my_loam/extractor/extractor.cc b/oh_my_loam/extractor/extractor.cc index a56d174..ece1095 100644 --- a/oh_my_loam/extractor/extractor.cc +++ b/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(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( - feature->cloud_surf, dowm_sampled.get(), - config_["downsample_voxel_size"].as()); - 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(scan[i], scan[j]); }; - double neighbor_point_dist_th = config_["neighbor_point_dist_th"].as(); + double neighbor_point_dist_sq_th = + config_["neighbor_point_dist_sq_th"].as(); 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; } } diff --git a/oh_my_loam/odometer/odometer.cc b/oh_my_loam/odometer/odometer.cc index 6fcb67b..058b60e 100644 --- a/oh_my_loam/odometer/odometer.cc +++ b/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(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 &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(), verbose_, &pose_curr2last_); @@ -84,103 +78,81 @@ void Odometer::Process(double timestamp, const std::vector &features, void Odometer::MatchCorn(const TPointCloud &src, std::vector *const pairs) const { - double dist_sq_thresh = config_["match_dist_sq_th"].as(); - for (const auto &point : src) { - TPoint query_pt = TransformToStart(pose_curr2last_, point); - std::vector indices(1); - std::vector dists(1); - if (kdtree_corn_.nearestKSearch(query_pt, 1, indices, dists) != 1) { + double dist_sq_thresh = config_["corn_match_dist_sq_th"].as(); + for (const auto &pt : src) { + TPoint query_pt = TransformToStart(pose_curr2last_, pt); + std::vector indices; + std::vector 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(0, pt1_scan_id - kNearScanN); int i_end = std::min(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 *const pairs) const { - double dist_sq_thresh = config_["match_dist_sq_th"].as(); - for (const auto &point : src) { - TPoint query_pt = TransformToStart(pose_curr2last_, point); + double dist_sq_thresh = config_["surf_match_dist_sq_th"].as(); + for (const auto &pt : src) { + TPoint query_pt = TransformToStart(pose_curr2last_, pt); std::vector indices; std::vector 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; + TPoint pt2; + bool pt2_fount = false; + float min_pt2_dist_sq = dist_sq_thresh; + int i_begin = std::max(0, pt1_scan_id - kNearScanN); + int i_end = + std::min(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) { + 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]; - 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; + 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]); - if (!pt3_found) { - float min_pt3_dist_squre = dist_sq_thresh; - int i_begin = std::max(0, pt1_scan_id - kNearScanN); - int i_end = std::min(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 &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) { diff --git a/oh_my_loam/solver/cost_function.h b/oh_my_loam/solver/cost_function.h index 41af71f..9052556 100644 --- a/oh_my_loam/solver/cost_function.h +++ b/oh_my_loam/solver/cost_function.h @@ -2,6 +2,9 @@ #include +#include +#include + #include "common/common.h" #include "oh_my_loam/base/helper.h" @@ -13,7 +16,8 @@ class PointLineCostFunction { : pair_(pair), time_(time){}; template - 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( @@ -33,7 +37,8 @@ class PointPlaneCostFunction { : pair_(pair), time_(time){}; template - 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( @@ -47,46 +52,56 @@ class PointPlaneCostFunction { }; template -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 pnt(T(pt.x), T(pt.y), T(pt.z)); - Eigen::Matrix pnt1(T(pt1.x), T(pt1.y), T(pt1.z)); - Eigen::Matrix 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 p(T(pt.x), T(pt.y), T(pt.z)); + Eigen::Matrix p1(T(pt1.x), T(pt1.y), T(pt1.z)); + Eigen::Matrix p2(T(pt2.x), T(pt2.y), T(pt2.z)); - Eigen::Quaternion r(q[3], q[0], q[1], q[2]); - Eigen::Quaternion q_identity{T(1), T(0), T(0), T(0)}; - r = q_identity.slerp(T(time_), r); - Eigen::Matrix t(T(time_) * p[0], T(time_) * p[1], T(time_) * p[2]); - Eigen::Matrix pnt_n = r * pnt + t; + Eigen::Quaternion r(r_quat[3], r_quat[0], r_quat[1], r_quat[2]); + Eigen::Quaternion r_interp = + Eigen::Quaternion::Identity().slerp(T(time_), r); + Eigen::Matrix t(T(time_) * t_vec[0], T(time_) * t_vec[1], + T(time_) * t_vec[2]); + Eigen::Matrix p0 = r_interp * p + t; // norm of cross product: triangle area x 2 - Eigen::Matrix area = (pnt_n - pnt1).cross(pnt_n - pnt2) * 0.5; - T base_length = (pnt2 - pnt1).norm(); + Eigen::Matrix 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::value) { + AERROR << p.transpose() << ", "; + } return true; } template -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 pnt(T(pt.x), T(pt.y), T(pt.z)); - Eigen::Matrix pnt1(T(pt1.x), T(pt1.y), T(pt1.z)); - Eigen::Matrix pnt2(T(pt2.x), T(pt2.y), T(pt2.z)); - Eigen::Matrix pnt3(T(pt3.x), T(pt3.y), T(pt3.z)); + Eigen::Matrix p(T(pt.x), T(pt.y), T(pt.z)); + Eigen::Matrix p1(T(pt1.x), T(pt1.y), T(pt1.z)); + Eigen::Matrix p2(T(pt2.x), T(pt2.y), T(pt2.z)); + Eigen::Matrix p3(T(pt3.x), T(pt3.y), T(pt3.z)); - Eigen::Quaternion r(q[3], q[0], q[1], q[2]); - Eigen::Quaternion q_identity{T(1), T(0), T(0), T(0)}; - r = q_identity.slerp(T(time_), r); - Eigen::Matrix t(T(time_) * p[0], T(time_) * p[1], T(time_) * p[2]); - Eigen::Matrix pnt_n = r * pnt + t; + Eigen::Quaternion r(r_quat[3], r_quat[0], r_quat[1], r_quat[2]); + Eigen::Quaternion r_interp = + Eigen::Quaternion::Identity().slerp(T(time_), r); + Eigen::Matrix t(T(time_) * t_vec[0], T(time_) * t_vec[1], + T(time_) * t_vec[2]); + Eigen::Matrix p0 = r_interp * p + t; - Eigen::Matrix normal = (pnt2 - pnt1).cross(pnt3 - pnt1).normalized(); - residual[0] = (pnt_n - pnt1).dot(normal); + Eigen::Matrix normal = (p2 - p1).cross(p3 - p1).normalized(); + residual[0] = (p0 - p1).dot(normal); + if constexpr (!std::is_arithmetic::value) { + AERROR << "ppres" << residual[0].a; + } return true; } diff --git a/oh_my_loam/solver/solver.cc b/oh_my_loam/solver/solver.cc index 2e42d10..2b5c3f9 100644 --- a/oh_my_loam/solver/solver.cc +++ b/oh_my_loam/solver/solver.cc @@ -2,6 +2,7 @@ #include #include +#include #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) { diff --git a/oh_my_loam/visualizer/odometer_visualizer.cc b/oh_my_loam/visualizer/odometer_visualizer.cc index b6bd80c..7253fb5 100644 --- a/oh_my_loam/visualizer/odometer_visualizer.cc +++ b/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(); - 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(src_corn_pts, CYAN, "src_corn_pts", 7); - DrawPointCloud(tgt_corn_pts, BLUE, "tgt_corn_pts", 4); - DrawPointCloud(src_surf_pts, PURPLE, "src_surf_pts", 7); - DrawPointCloud(tgt_surf_pts, RED, "tgt_surf_pts", 4); + DrawPointCloud(tgt_corn, YELLOW, "tgt_corn", 4); + DrawPointCloud(src_corn, RED, "src_corn", 4); + DrawPointCloud(tgt_surf, BLUE, "tgt_surf", 4); + DrawPointCloud(src_surf, CYAN, "src_surf", 4); std::vector poses_n; poses_n.reserve((poses_.size())); Pose3d pose_inv = frame.pose_curr2world.Inv();