| 11 | } |
| 12 | |
| 13 | void SDFMap::initMap(ros::NodeHandle& nh) { |
| 14 | mp_.reset(new MapParam); |
| 15 | md_.reset(new MapData); |
| 16 | mr_.reset(new MapROS); |
| 17 | mm_.reset(new MultiMapManager); |
| 18 | |
| 19 | // Params of map properties |
| 20 | double x_size, y_size, z_size; |
| 21 | nh.param("sdf_map/resolution", mp_->resolution_, -1.0); |
| 22 | nh.param("sdf_map/map_size_x", x_size, -1.0); |
| 23 | nh.param("sdf_map/map_size_y", y_size, -1.0); |
| 24 | nh.param("sdf_map/map_size_z", z_size, -1.0); |
| 25 | nh.param("sdf_map/obstacles_inflation", mp_->obstacles_inflation_, -1.0); |
| 26 | nh.param("sdf_map/local_bound_inflate", mp_->local_bound_inflate_, 1.0); |
| 27 | nh.param("sdf_map/local_map_margin", mp_->local_map_margin_, 1); |
| 28 | nh.param("sdf_map/ground_height", mp_->ground_height_, 1.0); |
| 29 | nh.param("sdf_map/default_dist", mp_->default_dist_, 5.0); |
| 30 | nh.param("sdf_map/optimistic", mp_->optimistic_, true); |
| 31 | nh.param("sdf_map/signed_dist", mp_->signed_dist_, false); |
| 32 | nh.param("sdf_map/no_drone_1", mp_->no_drone_1_, false); |
| 33 | |
| 34 | mp_->local_bound_inflate_ = max(mp_->resolution_, mp_->local_bound_inflate_); |
| 35 | mp_->resolution_inv_ = 1 / mp_->resolution_; |
| 36 | mp_->map_origin_ = Eigen::Vector3d(-x_size / 2.0, -y_size / 2.0, mp_->ground_height_); |
| 37 | mp_->map_size_ = Eigen::Vector3d(x_size, y_size, z_size); |
| 38 | for (int i = 0; i < 3; ++i) mp_->map_voxel_num_(i) = ceil(mp_->map_size_(i) / mp_->resolution_); |
| 39 | mp_->map_min_boundary_ = mp_->map_origin_; |
| 40 | mp_->map_max_boundary_ = mp_->map_origin_ + mp_->map_size_; |
| 41 | |
| 42 | // Params of raycasting-based fusion |
| 43 | nh.param("sdf_map/p_hit", mp_->p_hit_, 0.70); |
| 44 | nh.param("sdf_map/p_miss", mp_->p_miss_, 0.35); |
| 45 | nh.param("sdf_map/p_min", mp_->p_min_, 0.12); |
| 46 | nh.param("sdf_map/p_max", mp_->p_max_, 0.97); |
| 47 | nh.param("sdf_map/p_occ", mp_->p_occ_, 0.80); |
| 48 | nh.param("sdf_map/max_ray_length", mp_->max_ray_length_, -0.1); |
| 49 | nh.param("sdf_map/virtual_ceil_height", mp_->virtual_ceil_height_, -0.1); |
| 50 | |
| 51 | auto logit = [](const double& x) { return log(x / (1 - x)); }; |
| 52 | mp_->prob_hit_log_ = logit(mp_->p_hit_); |
| 53 | mp_->prob_miss_log_ = logit(mp_->p_miss_); |
| 54 | mp_->clamp_min_log_ = logit(mp_->p_min_); |
| 55 | mp_->clamp_max_log_ = logit(mp_->p_max_); |
| 56 | mp_->min_occupancy_log_ = logit(mp_->p_occ_); |
| 57 | mp_->unknown_flag_ = 0.01; |
| 58 | cout << "hit: " << mp_->prob_hit_log_ << ", miss: " << mp_->prob_miss_log_ |
| 59 | << ", min: " << mp_->clamp_min_log_ << ", max: " << mp_->clamp_max_log_ |
| 60 | << ", thresh: " << mp_->min_occupancy_log_ << endl; |
| 61 | |
| 62 | // Initialize data buffer of map |
| 63 | int buffer_size = mp_->map_voxel_num_(0) * mp_->map_voxel_num_(1) * mp_->map_voxel_num_(2); |
| 64 | md_->occupancy_buffer_ = vector<double>(buffer_size, mp_->clamp_min_log_ - mp_->unknown_flag_); |
| 65 | md_->occupancy_buffer_inflate_ = vector<char>(buffer_size, 0); |
| 66 | md_->distance_buffer_neg_ = vector<double>(buffer_size, mp_->default_dist_); |
| 67 | md_->distance_buffer_ = vector<double>(buffer_size, mp_->default_dist_); |
| 68 | md_->count_hit_and_miss_ = vector<short>(buffer_size, 0); |
| 69 | md_->count_hit_ = vector<short>(buffer_size, 0); |
| 70 | md_->count_miss_ = vector<short>(buffer_size, 0); |