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

fix: kdtree setInputCloud with empty cloud

main
feixyz10 2021-02-18 14:05:33 +08:00 committed by feixyz
parent 081db82478
commit caf01fb59f
5 changed files with 22 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
common/visualizer/lidar_visualizer_utils.cc
View File

@ -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<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);
}
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<Point>(cloud, color, id, viewer, 5);
}
} // namespace common

3
main.cc
View File

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

2
oh_my_loam/mapper/mapper.cc
View File

@ -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<PointLineCoeffPair> *const pairs) const {
if (kdtree_corn_.getInputCloud()->empty()) return;
std::vector<int> indices;
std::vector<float> dists;
int nearest_neighbor_k = config_["nearest_neighbor_k"].as<int>();
@ -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<PointPlaneCoeffPair> *const pairs) const {
if (kdtree_surf_.getInputCloud()->empty()) return;
std::vector<int> indices;
std::vector<float> dists;
int nearest_neighbor_k = config_["nearest_neighbor_k"].as<int>();

17
oh_my_loam/odometer/odometer.cc
View File

@ -76,6 +76,7 @@ void Odometer::Process(double timestamp, const std::vector<Feature> &features,
void Odometer::MatchCorn(const TPointCloud &src,
std::vector<PointLinePair> *const pairs) const {
if (kdtree_corn_.getInputCloud()->empty()) return;
double dist_sq_thresh = config_["corn_match_dist_sq_th"].as<double>();
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<PointPlanePair> *const pairs) const {
if (kdtree_surf_.getInputCloud()->empty()) return;
double dist_sq_thresh = config_["surf_match_dist_sq_th"].as<double>();
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<Feature> &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<PointLinePair> &pl_pairs,

2
oh_my_loam/visualizer/ohmyloam_visualizer.cc
View File

@ -25,7 +25,7 @@ void OhmyloamVisualizer::Draw() {
cloud_surf_trans.get());
DrawPointCloud<TPoint>(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(