| 33 | }; |
| 34 | |
| 35 | class ConcentricZoneModel : public ZoneModel { |
| 36 | public: |
| 37 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW |
| 38 | ConcentricZoneModel() {} |
| 39 | |
| 40 | ConcentricZoneModel(const std::string &sensor_model, |
| 41 | const double sensor_height, |
| 42 | const float min_range, |
| 43 | const float max_range) { |
| 44 | sensor_config_ = SensorConfig(sensor_model); |
| 45 | num_zones_ = sensor_config_.num_laser_channels_per_zone_.size(); |
| 46 | max_ring_index_in_first_zone = sensor_config_.num_laser_channels_per_zone_[0].size(); |
| 47 | |
| 48 | sensor_height_ = sensor_height; |
| 49 | min_range_ = min_range; |
| 50 | max_range_ = max_range; |
| 51 | |
| 52 | sqr_min_range_ = min_range * min_range; |
| 53 | sqr_max_range_ = max_range * max_range; |
| 54 | |
| 55 | set_concentric_zone_model(); |
| 56 | } |
| 57 | bool is_range_boundary_set_; |
| 58 | size_t max_ring_index_in_first_zone; |
| 59 | size_t num_zones_; |
| 60 | size_t num_total_rings_; |
| 61 | double sensor_height_; |
| 62 | float min_range_; |
| 63 | float max_range_; |
| 64 | float sqr_min_range_; |
| 65 | float sqr_max_range_; |
| 66 | |
| 67 | vector<int> num_sectors_per_ring_; |
| 68 | // sqr: For reducing computational complexity |
| 69 | vector<float> sqr_boundary_ranges_; |
| 70 | // For visualization |
| 71 | vector<float> boundary_ranges_; |
| 72 | vector<float> boundary_ratios_; |
| 73 | |
| 74 | ~ConcentricZoneModel() {} |
| 75 | |
| 76 | inline void set_concentric_zone_model() { |
| 77 | set_num_sectors_for_each_ring(); |
| 78 | // Seting ring boundaries to consider # of laser rings |
| 79 | float smallest_incidence_angle = 90.0 + sensor_config_.lower_fov_boundary_; |
| 80 | // For defensive programming |
| 81 | if (tan(DEG2RAD(smallest_incidence_angle)) * sensor_height_ < min_range_) { |
| 82 | cout << tan(DEG2RAD(smallest_incidence_angle)) * sensor_height_ << " vs " << min_range_ |
| 83 | << endl; |
| 84 | throw invalid_argument( |
| 85 | "\033[1;31m[CZM] The parameter `min_r` is wrong. " |
| 86 | "Check your sensor height or min. range\033[0m"); |
| 87 | } |
| 88 | sanity_check(); |
| 89 | set_sqr_boundary_ranges(sensor_height_, smallest_incidence_angle); |
| 90 | } |
| 91 | |
| 92 | inline void sanity_check() { |