8namespace azimuthresample {
12inline double angle_distance(
double first,
double second)
14 return 180.0 - std::fabs(std::fmod(std::fabs(first - second), 360.0) - 180.0);
17inline std::pair<double, unsigned> closest_of_two(
double azimuth,
18 const std::pair<double, unsigned>& i1,
19 const std::pair<double, unsigned>& i2)
21 if (angle_distance(i1.first, azimuth) < angle_distance(i2.first, azimuth))
32 for (
unsigned i = 0; i < azimuths.size(); ++i)
34 by_angle.insert(make_pair(azimuths(i), i));
50 auto i =
by_angle.lower_bound(azimuth);
58 if (i->first == azimuth)
62 std::map<double, unsigned>::const_iterator prev = i;
64 return closest_of_two(azimuth, *prev, *i);
71 double my_semi_amplitude = src_amplitude / 2.0;
73 static const double precision = 0.000000001;
75 double lowest_azimuth = dst_azimuth - dst_amplitude / 2 - my_semi_amplitude + precision;
76 while (lowest_azimuth < 0) lowest_azimuth += 360;
77 lowest_azimuth = fmod(lowest_azimuth, 360);
78 double highest_azimuth = dst_azimuth + dst_amplitude / 2 + my_semi_amplitude - precision;
79 while (highest_azimuth < 0) highest_azimuth += 360;
80 highest_azimuth = fmod(highest_azimuth, 360);
82 std::vector<pair<double, unsigned>> res;
84 if (lowest_azimuth <= highest_azimuth)
86 auto begin =
by_angle.lower_bound(lowest_azimuth);
87 auto end =
by_angle.lower_bound(highest_azimuth);
88 for (
auto i = begin; i != end; ++i)
91 auto begin =
by_angle.upper_bound(lowest_azimuth);
92 auto end =
by_angle.lower_bound(highest_azimuth);
93 for (
auto i = begin; i !=
by_angle.end(); ++i)
95 for (
auto i =
by_angle.begin(); i != end; ++i)
108 double max_distance = src_beam_width/2 + 360.0 / dst.
beam_count / 2;
111 for (
unsigned dst_idx = 0; dst_idx < dst.
beam_count; ++dst_idx)
113 double dst_azimuth = (double)dst_idx / (
double)dst.
beam_count * 360.0;
115 pair<double, unsigned> pos = index.closest(dst_azimuth);
117 if (angle_distance(dst_azimuth, pos.first) > max_distance)
132 dst.row(dst_idx) = src.row(pos.second);
139void MaxOfClosest<T>::resample_polarscan(
const PolarScan<T>& src, PolarScan<T>& dst,
double src_beam_width)
const
143 double max_distance = src_beam_width/2 + 360.0 / dst.beam_count / 2;
146 for (
unsigned dst_idx = 0; dst_idx < dst.beam_count; ++dst_idx)
148 double dst_azimuth = (double)dst_idx / (
double)dst.beam_count * 360.0;
149 vector<pair<double, unsigned>> positions = index.intersecting(dst_azimuth, 360.0 / dst.beam_count, src_beam_width);
150 if (positions.empty())
153 dst.elevations_real(dst_idx) = src.elevation;
156 dst.azimuths_real(dst_idx) = dst_azimuth;
162 auto i = positions.cbegin();
163 dst.row(dst_idx) = src.row(i->second);
164 el_sum += src.elevations_real(i->second);
165 az_sum += src.azimuths_real(i->second);
168 for (++i; i != positions.end(); ++i)
170 for (
unsigned bi = 0; bi < dst.beam_size; ++bi)
171 if (dst(dst_idx, bi) < src(i->second, bi))
172 dst(dst_idx, bi) = src(i->second, bi);
173 el_sum += src.elevations_real(i->second);
174 az_sum += src.azimuths_real(i->second);
178 dst.elevations_real(dst_idx) = el_sum / positions.size();
181 dst.azimuths_real(dst_idx) = az_sum / positions.size();
186template class Closest<double>;
187template class Closest<unsigned char>;
188template class MaxOfClosest<double>;
189template class MaxOfClosest<unsigned char>;
PolarScan - structure to describe a polarScan containing a matrix of data and conversion factors.
std::pair< double, unsigned > closest(double azimuth) const
Get the closest position to an azimuth angle.
std::vector< std::pair< double, unsigned > > intersecting(double dst_azimuth, double dst_amplitude, double src_amplitude) const
Get all the positions intersecting an angle centered on azimuth and with the given amplitude.
std::map< double, unsigned > by_angle
map azimuth angles to beam indices
AzimuthIndex(const Eigen::VectorXd &azimuths)
Build an index with the azimuths of a PolarScan.
Classe to manage Index beam positions by azimuth angles.
double elevation
Nominal elevation of this PolarScan, which may be different from the effective elevation of each sing...
Eigen::VectorXd elevations_real
Vector of actual elevations for each beam.
unsigned beam_count
Count of beams in this scan.
Eigen::VectorXd azimuths_real
Vector of actual azimuths for each beam.