Elaboradar 0.1
Caricamento in corso...
Ricerca in corso...
Nessun risultato
cart.cpp
1#include <radarelab/cart.h>
2#include <cmath>
3
4namespace radarelab {
5
7 : beam_size(beam_size),
8 map_azimuth(beam_size * 2, beam_size * 2),
9 map_range(beam_size * 2, beam_size * 2)
10{
11 for (unsigned y = 0; y < beam_size * 2; ++y)
12 for (unsigned x = 0; x < beam_size * 2; ++x)
13 {
14 // x and y centered on the map center
15 double absx = (double)x - beam_size;
16 double absy = (double)y - beam_size;
17 //absx += (absx < 0) ? -0.5 : 0.5;
18 //absy += (absy < 0) ? -0.5 : 0.5;
19 // x and y (referred to map center) centered on the pixel center
20 absx += 0.5;
21 absy += 0.5;
22
23 // Compute range
24 map_range(y, x) = hypot(absx, absy);
25
26 // Compute azimuth
27 double az;
28 /* if (absx > 0)
29 if (absy < 0)
30 az = atan(absx / -absy) * M_1_PI * 180.;
31 else
32 az = 90.0 + atan(absy / absx) * M_1_PI * 180.;
33 else
34 if (absy > 0)
35 az = 180 + atan(-absx / absy) * M_1_PI * 180.;
36 else
37 az = 270 + atan(-absy / -absx) * M_1_PI * 180.;
38 map_azimuth(y, x) = az;
39 */
40 map_azimuth(y,x)= fmod (450 - atan2( -absy,absx)*M_1_PI*180., 360. );
41 }
42}
43
44void CoordinateMapping::sample(unsigned beam_count, unsigned x, unsigned y, std::function<void(unsigned, unsigned)>& f) const
45{
46 // Map cartesian coordinates to angles and distances
47 unsigned range_idx = floor(map_range(y, x));
48 if (range_idx >= beam_size) return;
49
50 // Exact angle
51 double az = map_azimuth(y, x);
52
53#if 0
54 // Iterate indices 0.45° before and after
55 int az_min = floor((az - .45) * beam_count / 360.0);
56 int az_max = ceil((az + .45) * beam_count / 360.0);
57 if (az_min < 0)
58 {
59 az_min += beam_count;
60 az_max += beam_count;
61 }
62#endif
63
64 // Iterate on angles that actually overlap with the map cell
65 double d_az = M_1_PI * 180. / (range_idx + 0.5) / 2;
66 int az_min = round((az - d_az) * beam_count / 360.);
67 int az_max = round((az + d_az) * beam_count / 360.);
68
69 // Iterate all points between az_min and az_max
70 for (int iaz = az_min; iaz <= az_max; ++iaz)
71 f((unsigned)((iaz+beam_count) % beam_count), range_idx);
72}
73
74const unsigned IndexMapping::missing;
75
76IndexMapping::IndexMapping(unsigned height, unsigned width)
77 : height(height), width(width),
78 map_azimuth(Matrix2D<unsigned>::Constant(height, width, missing)),
79 map_range(Matrix2D<unsigned>::Constant(height, width, missing))
80{
81}
82
83FullsizeIndexMapping::FullsizeIndexMapping(unsigned beam_size)
84 : IndexMapping(beam_size * 2, beam_size * 2)
85{
86}
87
89{
90 CoordinateMapping raw_mapping(scan.beam_size);
91 map_max_sample(scan, raw_mapping);
92}
93
95{
96 for (unsigned y = 0; y < height; ++y)
97 for (unsigned x = 0; x < width; ++x)
98 {
99 bool first = true;
100 double maxval;
101 unsigned maxval_azimuth;
102 unsigned maxval_range;
103 std::function<void(unsigned, unsigned)> f = [&](unsigned azimuth, unsigned range) {
104 double sample = scan.get(azimuth, range);
105 if (first || sample > maxval)
106 {
107 maxval = sample;
108 maxval_azimuth = azimuth;
109 maxval_range = range;
110 first = false;
111 }
112 };
113 mapping.sample(scan.beam_count, x, y, f);
114
115 // If nothing has been found, skip this sample
116 if (first) continue;
117
118 // Store the indices for this mapping
119 map_azimuth(y, x) = maxval_azimuth;
120 map_range(y, x) = maxval_range;
121 }
122}
123
124
125ScaledIndexMapping::ScaledIndexMapping(const CoordinateMapping& mapping, unsigned image_side, unsigned fullsize_pixels_per_scaled_pixel)
126 : IndexMapping(image_side, image_side), mapping(mapping),
127 fullsize_pixels_per_scaled_pixel(fullsize_pixels_per_scaled_pixel),
128 image_offset(((int)mapping.beam_size * 2 - (int)image_side * (int)fullsize_pixels_per_scaled_pixel) / 2)
129{
130 if ((image_side * fullsize_pixels_per_scaled_pixel) % 2 != 0)
131 throw std::runtime_error("the image cannot be properly centered on the full size image");
132}
133
134void ScaledIndexMapping::sample(unsigned beam_count, unsigned x, unsigned y, std::function<void(unsigned, unsigned)>& f)
135{
136 // Load each sample with a value from a 4x4 window on the original image
137 for(unsigned sy = 0; sy < fullsize_pixels_per_scaled_pixel; ++sy)
138 for(unsigned sx = 0; sx < fullsize_pixels_per_scaled_pixel; ++sx)
139 {
140 // Use the full size mapping to get the volume value at this point
141 int src_x = x * fullsize_pixels_per_scaled_pixel + sx + image_offset;
142 int src_y = y * fullsize_pixels_per_scaled_pixel + sy + image_offset;
143 if (src_x < 0 || src_x >= mapping.beam_size || src_y < 0 || src_y >= mapping.beam_size) continue;
144
145 // Generate all the azimuth/range elements for this point in the
146 // full size map
147 std::function<void(unsigned, unsigned)> fullsize_sample = [&f](unsigned azimuth, unsigned range) {
148 f(azimuth, range);
149 };
150 mapping.sample(beam_count, src_x, src_y, fullsize_sample);
151 }
152}
153
155{
156 // ciclo sui punti della nuova matrice. per il primo prenderò il massimo tra i primi sedici etc..
157 for (unsigned y = 0; y < height; ++y)
158 for (unsigned x = 0; x < width; ++x)
159 {
160 bool first = true;
161 double maxval;
162 unsigned maxval_az = missing;
163 unsigned maxval_range = missing;
164
165 // Load each sample with a value from a 4x4 window on the original image
166 for(unsigned sy = 0; sy < fullsize_pixels_per_scaled_pixel; ++sy)
167 for(unsigned sx = 0; sx < fullsize_pixels_per_scaled_pixel; ++sx)
168 {
169 // Use the full size mapping to get the volume value at this point
170 int src_x = x * fullsize_pixels_per_scaled_pixel + sx + image_offset;
171 int src_y = y * fullsize_pixels_per_scaled_pixel + sy + image_offset;
172 if (src_x < 0 || src_x >= mapping.width || src_y < 0 || src_y >= mapping.height) continue;
173 unsigned range = mapping.map_range(src_y, src_x);
174 if (range == missing) continue;
175 unsigned az = mapping.map_azimuth(src_y, src_x);
176 double sample = scan.get(az, range);
177
178 // Find the source point with the maximum sample
179 if (first || sample > maxval)
180 {
181 maxval = sample;
182 maxval_az = az;
183 maxval_range = range;
184 first = false;
185 }
186 }
187
188 // If nothing has been found, skip this sample
189 if (first) continue;
190
191 // Store the indices for this mapping
192 map_azimuth(y, x) = maxval_az;
193 map_range(y, x) = maxval_range;
194 }
195}
196
197}
static const unsigned missing
Missing value in the azimuth and range index mappings.
Definition cart.h:66
Matrix2D< unsigned > map_range
Range indices to use to lookup a map point in a volume -1 means no mapping.
Definition cart.h:76
Matrix2D< unsigned > map_azimuth
Azimuth indices to use to lookup a map point in a volume -1 means no mapping.
Definition cart.h:73
Mapping of cartesian coordinates to specific azimuth and range volume indices.
Definition cart.h:63
T get(unsigned az, unsigned beam) const
Get a beam value.
Definition volume.h:161
PolarScan - structure to describe a polarScan containing a matrix of data and conversion factors.
Definition volume.h:113
String functions.
Definition cart.cpp:4
const unsigned beam_size
Beam size of the volume that we are mapping to cartesian coordinates.
Definition cart.h:24
CoordinateMapping(unsigned beam_size)
Build a cartography mapping cartesian coordinates to volume polar indices.
Definition cart.cpp:6
Matrix2D< double > map_range
Range indices to use to lookup a map point in a volume.
Definition cart.h:44
void sample(unsigned beam_count, unsigned x, unsigned y, std::function< void(unsigned, unsigned)> &f) const
Generate all the (azimuth, range) indices corresponding to a map point.
Definition cart.cpp:44
Matrix2D< double > map_azimuth
Azimuth indices to use to lookup a map point in a volume.
Definition cart.h:34
Mapping of cartesian coordinates to raw azimuth angles and range distances.
Definition cart.h:22
void map_max_sample(const PolarScan< double > &scan)
Map cartesian cardinates to polar volume indices.
Definition cart.cpp:88
Index mapping where the pixel size corresponds to the radar cell size.
Definition cart.h:138
Base for all matrices we use, since we rely on row-major data.
Definition matrix.h:37
unsigned beam_size
Number of samples in each beam.
Definition volume.h:33
unsigned beam_count
Count of beams in this scan.
Definition volume.h:30
int image_offset
Image offset in full size pixels.
Definition cart.h:167
void map_max_sample(const PolarScan< double > &scan, const FullsizeIndexMapping &mapping)
Map cartesian cardinates to polar volume indices.
Definition cart.cpp:154
void sample(unsigned beam_count, unsigned x, unsigned y, std::function< void(unsigned, unsigned)> &f)
Generate all the (azimuth, range) indices corresponding to a map point.
Definition cart.cpp:134