Illustrated C/C++ source sheets, symbol index, API relationship graph, compiler console and debug trace

LIVE AI / FLEXIBLE SENSING / FLIGHT CONTEXT

Sensoraia API

Live AI and sensing, connected to the flight.
See, listen, inspect and return supported results while airborne.

Loading...
Searching...
No Matches
dipla_gp_belief_planner.hpp
Go to the documentation of this file.
1/*
2 * Copyright 2026 OpenUAS
3 * SPDX-License-Identifier: Apache-2.0
4 *
5 * Licensed under the Apache License, Version 2.0 (the "License");
6 * you may not use this file except in compliance with the License.
7 * You may obtain a copy of the License at
8 *
9 * http://www.apache.org/licenses/LICENSE-2.0
10 *
11 * Unless required by applicable law or agreed to in writing, software
12 * distributed under the License is distributed on an "AS IS" BASIS,
13 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
14 * See the License for the specific language governing permissions and
15 * limitations under the License.
16 */
17
18/* Additional author attribution: Copyright 2026 Akshat Jain */
19
20/**
21 * @file dipla_gp_belief_planner.hpp
22 * @brief Native finite-grid GP sensor model and trajectory-planning API.
23 * @details Provides the reusable research/reference planner used by the
24 * explicitly selected CMA-ES comparison mode. The implementation stores dense
25 * covariance in build-selected row-major matrices, so the 2601-cell cap is a deliberate
26 * memory bound rather than an arbitrary input restriction.
27 */
28#pragma once
29
30#include "aigenlib/compat.hpp"
31#include <cstddef>
32#include <functional>
33#include <random>
34#include <vector>
35
36namespace aipp {
37/** Default route-pose sampling interval for look-ahead scoring. */
38inline constexpr int lookahead_stride = 4;
39/** Innovation-system row budget, independent of worker FIFO capacity. */
40inline constexpr int lookahead_measurement_budget = 64;
41/** Default emitted spline samples per control segment. */
42inline constexpr int spline_samples_per_segment = 5;
43
44/** Dense row-major covariance layout chosen for contiguous row updates. */
46/** Dynamic GP mean, measurement, or per-cell variance vector. */
48/** Three-dimensional local east/north/altitude point, in metres. */
50
51/**
52 * @brief Rectangular, evenly spaced local map grid.
53 * @details The upper bounds are snapped to the last grid coordinate that fits
54 * the requested spacing. Construction rejects grids larger than 2601 cells to
55 * bound dense covariance memory on the target board.
56 */
57struct Grid {
58 /** Inclusive lower east bound in metres. */
59 double xmin = 0.0;
60 /** Inclusive, grid-aligned upper east bound in metres. */
61 double xmax = 100.0;
62 /** Inclusive lower north bound in metres. */
63 double ymin = 0.0;
64 /** Inclusive, grid-aligned upper north bound in metres. */
65 double ymax = 100.0;
66 /** Uniform spacing in metres. */
67 double step = 2.0;
68 /** Number of columns. */
69 int width = 0;
70 /** Number of rows. */
71 int height = 0;
72
73 /**
74 * @brief Create a grid and align its effective upper bounds.
75 * @details Dimensions include only complete spacing intervals; effective
76 * upper bounds become the final sampled grid coordinates.
77 * @param xminimum Inclusive lower east coordinate.
78 * @param xmaximum Requested upper east coordinate.
79 * @param yminimum Inclusive lower north coordinate.
80 * @param ymaximum Requested upper north coordinate.
81 * @param spacing Positive uniform sample interval.
82 * @throws std::invalid_argument If bounds/spacing are invalid or exceed the dense-grid cap.
83 */
84 Grid(double xminimum = 0.0, double xmaximum = 100.0,
85 double yminimum = 0.0, double ymaximum = 100.0, double spacing = 2.0);
86 /** @brief Return the number of grid cells.
87 * @details The grid is stored as a dense row-major domain.
88 * @return `width * height`.
89 */
90 int size() const;
91 /** @brief Convert a column index to its east coordinate.
92 * @details Uses the affine grid mapping `xmin + column * step`.
93 * @param column Zero-based column.
94 * @return East coordinate in metres.
95 */
96 double x(int column) const;
97 /** @brief Convert a row index to its north coordinate.
98 * @details Uses the affine grid mapping `ymin + row * step`.
99 * @param row Zero-based row.
100 * @return North coordinate in metres.
101 */
102 double y(int row) const;
103 /** @brief Find the nearest clamped column; half-cell ties round downward.
104 * @details The explicit tie rule avoids dependence on the process rounding mode.
105 * @param value East coordinate in metres.
106 * @return Valid zero-based column.
107 * @throws std::invalid_argument For a nonfinite coordinate.
108 */
109 int nearest_x(double value) const;
110 /** @brief Find the nearest clamped row; half-cell ties round downward.
111 * @details The explicit tie rule avoids dependence on the process rounding mode.
112 * @param value North coordinate in metres.
113 * @return Valid zero-based row.
114 * @throws std::invalid_argument For a nonfinite coordinate.
115 */
116 int nearest_y(double value) const;
117};
118
119/**
120 * @brief Sparse block-averaging observation operator.
121 * @details Each row lists grid-cell indices averaged into one measurement;
122 * `variances` stores the independent observation variance for each row.
123 */
125
126/**
127 * @brief Search, utility, and optimizer controls for the reference planner.
128 * @details `keep_planning` is polled inside expensive search/refinement loops;
129 * it provides cooperative cancellation, not preemptive interruption.
130 */
132 /** Confidence multiplier applied to posterior standard deviation. */
133 double beta = 1.0;
134 /** Field-value threshold used to define planning importance. */
135 double threshold = 0.3;
136 /** Minimum permitted altitude in metres. */
137 double zmin = 10.0;
138 /** Maximum permitted altitude in metres. */
139 double zmax = 40.0;
140 /** Full sensor field-of-view angle in degrees. */
141 double angle_of_view = 60.0;
142 /** Exponent used to penalize travel distance. */
143 double cost_exponent = 1.0;
144 /** Initial CMA-ES standard deviation for horizontal coordinates. */
145 double step_xy = 20.0;
146 /** Initial CMA-ES standard deviation for altitude. */
147 double step_z = 8.0;
148 /** Greedy waypoint count. */
149 int horizon = 8;
150 /** Maximum CMA-ES generations. */
151 int maxiter = 25;
152 /** CMA-ES population size and evaluation batch size. */
153 int popsize = 12;
154 /** Maximum objective evaluations; an incomplete population is not run. */
155 int maxfevals = 1000;
156 /** Reproducible optimizer seed. */
157 long seed = 55;
158 /** Choose lower-confidence-bound versus upper-confidence-bound utility. */
160 /** Run CMA-ES refinement after greedy lattice selection. */
161 bool refine = true;
162 /** Fixed observation variance; negative selects the altitude noise model. */
163 double observation_variance = -1.0;
164 /** Horizontal control inset in grid cells, independent of altitude. */
165 unsigned margin_cells = 2;
166 /** Optional cooperative continue predicate, checked during planning. */
167 std::function<bool()> keep_planning;
168};
169
170/**
171 * @brief Compute the ground-plane footprint radius from altitude and full FOV angle.
172 * @details Uses the tangent of the half-angle because the input is the full FOV.
173 * @param altitude Sensor altitude above ground in metres.
174 * @param angle Full field-of-view angle in degrees.
175 * @return Ground-plane footprint radius in metres.
176 */
177double fov_radius(double altitude, double angle = 60.0);
178/**
179 * @brief Select the research sensor's altitude-dependent aggregation block size.
180 * @details Piecewise aggregation preserves the reference sensor's coarser
181 * effective resolution at higher altitude.
182 * @param altitude Sensor altitude in metres.
183 * @return Aggregation block width in cells.
184 */
185int resolution_block_size(double altitude);
186/**
187 * @brief Compute modeled observation variance at an altitude.
188 * @details Exponentially approaches the reference high-altitude variance rather
189 * than increasing without bound.
190 * @param altitude Sensor altitude in metres.
191 * @param minimum_altitude Reference altitude for the minimum variance.
192 * @return Modeled measurement variance.
193 */
194double noise_model(double altitude, double minimum_altitude = 10.0);
195/**
196 * @brief Initialize a dense Matérn-3/2 GP covariance matrix.
197 * @details Kernel values are cached by displacement on the uniform grid; the
198 * dense matrix then supports the block updates used by conditioning.
199 * @param grid Grid defining the covariance coordinates.
200 * @param sigma2 Kernel marginal variance.
201 * @param lengthscale Matérn spatial length scale in metres.
202 * @return Symmetric dense prior covariance.
203 * @throws std::invalid_argument If kernel amplitude or length scale is invalid.
204 */
205Matrix initialize_covariance(const Grid& grid, double sigma2, double lengthscale);
206/**
207 * @brief Build one block-averaged sensor footprint at a pose.
208 * @details Grid cells inside the square bounding the circular FOV are grouped
209 * by altitude-dependent resolution blocks, matching the reference sensor's
210 * coarse measurement behavior.
211 * @param grid Map grid.
212 * @param pose Sensor east/north/altitude in metres.
213 * @param angle Full field-of-view angle in degrees.
214 * @param observation_variance Fixed variance, or negative to use the altitude model.
215 * @return Sparse sensor rows and their independent variances.
216 */
217Sensor build_sensor(const Grid& grid, const Point& pose, double angle = 60.0,
218 double observation_variance = -1.0);
219/**
220 * @brief Approximate future measurements along a path under a bounded budget.
221 * @details Samples path poses by `stride`, always includes the final waypoint,
222 * and evenly subsamples sensor rows if their total exceeds `max_measurements`.
223 * This trades a compact innovation matrix for some spatial detail.
224 * @param grid Map grid.
225 * @param path Ordered trajectory in metres.
226 * @param angle Full field-of-view angle in degrees.
227 * @param stride Path-pose interval used for sampling.
228 * @param max_measurements Maximum number of returned measurement rows.
229 * @param observation_variance Fixed variance, or negative to use the altitude model.
230 * @return Combined sensor operator within the requested measurement budget.
231 * @throws std::invalid_argument If stride or the measurement budget is nonpositive.
232 */
233Sensor future_sensor(const Grid& grid, const std::vector<Point>& path,
234 double angle = 60.0, int stride = lookahead_stride,
235 int max_measurements = lookahead_measurement_budget,
236 double observation_variance = -1.0);
237/**
238 * @brief Apply the sensor's block averages to a full-grid field.
239 * @details Each sparse row is treated as a uniform averaging operator.
240 * @param sensor Sparse sensor operator.
241 * @param field One value per grid cell.
242 * @return One averaged value per sensor row.
243 */
244Vector observe(const Sensor& sensor, const Vector& field);
245/**
246 * @brief Draw independent zero-mean noise using each sensor row's variance.
247 * @details Uses one standard-normal draw per row and scales by that row's
248 * standard deviation; the caller owns the generator to control reproducibility.
249 * @param sensor Per-row variances define Gaussian scales.
250 * @param generator Caller-owned random engine for reproducibility.
251 * @return One independent noise value per sensor row.
252 */
253Vector sample_sensor_noise(const Sensor& sensor, std::mt19937_64& generator);
254/**
255 * @brief Select cells whose confidence bounds satisfy the utility threshold.
256 * @details If no cell meets the requested bound, all cells are selected so the
257 * optimizer does not receive a degenerate zero-utility objective.
258 * @param mean Posterior mean.
259 * @param covariance Posterior covariance.
260 * @param options Confidence-bound direction, beta, and threshold.
261 * @return Boolean importance mask in grid order.
262 */
263std::vector<bool> importance_mask(const Vector& mean, const Matrix& covariance,
264 const PlannerOptions& options);
265/**
266 * @brief Sum expected marginal-variance reduction over important cells.
267 * @details Uses the sensor innovation covariance and clamps numerical roundoff
268 * to each cell's prior marginal variance.
269 * @param covariance Prior covariance.
270 * @param mask Cells contributing to utility.
271 * @param sensor Candidate observation operator.
272 * @return Sum of marginal-variance reduction over selected cells.
273 */
274double expected_reduction(const Matrix& covariance, const std::vector<bool>& mask,
275 const Sensor& sensor);
276/**
277 * @brief Apply a Gaussian linear-observation update in place.
278 * @details Solves the innovation system rather than forming an inverse, updates
279 * the posterior mean only when both optional pointers are supplied, then updates
280 * covariance in row blocks to limit temporary allocations and restores symmetry.
281 * @param covariance Prior covariance, replaced by the posterior covariance.
282 * @param mean Optional prior mean; used only when `measurement` is non-null.
283 * @param sensor Observation operator.
284 * @param measurement Optional observed vector paired with `mean`.
285 */
286void condition(Matrix& covariance, Vector* mean, const Sensor& sensor, const Vector* measurement);
287/**
288 * @brief Clamp a candidate to the map safety inset and altitude interval.
289 * @details Horizontal margins are two grid steps, independent of optimizer
290 * penalty settings, so downstream routes retain a geometric boundary buffer.
291 * @param grid Map geometry and horizontal inset.
292 * @param point Candidate east/north/altitude point.
293 * @param options Permitted altitude interval.
294 * @return Bounded point.
295 */
296Point clip_point(const Grid& grid, const Point& point, const PlannerOptions& options);
297/**
298 * @brief Generate the deduplicated coarse-to-fine candidate lattice.
299 * @details Nested layers trade uniform resolution for broad coverage and
300 * progressively finer central choices.
301 * @param grid Map geometry and spacing.
302 * @param options Altitude range for lattice layers.
303 * @return Unique candidate positions.
304 */
305std::vector<Point> lattice(const Grid& grid, const PlannerOptions& options);
306/**
307 * @brief Interpolate controls with a natural cubic spline parameterized by distance.
308 * @details Coincident consecutive knots are removed because their zero chord
309 * lengths make the spline system singular; an all-coincident path is returned
310 * as its single unique point.
311 * @param start Initial path point.
312 * @param waypoints Ordered spline controls.
313 * @param samples_per_segment Number of emitted samples per chord.
314 * @return Interpolated path including the final knot.
315 * @throws std::invalid_argument If samples per segment is not positive.
316 */
317std::vector<Point> spline(const Point& start, const std::vector<Point>& waypoints,
318 int samples_per_segment = spline_samples_per_segment);
319/**
320 * @brief Score a trajectory by safety violation and distance-normalized information gain.
321 * @details Controls are clipped for geometric evaluation, while raw-control and
322 * spline overshoot is retained as a penalty so the optimizer cannot hide an
323 * unsafe proposal by clipping it before scoring.
324 * @param grid Map geometry and safety bounds.
325 * @param covariance Current covariance.
326 * @param mask Cells included in information gain.
327 * @param start Current position.
328 * @param raw_waypoints Candidate controls before clipping.
329 * @param options Sensor geometry and distance-cost settings.
330 * @return Scalar objective; lower values are preferred.
331 */
332double trajectory_objective(const Grid& grid, const Matrix& covariance,
333 const std::vector<bool>& mask, const Point& start,
334 const std::vector<Point>& raw_waypoints, const PlannerOptions& options);
335/**
336 * @brief Greedily select a bounded route by information gain per travel cost.
337 * @details Each chosen sensor conditions a private covariance copy before the
338 * next selection, giving cheap sequential look-ahead without mutating observed
339 * belief state. Cancellation returns an empty/incomplete route.
340 * @param grid Candidate world grid.
341 * @param mean Current posterior mean.
342 * @param covariance Current posterior covariance.
343 * @param start Initial position.
344 * @param options Horizon, utility, sensor, and cancellation settings.
345 * @return Ordered greedy controls, possibly empty when interrupted.
346 */
347std::vector<Point> grid_search(const Grid& grid, const Vector& mean, const Matrix& covariance,
348 const Point& start, const PlannerOptions& options);
349/**
350 * @brief Refine a greedy route with bounded CMA-ES while retaining its best candidate.
351 * @details Fixed-altitude problems omit Z from the search dimension; boundary
352 * transforms keep the optimizer's latent Gaussian unconstrained while all
353 * evaluated waypoints remain inside flight bounds.
354 * @param grid Map geometry and safety bounds.
355 * @param mean Current posterior mean.
356 * @param covariance Current posterior covariance.
357 * @param start Initial position.
358 * @param initial Greedy route used as the warm start and incumbent.
359 * @param options Optimizer budget, seed, bounds, and cancellation predicate.
360 * @return Best route found, or the unchanged initial route if refinement cannot start.
361 */
362std::vector<Point> refine(const Grid& grid, const Vector& mean, const Matrix& covariance,
363 const Point& start, const std::vector<Point>& initial,
364 const PlannerOptions& options);
365/**
366 * @brief Run greedy route selection followed by optional CMA-ES refinement.
367 * @details Greedy search provides a feasible incumbent; refinement is skipped
368 * when `options.refine` is false.
369 * @param grid Map geometry.
370 * @param mean Current posterior mean.
371 * @param covariance Current posterior covariance.
372 * @param start Initial position.
373 * @param options Search and optimizer settings.
374 * @return Ordered control points, or an empty route if cancelled during greedy search.
375 */
376std::vector<Point> plan(const Grid& grid, const Vector& mean, const Matrix& covariance,
377 const Point& start, const PlannerOptions& options);
378
379}
Eigen-style Aigen values and build-selected planner aliases.
static bool start(uint32_t session)
Admit a nonzero session and discard correlation from the previous one.
struct latest_measurement measurement
uint8_t row[LWIR_MAX_WIDTH *3U]
Definition lwir_image.c:63
VectorXd Vector
Definition compat.hpp:278
Vector3d Point
Definition compat.hpp:280
MatrixXd Matrix
Definition compat.hpp:276
dipla::aigenlib::Sensor Sensor
Sparse block-averaging observation operator.
double fov_radius(double altitude, double angle)
Compute the ground-plane footprint radius from altitude and full FOV angle.
constexpr int lookahead_measurement_budget
std::vector< Point > refine(const Grid &grid, const Vector &mean, const Matrix &covariance, const Point &start, const std::vector< Point > &initial, const PlannerOptions &options)
Refine a greedy route with bounded CMA-ES while retaining its best candidate.
std::vector< Point > plan(const Grid &grid, const Vector &mean, const Matrix &covariance, const Point &start, const PlannerOptions &options)
Run greedy route selection followed by optional CMA-ES refinement.
int resolution_block_size(double altitude)
Select the research sensor's altitude-dependent aggregation block size.
double noise_model(double altitude, double minimum_altitude)
Compute modeled observation variance at an altitude.
constexpr int spline_samples_per_segment
std::vector< Point > spline(const Point &start, const std::vector< Point > &waypoints, int samples_per_segment)
Interpolate controls with a natural cubic spline parameterized by distance.
std::vector< bool > importance_mask(const Vector &mean, const Matrix &covariance, const PlannerOptions &options)
Select cells whose confidence bounds satisfy the utility threshold.
Matrix initialize_covariance(const Grid &grid, double sigma2, double lengthscale)
Initialize a dense Matérn-3/2 GP covariance matrix.
constexpr int lookahead_stride
math::Point Point
Vector sample_sensor_noise(const Sensor &sensor, std::mt19937_64 &generator)
Draw independent zero-mean noise using each sensor row's variance.
double trajectory_objective(const Grid &grid, const Matrix &covariance, const std::vector< bool > &mask, const Point &start, const std::vector< Point > &raw_waypoints, const PlannerOptions &options)
Score a trajectory by safety violation and distance-normalized information gain.
math::Matrix Matrix
double expected_reduction(const Matrix &covariance, const std::vector< bool > &mask, const Sensor &sensor)
Sum expected marginal-variance reduction over important cells.
Sensor future_sensor(const Grid &grid, const std::vector< Point > &path, double angle, int stride, int max_measurements, double observation_variance)
Approximate future measurements along a path under a bounded budget.
Sensor build_sensor(const Grid &grid, const Point &pose, double angle, double observation_variance)
Build one block-averaged sensor footprint at a pose.
math::Vector Vector
void condition(Matrix &covariance, Vector *mean, const Sensor &sensor, const Vector *measurement)
Apply a Gaussian linear-observation update in place.
std::vector< Point > grid_search(const Grid &grid, const Vector &mean, const Matrix &covariance, const Point &start, const PlannerOptions &options)
Greedily select a bounded route by information gain per travel cost.
std::vector< Point > lattice(const Grid &grid, const PlannerOptions &options)
Generate the deduplicated coarse-to-fine candidate lattice.
Vector observe(const Sensor &sensor, const Vector &field)
Apply the sensor's block averages to a full-grid field.
Point clip_point(const Grid &grid, const Point &point, const PlannerOptions &options)
Clamp a candidate to the map safety inset and altitude interval.
Rectangular, evenly spaced local map grid.
int nearest_y(double value) const
Find the nearest clamped row; half-cell ties round downward.
int nearest_x(double value) const
Find the nearest clamped column; half-cell ties round downward.
double x(int column) const
Convert a column index to its east coordinate.
int size() const
Return the number of grid cells.
double y(int row) const
Convert a row index to its north coordinate.
Grid(double xminimum=0.0, double xmaximum=100.0, double yminimum=0.0, double ymaximum=100.0, double spacing=2.0)
Create a grid and align its effective upper bounds.
Search, utility, and optimizer controls for the reference planner.
std::function< bool()> keep_planning
Sparse uniform-averaging measurement operator.
Definition aigenlib.hpp:161
Validated capture and reporting policy, fixed before the sample loop.
Definition earcam.c:137
One timestamped geographic position for a replay annotation.