From caf01fb59fab7448ef035f98c8f1c76110653a03 Mon Sep 17 00:00:00 2001 From: feixyz10 Date: Thu, 18 Feb 2021 14:05:33 +0800 Subject: [PATCH] fix: kdtree setInputCloud with empty cloud --- common/visualizer/lidar_visualizer_utils.cc | 10 ++++------ main.cc | 3 ++- oh_my_loam/mapper/mapper.cc | 2 ++ oh_my_loam/odometer/odometer.cc | 17 +++++++++++++---- oh_my_loam/visualizer/ohmyloam_visualizer.cc | 2 +- 5 files changed, 22 insertions(+), 12 deletions(-) diff --git a/common/visualizer/lidar_visualizer_utils.cc b/common/visualizer/lidar_visualizer_utils.cc index ae336b3..d809fcb 100644 --- a/common/visualizer/lidar_visualizer_utils.cc +++ b/common/visualizer/lidar_visualizer_utils.cc @@ -4,12 +4,10 @@ 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(); - Eigen::Vector3f p2 = trajectory.at(i + 1).t_vec().cast(); - AddLine({p1.x(), p1.y(), p1.z()}, {p2.x(), p2.y(), p2.z()}, color, - id + std::to_string(i), viewer); - } + const auto &points = trajectory.GetPointSeq(); + PointCloudPtr cloud(new PointCloud); + for (const auto &p : points) cloud->points.emplace_back(p.x(), p.y(), p.z()); + AddPointCloud(cloud, color, id, viewer, 5); } } // namespace common \ No newline at end of file diff --git a/main.cc b/main.cc index a74c4e4..98961e6 100644 --- a/main.cc +++ b/main.cc @@ -47,6 +47,7 @@ void PointCloudHandler(const sensor_msgs::PointCloud2ConstPtr &msg, double timestamp = msg->header.stamp.toSec(); static size_t frame_id = 0; AINFO << "Ohmyloam: frame_id = " << ++frame_id - << ", timestamp = " << FMT_TIMESTAMP(timestamp); + << ", timestamp = " << FMT_TIMESTAMP(timestamp) + << ", point_number = " << cloud->size(); slam->Run(timestamp, cloud); } \ No newline at end of file diff --git a/oh_my_loam/mapper/mapper.cc b/oh_my_loam/mapper/mapper.cc index f1af48a..5dfe535 100644 --- a/oh_my_loam/mapper/mapper.cc +++ b/oh_my_loam/mapper/mapper.cc @@ -113,6 +113,7 @@ void Mapper::Run(const TPointCloudConstPtr &cloud_corn, void Mapper::MatchCorn(const TPointCloudConstPtr &cloud_curr, const common::Pose3d &pose_curr2map, std::vector *const pairs) const { + if (kdtree_corn_.getInputCloud()->empty()) return; std::vector indices; std::vector dists; int nearest_neighbor_k = config_["nearest_neighbor_k"].as(); @@ -137,6 +138,7 @@ void Mapper::MatchCorn(const TPointCloudConstPtr &cloud_curr, void Mapper::MatchSurf(const TPointCloudConstPtr &cloud_curr, const common::Pose3d &pose_curr2map, std::vector *const pairs) const { + if (kdtree_surf_.getInputCloud()->empty()) return; std::vector indices; std::vector dists; int nearest_neighbor_k = config_["nearest_neighbor_k"].as(); diff --git a/oh_my_loam/odometer/odometer.cc b/oh_my_loam/odometer/odometer.cc index 919e494..a65e133 100644 --- a/oh_my_loam/odometer/odometer.cc +++ b/oh_my_loam/odometer/odometer.cc @@ -76,6 +76,7 @@ void Odometer::Process(double timestamp, const std::vector &features, void Odometer::MatchCorn(const TPointCloud &src, std::vector *const pairs) const { + if (kdtree_corn_.getInputCloud()->empty()) return; double dist_sq_thresh = config_["corn_match_dist_sq_th"].as(); for (const auto &pt : src) { TPoint query_pt = TransformToStart(pose_curr2last_, pt); @@ -98,6 +99,7 @@ void Odometer::MatchCorn(const TPointCloud &src, for (int i = i_begin; i < i_end; ++i) { if (i == pt1_scan_id) continue; const auto &kdtree = kdtrees_scan_corn_[i]; + if (kdtree.getInputCloud()->empty()) continue; if (kdtree.nearestKSearch(query_pt, 1, indices, dists) < 1) { continue; } @@ -115,6 +117,7 @@ void Odometer::MatchCorn(const TPointCloud &src, void Odometer::MatchSurf(const TPointCloud &src, std::vector *const pairs) const { + if (kdtree_surf_.getInputCloud()->empty()) return; double dist_sq_thresh = config_["surf_match_dist_sq_th"].as(); for (const auto &pt : src) { TPoint query_pt = TransformToStart(pose_curr2last_, pt); @@ -137,6 +140,7 @@ void Odometer::MatchSurf(const TPointCloud &src, for (int i = i_begin; i < i_end; ++i) { if (i == pt1_scan_id) continue; const auto &kdtree = kdtrees_scan_surf_[i]; + if (kdtree.getInputCloud()->empty()) continue; if (kdtree.nearestKSearch(query_pt, 1, indices, dists) < 1) { continue; } @@ -149,6 +153,7 @@ void Odometer::MatchSurf(const TPointCloud &src, if (!pt2_fount) continue; const auto &kdtree = kdtrees_scan_surf_[pt1_scan_id]; + if (kdtree.getInputCloud()->empty()) continue; if (kdtree.nearestKSearch(query_pt, 2, indices, dists) < 2) continue; if (dists[1] >= dist_sq_thresh) continue; TPoint pt3 = kdtree.getInputCloud()->at(indices[1]); @@ -178,11 +183,15 @@ void Odometer::UpdatePre(const std::vector &features) { } *corn_pre += *scan_corn_pre; *surf_pre += *scan_surf_pre; - kdtrees_scan_corn_[i].setInputCloud(scan_corn_pre); - kdtrees_scan_surf_[i].setInputCloud(scan_surf_pre); + if (!scan_corn_pre->empty()) { + kdtrees_scan_corn_[i].setInputCloud(scan_corn_pre); + } + if (!scan_surf_pre->empty()) { + kdtrees_scan_surf_[i].setInputCloud(scan_surf_pre); + } } - kdtree_corn_.setInputCloud(corn_pre); - kdtree_surf_.setInputCloud(surf_pre); + if (!corn_pre->empty()) kdtree_corn_.setInputCloud(corn_pre); + if (!surf_pre->empty()) kdtree_surf_.setInputCloud(surf_pre); } void Odometer::Visualize(const std::vector &pl_pairs, diff --git a/oh_my_loam/visualizer/ohmyloam_visualizer.cc b/oh_my_loam/visualizer/ohmyloam_visualizer.cc index 5129dc4..2da52f0 100644 --- a/oh_my_loam/visualizer/ohmyloam_visualizer.cc +++ b/oh_my_loam/visualizer/ohmyloam_visualizer.cc @@ -25,7 +25,7 @@ void OhmyloamVisualizer::Draw() { cloud_surf_trans.get()); DrawPointCloud(cloud_surf_trans, "time", "cloud_surf"); traj_.AddPose(frame.pose_map); - AddTrajectory(traj_, PINK, "trajectory", viewer_.get()); + AddTrajectory(traj_, VIOLET, "trajectory", viewer_.get()); } void OhmyloamVisualizer::KeyboardEventCallback(