Elaboradar  0.1
 Tutto Classi Namespace File Funzioni Variabili Tipi enumerati (enum) Gruppi
azimuth_resample.cpp
1 #include "azimuth_resample.h"
2 #include <cmath>
3 
4 using namespace std;
5 
6 namespace radarelab {
7 namespace algo {
8 namespace azimuthresample {
9 
10 namespace {
11 
12 inline double angle_distance(double first, double second)
13 {
14  return 180.0 - std::fabs(std::fmod(std::fabs(first - second), 360.0) - 180.0);
15 }
16 
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)
20 {
21  if (angle_distance(i1.first, azimuth) < angle_distance(i2.first, azimuth))
22  return i1;
23  else
24  return i2;
25 }
26 
27 }
28 
29 
30 AzimuthIndex::AzimuthIndex(const Eigen::VectorXd& azimuths)
31 {
32  for (unsigned i = 0; i < azimuths.size(); ++i)
33  {
34  by_angle.insert(make_pair(azimuths(i), i));
35  /*
36  map<double, unsigned>::iterator old;
37  bool inserted;
38  tie(old, inserted) = by_angle.insert(make_pair(azimuths(i), i));
39  if (!inserted)
40  {
41  fprintf(stderr, "IDX new %u old %u, val %f\n", i, old->second, old->first);
42  throw std::runtime_error("source PolarScan has two beams with the same azimuth");
43  }
44  */
45  }
46 }
47 
48 pair<double, unsigned> AzimuthIndex::closest(double azimuth) const
49 {
50  auto i = by_angle.lower_bound(azimuth);
51 
52  // Result between the end and the beginning: assume it falls between
53  // first and last
54  if (i == by_angle.end() || i == by_angle.begin())
55  return closest_of_two(azimuth, *by_angle.rbegin(), *by_angle.begin());
56 
57  // Exact match: return the Position
58  if (i->first == azimuth)
59  return *i;
60 
61  // Return the closest between the previous element and this one
62  std::map<double, unsigned>::const_iterator prev = i;
63  --prev;
64  return closest_of_two(azimuth, *prev, *i);
65 }
66 
67 std::vector<pair<double, unsigned>> AzimuthIndex::intersecting(double dst_azimuth, double dst_amplitude, double src_amplitude) const
68 {
69  // Compute the amplitude between our beams assuming the angles we have are
70  // close to evenly spaced
71  double my_semi_amplitude = src_amplitude / 2.0;
72  // Angles closer than this amount are considered the same for overlap detection
73  static const double precision = 0.000000001;
74 
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);
81 
82  std::vector<pair<double, unsigned>> res;
83 
84  if (lowest_azimuth <= highest_azimuth)
85  {
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)
89  res.push_back(*i);
90  } else {
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)
94  res.push_back(*i);
95  for (auto i = by_angle.begin(); i != end; ++i)
96  res.push_back(*i);
97  }
98 
99  return res;
100 }
101 
102 
103 template<typename T>
104 void Closest<T>::resample_polarscan(const PolarScan<T>& src, PolarScan<T>& dst, double src_beam_width) const
105 {
106  AzimuthIndex index(src.azimuths_real);
107 
108  double max_distance = src_beam_width/2 + 360.0 / dst.beam_count / 2;
109 
110  // Do the merge
111  for (unsigned dst_idx = 0; dst_idx < dst.beam_count; ++dst_idx)
112  {
113  double dst_azimuth = (double)dst_idx / (double)dst.beam_count * 360.0;
114 
115  pair<double, unsigned> pos = index.closest(dst_azimuth);
116 
117  if (angle_distance(dst_azimuth, pos.first) > max_distance)
118  {
120  dst.elevations_real(dst_idx) = src.elevation;
121 
123  dst.azimuths_real(dst_idx) = dst_azimuth;
124  } else {
126  dst.elevations_real(dst_idx) = src.elevations_real(pos.second);
127 
129  dst.azimuths_real(dst_idx) = pos.first;
130 
132  dst.row(dst_idx) = src.row(pos.second);
133  }
134  }
135 }
136 
137 
138 template<typename T>
139 void MaxOfClosest<T>::resample_polarscan(const PolarScan<T>& src, PolarScan<T>& dst, double src_beam_width) const
140 {
141  AzimuthIndex index(src.azimuths_real);
142 
143  double max_distance = src_beam_width/2 + 360.0 / dst.beam_count / 2;
144 
145  // Do the merge
146  for (unsigned dst_idx = 0; dst_idx < dst.beam_count; ++dst_idx)
147  {
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())
151  {
153  dst.elevations_real(dst_idx) = src.elevation;
154 
156  dst.azimuths_real(dst_idx) = dst_azimuth;
157  } else {
158  double el_sum = 0;
159  double az_sum = 0;
160 
161  // Copy the first beam
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);
166 
167  // Take the maximum of all beam values
168  for (++i; i != positions.end(); ++i)
169  {
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);
175  }
176 
177  // The real elevation of this beam is the average of the beams we used
178  dst.elevations_real(dst_idx) = el_sum / positions.size();
179 
180  // The real azimuth of this beam is the average of the azimuths we used
181  dst.azimuths_real(dst_idx) = az_sum / positions.size();
182  }
183  }
184 }
185 
186 template class Closest<double>;
187 template class Closest<unsigned char>;
188 template class MaxOfClosest<double>;
189 template class MaxOfClosest<unsigned char>;
190 
191 }
192 }
193 }
194 
Eigen::VectorXd elevations_real
Vector of actual elevations for each beam.
Definition: volume.h:45
PolarScan - structure to describe a polarScan containing a matrix of data and conversion factors...
Definition: volume.h:112
unsigned beam_count
Count of beams in this scan.
Definition: volume.h:30
Classe to manage Index beam positions by azimuth angles.
Eigen::VectorXd azimuths_real
Vector of actual azimuths for each beam.
Definition: volume.h:36
double elevation
Nominal elevation of this PolarScan, which may be different from the effective elevation of each sing...
Definition: volume.h:42