Elaboradar 0.1
Caricamento in corso...
Ricerca in corso...
Nessun risultato
azimuth_resample.cpp
1#include "azimuth_resample.h"
2#include <cmath>
3
4using namespace std;
5
6namespace radarelab {
7namespace algo {
8namespace azimuthresample {
9
10namespace {
11
12inline 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
17inline 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
30AzimuthIndex::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
48pair<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
67std::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
103template<typename T>
104void 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
138template<typename T>
139void 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
186template class Closest<double>;
187template class Closest<unsigned char>;
188template class MaxOfClosest<double>;
189template class MaxOfClosest<unsigned char>;
190
191}
192}
193}
194
PolarScan - structure to describe a polarScan containing a matrix of data and conversion factors.
Definition: volume.h:113
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.
String functions.
Definition: cart.cpp:4
double elevation
Nominal elevation of this PolarScan, which may be different from the effective elevation of each sing...
Definition: volume.h:42
Eigen::VectorXd elevations_real
Vector of actual elevations for each beam.
Definition: volume.h:45
unsigned beam_count
Count of beams in this scan.
Definition: volume.h:30
Eigen::VectorXd azimuths_real
Vector of actual azimuths for each beam.
Definition: volume.h:36