38 #include <pcl/point_types.h> 54 const float map_grid_x,
const float map_grid_y,
const float map_grid_z)
55 : map_grid_x_(map_grid_x)
56 , map_grid_y_(map_grid_y)
57 , map_grid_z_(map_grid_z)
65 const std::string& name)
69 int num_points, num_points_global;
70 pnh.
param(
"num_points", num_points, 3);
71 pnh.
param(
"num_points_global", num_points_global, 0);
75 double clip_near, clip_far;
76 pnh.
param(
"clip_near", clip_near, 0.5);
77 pnh.
param(
"clip_far", clip_far, 4.0);
81 double clip_z_min, clip_z_max;
82 pnh.
param(
"clip_z_min", clip_z_min, -2.0);
83 pnh.
param(
"clip_z_max", clip_z_max, 2.0);
87 double beam_likelihood_min;
88 pnh.
param(
"beam_likelihood", beam_likelihood_min, 0.2);
90 beam_likelihood_ = std::pow(beam_likelihood_min, 1.0 / static_cast<float>(num_points));
93 pnh.
param(
"ang_total_ref", ang_total_ref, M_PI / 6.0);
97 pnh.
param(
"filter_label_max", filter_label_max, static_cast<int>(0xFFFFFFFF));
102 pnh.
param(
"hit_range", hit_range, 0.3);
104 bool use_raycast_using_dda;
105 pnh.
param(
"use_raycast_using_dda", use_raycast_using_dda,
false);
106 if (use_raycast_using_dda)
108 double ray_angle_half;
109 pnh.
param(
"ray_angle_half", ray_angle_half, 0.25 * M_PI / 180.0);
110 double dda_grid_size;
111 pnh.
param(
"dda_grid_size", dda_grid_size, 0.2);
113 if (dda_grid_size < grid_size_max)
115 ROS_WARN(
"dda_grid_size must be larger than grid size. New value: %f", grid_size_max);
116 dda_grid_size = grid_size_max;
119 raycaster_ = std::make_shared<RaycastUsingDDA<PointType>>(
124 raycaster_ = std::make_shared<RaycastUsingKDTree<PointType>>(
130 const size_t num_particles,
131 const size_t current_num_particles)
133 if (current_num_particles <= num_particles)
145 typename pcl::PointCloud<LidarMeasurementModelBase::PointType>::Ptr
147 const typename pcl::PointCloud<LidarMeasurementModelBase::PointType>::ConstPtr& pc)
const 159 pcl::PointCloud<LidarMeasurementModelBase::PointType>::Ptr pc_filtered(
160 new pcl::PointCloud<LidarMeasurementModelBase::PointType>);
163 std::remove_if(pc_filtered->begin(), pc_filtered->end(), local_points_filter), pc_filtered->end());
164 pc_filtered->width = 1;
165 pc_filtered->height = pc_filtered->points.size();
172 const typename pcl::PointCloud<LidarMeasurementModelBase::PointType>::ConstPtr& pc,
173 const std::vector<Vec3>& origins,
180 pcl::PointCloud<LidarMeasurementModelBase::PointType>::Ptr pc_particle(
181 new pcl::PointCloud<LidarMeasurementModelBase::PointType>);
183 float score_beam = 1.0;
186 for (
auto& p : pc_particle->points)
188 const int beam_header_id = p.label;
205 const Vec3& lidar_pos,
206 const Vec3& scan_pos,
209 raycaster_->setRay(kdtree, lidar_pos, scan_pos);
219 const float distance_from_point_sq = std::pow(scan_pos[0] - result.
point_->x, 2) +
220 std::pow(scan_pos[1] - result.
point_->y, 2) +
221 std::pow(scan_pos[2] - result.
point_->z, 2);
float beam_likelihood_min_
void transform(pcl::PointCloud< PointType > &pc) const
bool add_penalty_short_only_mode_
uint32_t filter_label_max_
pcl::PointCloud< PointType >::Ptr filter(const pcl::PointCloud< PointType >::ConstPtr &pc) const
std::shared_ptr< ChunkedKdtree > Ptr
std::shared_ptr< Raycast< LidarMeasurementModelBase::PointType > > raycaster_
const POINT_TYPE * point_
LidarMeasurementResult measure(ChunkedKdtree< PointType >::Ptr &kdtree, const pcl::PointCloud< PointType >::ConstPtr &pc, const std::vector< Vec3 > &origins, const State6DOF &s) const
std::shared_ptr< SamplerType > sampler_
BeamStatus getBeamStatus(ChunkedKdtree< PointType >::Ptr &kdtree, const Vec3 &beam_begin, const Vec3 &beam_end, typename mcl_3dl::Raycast< PointType >::CastResult &result) const
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val) const
size_t num_points_default_
void setGlobalLocalizationStatus(const size_t num_particles, const size_t current_num_particles)
void loadConfig(const ros::NodeHandle &nh, const std::string &name)
LidarMeasurementModelBeam(const float grid_size_x, const float grid_size_y, const float grid_size_z)
size_t num_points_global_