Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Avoid kdtree build for empty cloud. #82

Merged
merged 3 commits into from
Aug 30, 2017
Merged

Avoid kdtree build for empty cloud. #82

Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
66d7cd2
Avoid kdtree build for empty cloud.
at-wat Aug 30, 2017
9e4ce75
Fix pointer accessor.
at-wat Aug 30, 2017
0d6726d
Fix pc points size usage.
at-wat Aug 30, 2017
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
48 changes: 29 additions & 19 deletions src/mcl_3dl.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -342,6 +342,13 @@ class MCL3dlNode
pcl::PointCloud<pcl::PointXYZI> pc_tmp;
pcl::fromROSMsg(*msg, pc_tmp);

if (pc_tmp.points.size() == 0)
{
ROS_ERROR("Empty map received.");
has_map_ = false;
return;
}

pc_map_.reset(new pcl::PointCloud<pcl::PointXYZI>);
pc_map2_.reset(new pcl::PointCloud<pcl::PointXYZI>);
pc_update_.reset(new pcl::PointCloud<pcl::PointXYZI>);
Expand Down Expand Up @@ -372,7 +379,11 @@ class MCL3dlNode
kdtree_.reset(new pcl::KdTreeFLANN<pcl::PointXYZI>);
kdtree_->setEpsilon(params_.map_grid_min / 2);
kdtree_->setPointRepresentation(boost::make_shared<const MyPointRepresentation>(point_rep_));
kdtree_->setInputCloud(pc_map2_);

if (pc_map2_->points.size() == 0)
kdtree_->setInputCloud(pc_map_);
else
kdtree_->setInputCloud(pc_map2_);
}
void cbMapcloudUpdate(const sensor_msgs::PointCloud2::ConstPtr &msg)
{
Expand Down Expand Up @@ -598,11 +609,9 @@ class MCL3dlNode
return true;
return false;
};
pc_local->points.erase(
std::remove_if(pc_local->points.begin(), pc_local->points.end(), local_points_filter),
pc_local->points.end());
pc_local->width = 1;
pc_local->height = pc_local->points.size();
pc_local->erase(
std::remove_if(pc_local->begin(), pc_local->end(), local_points_filter),
pc_local->end());
pc_local = random_sample(pc_local, static_cast<size_t>(params_.num_points));

pcl::PointCloud<pcl::PointXYZI>::Ptr pc_local_beam(new pcl::PointCloud<pcl::PointXYZI>);
Expand All @@ -619,15 +628,13 @@ class MCL3dlNode
return true;
return false;
};
pc_local_beam->points.erase(
std::remove_if(pc_local_beam->points.begin(), pc_local_beam->points.end(), local_beam_filter),
pc_local_beam->points.end());
pc_local_beam->width = 1;
pc_local_beam->height = pc_local_beam->points.size();
pc_local_beam->erase(
std::remove_if(pc_local_beam->begin(), pc_local_beam->end(), local_beam_filter),
pc_local_beam->end());
pc_local_beam = random_sample(pc_local_beam, static_cast<size_t>(params_.num_points_beam));

assert(pc_local_beam->points.size() == params_.num_points_beam &&
pc_local->points.size() == params_.num_points);
assert(pc_local_beam->points.size() == static_cast<size_t>(params_.num_points_beam) &&
pc_local->points.size() == static_cast<size_t>(params_.num_points));

pcl::PointCloud<pcl::PointXYZI>::Ptr pc_particle(new pcl::PointCloud<pcl::PointXYZI>);
pcl::PointCloud<pcl::PointXYZI>::Ptr pc_particle_beam(new pcl::PointCloud<pcl::PointXYZI>);
Expand Down Expand Up @@ -1230,15 +1237,18 @@ class MCL3dlNode
return true;
return false;
};
pc_map2_->points.erase(
std::remove_if(pc_map2_->points.begin(), pc_map2_->points.end(), pc_map_filter),
pc_map2_->points.end());
pc_map2_->width = 1;
pc_map2_->height = pc_map2_->points.size();
pc_map2_->erase(
std::remove_if(pc_map2_->begin(), pc_map2_->end(), pc_map_filter),
pc_map2_->end());

kdtree_.reset(new pcl::KdTreeFLANN<pcl::PointXYZI>);
kdtree_->setEpsilon(params_.map_grid_min);
kdtree_->setPointRepresentation(boost::make_shared<const MyPointRepresentation>(point_rep_));
kdtree_->setInputCloud(pc_map2_);

if (pc_map2_->points.size() == 0)
kdtree_->setInputCloud(pc_map_);
else
kdtree_->setInputCloud(pc_map2_);

sensor_msgs::PointCloud2 out;
pcl::toROSMsg(*pc_map2_, out);
Expand Down