37 #include <pcl/point_types.h> 51 const std::string& name)
55 int num_points, num_points_global;
56 pnh.
param(
"num_points", num_points, 96);
57 pnh.
param(
"num_points_global", num_points_global, 8);
61 double clip_near, clip_far;
62 pnh.
param(
"clip_near", clip_near, 0.5);
63 pnh.
param(
"clip_far", clip_far, 10.0);
67 double clip_z_min, clip_z_max;
68 pnh.
param(
"clip_z_min", clip_z_min, -2.0);
69 pnh.
param(
"clip_z_max", clip_z_max, 2.0);
74 pnh.
param(
"match_weight", match_weight, 5.0);
77 double match_dist_min, match_dist_flat;
78 pnh.
param(
"match_dist_min", match_dist_min, 0.2);
79 pnh.
param(
"match_dist_flat", match_dist_flat, 0.05);
84 const size_t num_particles,
85 const size_t current_num_particles)
87 if (current_num_particles <= num_particles)
99 typename pcl::PointCloud<LidarMeasurementModelBase::PointType>::Ptr
101 const typename pcl::PointCloud<LidarMeasurementModelBase::PointType>::ConstPtr& pc)
const 113 pcl::PointCloud<LidarMeasurementModelBase::PointType>::Ptr pc_filtered(
114 new pcl::PointCloud<LidarMeasurementModelBase::PointType>);
117 std::remove_if(pc_filtered->begin(), pc_filtered->end(), local_points_filter), pc_filtered->end());
118 pc_filtered->width = 1;
119 pc_filtered->height = pc_filtered->points.size();
126 const typename pcl::PointCloud<LidarMeasurementModelBase::PointType>::ConstPtr& pc,
127 const std::vector<Vec3>& origins,
134 pcl::PointCloud<LidarMeasurementModelBase::PointType>::Ptr pc_particle(
135 new pcl::PointCloud<LidarMeasurementModelBase::PointType>);
136 std::vector<int> id(1);
137 std::vector<float> sqdist(1);
139 float score_like = 0;
143 for (
auto& p : pc_particle->points)
155 const float match_ratio =
static_cast<float>(num) / pc_particle->points.size();
LidarMeasurementResult measure(ChunkedKdtree< PointType >::Ptr &kdtree, const pcl::PointCloud< PointType >::ConstPtr &pc, const std::vector< Vec3 > &origins, const State6DOF &s) const
void transform(pcl::PointCloud< PointType > &pc) const
std::shared_ptr< ChunkedKdtree > Ptr
void setGlobalLocalizationStatus(const size_t num_particles, const size_t current_num_particles)
size_t num_points_global_
std::shared_ptr< SamplerType > sampler_
pcl::PointCloud< PointType >::Ptr filter(const pcl::PointCloud< PointType >::ConstPtr &pc) const
void loadConfig(const ros::NodeHandle &nh, const std::string &name)
int radiusSearch(const POINT_TYPE &p, const float &radius, std::vector< int > &id, std::vector< float > &dist_sq, const size_t &num)
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val) const
size_t num_points_default_