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