Elaboradar  0.1
 Tutto Classi Namespace File Funzioni Variabili Tipi enumerati (enum) Gruppi
cart.cpp
1 #include <radarelab/cart.h>
2 #include <cmath>
3 
4 namespace 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 
44 void 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 
74 const unsigned IndexMapping::missing;
75 
76 IndexMapping::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 
83 FullsizeIndexMapping::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 
125 ScaledIndexMapping::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 
134 void 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 }
CoordinateMapping(unsigned beam_size)
Build a cartography mapping cartesian coordinates to volume polar indices.
Definition: cart.cpp:6
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
T get(unsigned az, unsigned beam) const
Get a beam value.
Definition: volume.h:161
const unsigned beam_size
Beam size of the volume that we are mapping to cartesian coordinates.
Definition: cart.h:24
int image_offset
Image offset in full size pixels.
Definition: cart.h:167
Base for all matrices we use, since we rely on row-major data.
Definition: matrix.h:36
unsigned beam_size
Number of samples in each beam.
Definition: volume.h:33
Matrix2D< unsigned > map_range
Range indices to use to lookup a map point in a volume -1 means no mapping.
Definition: cart.h:76
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
static const unsigned missing
Missing value in the azimuth and range index mappings.
Definition: cart.h:66
Mapping of cartesian coordinates to raw azimuth angles and range distances.
Definition: cart.h:21
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_range
Range indices to use to lookup a map point in a volume.
Definition: cart.h:44
Index mapping where the pixel size corresponds to the radar cell size.
Definition: cart.h:137
void map_max_sample(const PolarScan< double > &scan)
Map cartesian cardinates to polar volume indices.
Definition: cart.cpp:88
Mapping of cartesian coordinates to specific azimuth and range volume indices.
Definition: cart.h:62
Matrix2D< unsigned > map_azimuth
Azimuth indices to use to lookup a map point in a volume -1 means no mapping.
Definition: cart.h:73
Matrix2D< double > map_azimuth
Azimuth indices to use to lookup a map point in a volume.
Definition: cart.h:34
void map_max_sample(const PolarScan< double > &scan, const FullsizeIndexMapping &mapping)
Map cartesian cardinates to polar volume indices.
Definition: cart.cpp:154