8 namespace azimuthresample {
12 inline double angle_distance(
double first,
double second)
14 return 180.0 - std::fabs(std::fmod(std::fabs(first - second), 360.0) - 180.0);
17 inline 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))
30 AzimuthIndex::AzimuthIndex(
const Eigen::VectorXd& azimuths)
32 for (
unsigned i = 0; i < azimuths.size(); ++i)
34 by_angle.insert(make_pair(azimuths(i), i));
48 pair<double, unsigned> AzimuthIndex::closest(
double azimuth)
const
50 auto i = by_angle.lower_bound(azimuth);
54 if (i == by_angle.end() || i == by_angle.begin())
55 return closest_of_two(azimuth, *by_angle.rbegin(), *by_angle.begin());
58 if (i->first == azimuth)
62 std::map<double, unsigned>::const_iterator prev = i;
64 return closest_of_two(azimuth, *prev, *i);
67 std::vector<pair<double, unsigned>> AzimuthIndex::intersecting(
double dst_azimuth,
double dst_amplitude,
double src_amplitude)
const
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);
139 void MaxOfClosest<T>::resample_polarscan(
const PolarScan<T>& src, PolarScan<T>& dst,
double src_beam_width)
const
141 AzimuthIndex index(src.azimuths_real);
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();
186 template class Closest<double>;
187 template class Closest<unsigned char>;
188 template class MaxOfClosest<double>;
189 template class MaxOfClosest<unsigned char>;
PolarScan - structure to describe a polarScan containing a matrix of data and conversion factors.
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.