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.cpp
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.cpp
22 * @brief Finite-grid GP observations, uncertainty updates, and reference route scoring.
23 * @details
24 * Despite the historical filename, this file does not train the ONNX denoiser.
25 * It maintains and queries a Gaussian-process (GP) belief about a scalar field,
26 * builds synthetic sensor operators, and provides the greedy/spline machinery
27 * used by the explicitly selected comparison planner.
28 *
29 * @par The shared linear-observation model
30 * For N grid cells and M sensor rows, each row of H uniformly averages its
31 * listed cells. We store those indices rather than a mostly zero M-by-N matrix.
32 * Sigma is the dense N-by-N covariance; R is diagonal independent sensor noise.
33 * @code{.text}
34 * P = H * Sigma (M by N)
35 * S = H * Sigma * H^T + R (M by M)
36 * B = solve(S, P) (M by N)
37 * posterior_mean = mean + B^T * (measurement - H * mean)
38 * posterior_covariance = Sigma - P^T * B
39 * @endcode
40 * Expected information gain needs the covariance update but not future measured
41 * values. That distinction lets look-ahead reduce hypothetical uncertainty
42 * without handing the planner the hidden field's answer sheet.
43 *
44 * @par When changing numerical behavior
45 * The Eigen branch below and the default aigenlib kernels implement the same
46 * valid-input model. Keep operator weights, grid order, solver cutoffs, and
47 * reduction rules aligned, and run differential tests when changing either.
48 * Public contracts stay in the header; implementation notes here explain why.
49 *
50 * @warning Grid construction caps N, not whole-process memory. One 2601-by-2601
51 * double covariance is about 51.6 MiB; hypothetical copies, M-by-N workspaces,
52 * and solver allocations are additional. Some helpers assume valid inputs
53 * rather than checking every precondition in both numerical backends.
54 * @see dipla_gp_belief_planner.hpp Public geometry, sensor, and planner API.
55 */
57
58#include <algorithm>
59#include <cmath>
60#include <limits>
61#include <map>
62#include <stdexcept>
63#include <utility>
64
65namespace aipp {
66namespace {
67constexpr double grid_interval_tolerance = 1e-9;
68constexpr double minimum_spline_chord_m = 1e-9;
69constexpr int eigen_covariance_block_rows = 64;
70constexpr double sensor_base_variance = 0.01;
71constexpr double sensor_variance_increment = 0.03;
72constexpr double sensor_noise_height_scale_m = 30.0;
73constexpr double sensor_noise_saturation_factor = 4.0;
74}
75#if DIPLA_USE_EIGEN
76namespace {
77
78/**
79 * @brief Form measurement-to-grid cross-covariance without materializing H.
80 * @details Averaging covariance rows applies the same weights as observe().
81 * Summing instead of averaging would silently give larger footprint blocks
82 * more signal simply because they contain more cells, changing the sensor model.
83 * @param[in] sensor Sparse uniform-average rows.
84 * @param[in] covariance Symmetric N-by-N prior or posterior covariance.
85 * @return M-by-N projected covariance P = H * Sigma.
86 * @pre Every sensor row is nonempty and contains valid grid indices.
87 * @note Sparse H does not make P sparse; budget M * N doubles for this result.
88 */
89Matrix project(const Sensor& sensor, const Matrix& covariance) {
90 Matrix projected = Matrix::Zero(static_cast<int>(sensor.rows.size()), covariance.cols());
91 for (std::size_t row = 0; row < sensor.rows.size(); ++row) {
92 for (const int index : sensor.rows[row]) {
93 projected.row(static_cast<int>(row)) += covariance.row(index);
94 }
95 projected.row(static_cast<int>(row)) /= static_cast<double>(sensor.rows[row].size());
96 }
97 return projected;
98}
99
100/**
101 * @brief Form the measurement-space innovation covariance.
102 * @details Applies the same row-average operator on both sides of the prior
103 * covariance and adds independent measurement variance to the diagonal. Shared
104 * field cells correlate measurements through Sigma, not through extra entries
105 * in R; changing to correlated sensor noise requires changing this construction.
106 * @param[in] sensor Operator and one finite, nonnegative noise variance per row.
107 * @param[in] projected M-by-N cross-covariance from project().
108 * @return M-by-M innovation system S = H * Sigma * H^T + R.
109 * @pre Sensor indices and dimensions match projected, with no empty sensor rows.
110 */
111Matrix innovation(const Sensor& sensor, const Matrix& projected) {
112 const int count = static_cast<int>(sensor.rows.size());
114 for (int row = 0; row < count; ++row) {
115 for (int column = 0; column < count; ++column) {
116 for (const int index : sensor.rows[row]) {
117 result(row, column) += projected(column, index);
118 }
119 result(row, column) /= static_cast<double>(sensor.rows[row].size());
120 }
121 result(row, row) += sensor.variances[row];
122 }
123 return result;
124}
125
126/**
127 * @brief Solve a symmetric positive-definite system with a stable fallback.
128 * @details Cholesky is the fast path for the expected positive-definite sensor
129 * system. Near-singular duplicate/overlapping measurements can defeat LLT, so a
130 * self-adjoint eigensolve applies a scale-relative pseudoinverse cutoff instead
131 * of amplifying numerical null modes.
132 * @param[in] system Symmetric M-by-M innovation system.
133 * @param[in] right M-by-N right-hand sides, solved together.
134 * @return Owning solution with the same shape as right.
135 * @pre Inputs are finite and dimensionally compatible.
136 * @throws std::runtime_error If the fallback eigendecomposition fails.
137 * @note The fallback retains the sign of significant eigenvalues. It is a
138 * symmetric pseudoinverse, not a repair that forces an indefinite system to be
139 * positive definite. Do not add jitter or discard negative modes without
140 * treating that as a numerical-model change and testing it explicitly.
141 */
142Matrix solve_spd(const Matrix& system, const Matrix& right) {
143 Eigen::LLT<Matrix> decomposition(system);
144 if (decomposition.info() == Eigen::Success) {
145 return decomposition.solve(right);
146 }
147 Eigen::SelfAdjointEigenSolver<Matrix> fallback(system);
148 if (fallback.info() != Eigen::Success) {
149 throw std::runtime_error("Cannot factor sensor innovation covariance");
150 }
151 // Scale the null-space threshold to the system spectrum; a fixed absolute
152 // epsilon would behave differently when kernel amplitude is rescaled.
153 const double cutoff = dipla::aigenlib::pseudoinverse_relative_cutoff * fallback.eigenvalues().cwiseAbs().maxCoeff();
154 Vector inverse = fallback.eigenvalues().unaryExpr([cutoff](double value) {
155 return std::abs(value) > cutoff ? 1.0 / value : 0.0;
156 });
157 return fallback.eigenvectors() * inverse.asDiagonal() * fallback.eigenvectors().transpose() * right;
158}
159
160}
161#endif
162
163/**
164 * @details Validate geometry before converting dimensions to integers or
165 * allocating any dense GP matrix. The four-step width check accommodates the
166 * planner's two-step inset on both sides; the cell cap limits quadratic storage.
167 * @note The small tolerance in interval counting avoids losing a final sample
168 * to floating-point noise near an exact spacing multiple. Effective upper
169 * bounds are then the last lattice coordinates, not unsampled requested edges.
170 * @warning The cell cap is not a whole-board 512 MB admission check. Enlarging
171 * it requires budgeting covariance copies and solver workspaces, not just one matrix.
172 */
173Grid::Grid(double xminimum, double xmaximum, double yminimum, double ymaximum, double spacing)
174 : xmin(xminimum), xmax(xmaximum), ymin(yminimum), ymax(ymaximum), step(spacing) {
175 if (!std::isfinite(xmin) || !std::isfinite(xmax) || !std::isfinite(ymin) ||
176 !std::isfinite(ymax) || !std::isfinite(step) || step <= 0.0 ||
177 xmax - xmin <= 4.0 * step || ymax - ymin <= 4.0 * step) {
178 throw std::invalid_argument("Grid must be finite, positive, and wider than the safety margins");
179 }
180 const double columns = std::floor((xmax - xmin) / step + grid_interval_tolerance) + 1.0;
181 const double rows = std::floor((ymax - ymin) / step + grid_interval_tolerance) + 1.0;
182 if (columns * rows > dipla::aigenlib::max_extent) {
183 throw std::invalid_argument("Dense GP is limited to 2601 cells for the 512 MB Pi memory budget");
184 }
185 width = static_cast<int>(columns);
186 height = static_cast<int>(rows);
187 xmax = x(width - 1);
188 ymax = y(height - 1);
189}
190
191/** @details All field vectors and covariance axes share row * width + column ordering. */
192int Grid::size() const { return width * height; }
193/** @details Use the grid's affine mapping everywhere; an array index is not a metre coordinate. */
194double Grid::x(int column) const { return xmin + column * step; }
195/** @details North is the row coordinate, consistent with field flattening and sensor indices. */
196double Grid::y(int row) const { return ymin + row * step; }
197/**
198 * @details ceil(offset - 0.5) deliberately sends exact half-cell ties to the
199 * lower index, independent of the process rounding mode. This is the research
200 * grid rule, not the neural input marker's separate NumPy-style rounding rule.
201 * @throws std::invalid_argument For nonfinite coordinates.
202 */
203int Grid::nearest_x(double value) const {
204 if (!std::isfinite(value)) throw std::invalid_argument("Nonfinite grid coordinate");
205 const double offset = (value - xmin) / step;
206 if (offset <= 0) return 0;
207 if (offset >= width - 1) return width - 1;
208 return static_cast<int>(std::ceil(offset - 0.5));
209}
210/**
211 * @details Apply the same lower-index tie rule as nearest_x() so axis changes
212 * do not change which cell a half-grid pose selects.
213 * @throws std::invalid_argument For nonfinite coordinates.
214 */
215int Grid::nearest_y(double value) const {
216 if (!std::isfinite(value)) throw std::invalid_argument("Nonfinite grid coordinate");
217 const double offset = (value - ymin) / step;
218 if (offset <= 0) return 0;
219 if (offset >= height - 1) return height - 1;
220 return static_cast<int>(std::ceil(offset - 0.5));
221}
222
223/**
224 * @details The angle is the full FOV in degrees. Dividing by 360 both halves
225 * it and converts to radians; using 180 here would double the angular footprint.
226 * @pre altitude is finite and nonnegative; angle is finite and between 0 and 180 degrees.
227 * @note This ground-plane geometry assumes a level footprint. build_sensor()
228 * selects its square bounds, not a ray-traced or circular visibility mask.
229 */
230double fov_radius(double altitude, double angle) {
231 return altitude * std::tan(angle * std::acos(-1.0) / 360.0);
232}
233/**
234 * @details Higher altitude coarsens one measurement into averages of wider
235 * grid blocks. These thresholds belong to the research observation model,
236 * not the ONNX tensor resolution; changing them changes the experiment's H.
237 */
238int resolution_block_size(double altitude) { return altitude <= 20.0 ? 1 : altitude <= 30.0 ? 2 : 4; }
239/**
240 * @details Return variance, not standard deviation: 0.01 at minimum_altitude,
241 * approaching 0.04 from below at increasing height. The exponential preserves
242 * that saturation rather than making high-altitude noise grow without limit.
243 * @warning This formula is not clamped below its reference altitude and can
244 * become negative there. The 2 m application explicitly supplies variance 0.01;
245 * do not extrapolate this law to low flight or take sqrt of a negative result.
246 * @see build_sensor Fixed-variance override for a separately declared sensor experiment.
247 */
248double noise_model(double altitude, double minimum_altitude) {
249 return sensor_base_variance + sensor_variance_increment *
250 (1.0 - std::exp(-std::log(sensor_noise_saturation_factor) * (altitude - minimum_altitude) / sensor_noise_height_scale_m));
251}
252
253/**
254 * @details The Matern-3/2 kernel is sigma2 * (1 + q) * exp(-q), with
255 * q = sqrt(3) * distance / lengthscale. sigma2 is already the marginal variance;
256 * squaring it again would change both uncertainty scale and inferred utility.
257 * At zero distance the diagonal is sigma2; nearby cells share information
258 * according to lengthscale measured in the same metres as the grid.
259 * @note This initializes covariance only. The application owns the prior mean;
260 * do not infer its value from a bare GP initializer in another caller.
261 */
262Matrix initialize_covariance(const Grid& grid, double sigma2, double lengthscale) {
263 if (!(sigma2 > 0.0) || !(lengthscale > 0.0) || !std::isfinite(sigma2) || !std::isfinite(lengthscale)) {
264 throw std::invalid_argument("GP kernel parameters must be finite and positive");
265 }
266 Matrix kernel(grid.height, grid.width);
267 for (int north = 0; north < grid.height; ++north) {
268 for (int east = 0; east < grid.width; ++east) {
269 const double distance = std::hypot(east * grid.step, north * grid.step);
270 const double scaled = std::sqrt(3.0) * distance / lengthscale;
271 kernel(north, east) = sigma2 * (1.0 + scaled) * std::exp(-scaled);
272 }
273 }
274 Matrix covariance(grid.size(), grid.size());
275 for (int row = 0; row < grid.size(); ++row) {
276 for (int column = 0; column < grid.size(); ++column) {
277 covariance(row, column) = kernel(std::abs(row / grid.width - column / grid.width),
278 std::abs(row % grid.width - column % grid.width));
279 }
280 }
281 return covariance;
282}
283
284/**
285 * @details Use the square bounding the FOV, then group included cells by
286 * globally anchored resolution blocks. Blocks stay anchored to grid row/column
287 * zero, not to the moving footprint edge; otherwise tiny pose changes would
288 * redefine the measurement operator even over the same physical cells.
289 * @note Edge blocks average only their included cells. Do not normalize them
290 * by the nominal block area or add a circular-distance rejection: either would
291 * describe a different observation model from this reference implementation.
292 * @pre Geometry and pose are finite, altitude/FOV are valid, and the chosen
293 * fixed or altitude-derived observation variance is finite and nonnegative.
294 * @see observe The same row-average weights must be used when generating measurements.
295 */
296Sensor build_sensor(const Grid& grid, const Point& pose, double angle, double observation_variance) {
297 Sensor sensor;
298 const double radius = fov_radius(pose.z(), angle);
299 const int block = resolution_block_size(pose.z());
300 // Column-first traversal creates rows in first-encounter order. The map
301 // finds an existing block; its sorted keys do NOT reorder the emitted rows.
302 // Keep traversal stable because it also fixes measurement and noise ordering.
303 std::map<std::pair<int, int>, std::size_t> lookup;
304 for (int column = 0; column < grid.width; ++column) {
305 if (grid.x(column) < pose.x() - radius || grid.x(column) > pose.x() + radius) continue;
306 for (int row = 0; row < grid.height; ++row) {
307 if (grid.y(row) < pose.y() - radius || grid.y(row) > pose.y() + radius) continue;
308 const auto key = std::make_pair(row / block, column / block);
309 const auto inserted = lookup.emplace(key, sensor.rows.size());
310 if (inserted.second) {
311 sensor.rows.emplace_back();
312 // A nonnegative override bypasses the altitude law, including
313 // zero for explicit noiseless tests. Each ROW has one variance;
314 // it is not divided by cell count as though every cell were
315 // a separately noisy measurement before averaging.
316 sensor.variances.push_back(observation_variance >= 0.0 ? observation_variance : noise_model(pose.z()));
317 }
318 sensor.rows[inserted.first->second].push_back(row * grid.width + column);
319 }
320 }
321 return sensor;
322}
323
324/**
325 * @details Approximate a route's future observations without querying truth.
326 * Snap each sampled XY pose to the research grid before constructing its H;
327 * the supplied altitude still controls footprint, resolution, and default noise.
328 * @note Endpoint inclusion happens before row limiting. The final bounded
329 * operator evenly retains rows, not entire footprints, so a small budget may
330 * omit some cells or rows from any particular pose, including the endpoint.
331 * @warning max_measurements bounds the returned innovation dimension, not
332 * peak construction storage: the combined operator is built before subsampling.
333 * @pre Path values, sensor geometry, and selected variances satisfy build_sensor()'s preconditions.
334 */
335Sensor future_sensor(const Grid& grid, const std::vector<Point>& path, double angle,
336 int stride, int max_measurements, double observation_variance) {
338 if (stride <= 0 || max_measurements <= 0) throw std::invalid_argument("Invalid sensor sampling budget");
339 std::vector<std::size_t> selected;
340 for (std::size_t index = 0; index < path.size(); index += static_cast<std::size_t>(stride)) {
341 selected.push_back(index);
342 }
343 // Preserve the destination even when stride sampling would omit it; the
344 // end of a route is a meaningful observation opportunity, not a midpoint.
345 if (!path.empty() && path[selected.back()] != path.back()) selected.push_back(path.size() - 1);
346 for (const auto index : selected) {
347 Point pose = path[index];
348 pose.x() = grid.x(grid.nearest_x(pose.x()));
349 pose.y() = grid.y(grid.nearest_y(pose.y()));
350 // Keep repeated/overlapping rows: distinct future observations have
351 // independent noise even when their averaging operators coincide.
352 Sensor sensor = build_sensor(grid, pose, angle, observation_variance);
353 for (std::size_t row = 0; row < sensor.rows.size(); ++row) {
354 combined.rows.push_back(std::move(sensor.rows[row]));
355 combined.variances.push_back(sensor.variances[row]);
356 }
357 }
358 if (static_cast<int>(combined.rows.size()) <= max_measurements) return combined;
359 // The innovation solve scales cubically with measurement count. Evenly
360 // retain rows across the route instead of allowing long paths to create an
361 // unbounded dense measurement system.
363 for (int index = 0; index < max_measurements; ++index) {
364 const auto selected_row = max_measurements == 1 ? 0 : static_cast<std::size_t>(
365 static_cast<double>(combined.rows.size() - 1) * index / (max_measurements - 1));
366 limited.rows.push_back(std::move(combined.rows[selected_row]));
367 limited.variances.push_back(combined.variances[selected_row]);
368 }
369 return limited;
370}
371
372/**
373 * @details Apply H identically to hidden truth and predicted mean; the latter
374 * gives the expected measurement used by the GP residual. Noise is deliberately
375 * separate so scoring can use the operator without drawing synthetic evidence.
376 * @pre Rows are nonempty and indices address finite entries in field.
377 * @see sample_sensor_noise Independent noise is added by the observation owner.
378 */
379Vector observe(const Sensor& sensor, const Vector& field) {
380 Vector result(static_cast<int>(sensor.rows.size()));
381 for (std::size_t row = 0; row < sensor.rows.size(); ++row) {
382 double sum = 0.0;
383 for (const int index : sensor.rows[row]) sum += field[index];
384 result[static_cast<int>(row)] = sum / static_cast<double>(sensor.rows[row].size());
385 }
386 return result;
387}
388
389/**
390 * @details Scale a standard normal by sqrt(variance), not variance itself.
391 * Using the same per-row variance in innovation() makes sampled measurements
392 * and GP likelihood describe one experiment rather than two incompatible sensors.
393 * @pre Each sensor row has one finite, nonnegative variance.
394 * @note The caller owns random-stream lifetime. Even a zero-variance row draws
395 * a normal here; changing row order or draw count changes later noise samples.
396 * Equal seeds alone do not promise NumPy or cross-library bitwise equivalence.
397 */
398Vector sample_sensor_noise(const Sensor& sensor, std::mt19937_64& generator) {
399 std::normal_distribution<double> normal(0.0, 1.0);
400 Vector noise(static_cast<int>(sensor.rows.size()));
401 for (std::size_t row = 0; row < sensor.rows.size(); ++row) {
402 noise[static_cast<int>(row)] = normal(generator) * std::sqrt(sensor.variances[row]);
403 }
404 return noise;
405}
406
407/**
408 * @details Select utility using mean plus/minus beta times marginal standard
409 * deviation. The lower-bound mode asks whether a cell could lie below threshold;
410 * the upper-bound mode asks whether it could lie above. Preserve the inequality
411 * directions when adapting a task: swapping them changes what exploration values.
412 * @note If every cell fails, score the whole field instead of handing search a
413 * flat zero-gain objective. This is an explicit exploration policy, not a claim
414 * that every cell now meets the requested confidence-bound test.
415 * @pre Mean, covariance, and utility settings are finite and dimensionally compatible.
416 */
417std::vector<bool> importance_mask(const Vector& mean, const Matrix& covariance, const PlannerOptions& options) {
418 std::vector<bool> mask(static_cast<std::size_t>(mean.size()));
419 bool any = false;
420 for (int index = 0; index < mean.size(); ++index) {
421 const double uncertainty = options.beta * std::sqrt(std::max(0.0, covariance(index, index)));
422 mask[index] = options.lower_confidence_bound ? mean[index] - uncertainty <= options.threshold
423 : mean[index] + uncertainty >= options.threshold;
424 any = any || mask[index];
425 }
426 if (!any) std::fill(mask.begin(), mask.end(), true);
427 return mask;
428}
429
430/**
431 * @details For each selected cell, the corresponding column of P and B yields
432 * the diagonal reduction in Sigma - P^T * B. Sum only those diagonals rather
433 * than allocating a full posterior just to obtain one candidate's utility.
434 * Future measurement values are unnecessary: linear-Gaussian covariance updates
435 * depend on the observation operator and noise, not the unknown observed value.
436 * @note Clamp each computed reduction to [0, prior marginal variance] to contain
437 * round-off. This is a trace-reduction utility, not entropy or a field-value reward.
438 * @pre Covariance is finite and symmetric; mask has N entries and sensor rows
439 * and variances are valid for the same N-cell domain.
440 */
441double expected_reduction(const Matrix& covariance, const std::vector<bool>& mask, const Sensor& sensor) {
442#if DIPLA_USE_EIGEN
443 if (sensor.rows.empty()) return 0.0;
444 const Matrix projected = project(sensor, covariance);
445 const Matrix solved = solve_spd(innovation(sensor, projected), projected);
446 double gain = 0.0;
447 for (int index = 0; index < covariance.cols(); ++index) {
448 if (mask[index]) {
449 gain += std::clamp(projected.col(index).dot(solved.col(index)), 0.0,
450 std::max(0.0, covariance(index, index)));
451 }
452 }
453 return gain;
454#else
455 // Keep the default backend's checked, bounded-storage kernel as the owner
456 // of its numerical implementation; do not grow a third GP algorithm here.
457 return dipla::aigenlib::expected_reduction(covariance, mask, sensor);
458#endif
459}
460
461/**
462 * @details Compute P and solve S * B = P once, then reuse B for both posterior
463 * mean and covariance. Never form a general explicit inverse merely to multiply
464 * it away again; the measurement-space solve is the smaller numerical problem.
465 * @pre Supply mean and measurement together with compatible finite dimensions,
466 * or pass both as nullptr for hypothetical covariance-only conditioning.
467 * @pre Covariance and sensor describe the same valid finite grid domain.
468 * @warning The update mutates storage in place; it is not a transactional
469 * operation with rollback after an arithmetic or allocation failure. Symmetry
470 * restoration fixes triangle disagreement, not arbitrary loss of definiteness.
471 * @see expected_reduction Same covariance algebra without materializing its update.
472 */
473void condition(Matrix& covariance, Vector* mean, const Sensor& sensor, const Vector* measurement) {
474 if (sensor.rows.empty()) return;
475#if DIPLA_USE_EIGEN
476 const Matrix projected = project(sensor, covariance);
477 const Matrix system = innovation(sensor, projected);
478 const Matrix solved = solve_spd(system, projected);
479 // Null mean/measurement is used by look-ahead planning: it updates only
480 // simulated covariance, without pretending an unobserved value is known.
481 // There is no crystal ball hiding in the optional measurement pointer.
482 if (mean && measurement) {
483 const Vector residual = *measurement - observe(sensor, *mean);
484 *mean += solved.transpose() * residual;
485 }
486 // Bound the temporary transpose/product workspace. A full 2601x2601 dense
487 // temporary would add roughly 51 MiB to an already memory-heavy GP update.
488 // noalias is valid because projected/solved are separate owning matrices;
489 // do not replace them with views into covariance without revisiting aliasing.
490 for (int first = 0; first < covariance.rows(); first += eigen_covariance_block_rows) {
491 const int count = std::min(eigen_covariance_block_rows, static_cast<int>(covariance.rows()) - first);
492 covariance.middleRows(first, count).noalias() -= projected.middleCols(first, count).transpose() * solved;
493 }
494 // Roundoff can make the independently updated triangles differ slightly;
495 // restore symmetry before later Cholesky/eigen solves depend on it.
496 for (int row = 0; row < covariance.rows(); ++row) {
497 for (int column = 0; column < row; ++column) {
498 covariance(row, column) = covariance(column, row) =
499 0.5 * (covariance(row, column) + covariance(column, row));
500 }
501 }
502#else
503 // aigenlib applies the same update with its own one-row scratch and tiled
504 // symmetry pass. Keep reference comparisons when changing operation order.
505 dipla::aigenlib::condition(covariance, mean, sensor, measurement);
506#endif
507}
508
509/**
510 * @details Apply the comparison planner's two-grid-step XY inset and declared
511 * Z interval. This constrains a control point, not every point of a cubic spline
512 * between controls; trajectory_objective() separately scores spline overshoot.
513 * @pre Coordinates and altitude bounds are finite, with options.zmin <= options.zmax.
514 * @warning A geometric target inset is not an aircraft geofence or an arrival test.
515 */
516Point clip_point(const Grid& grid, const Point& point, const PlannerOptions& options) {
517 return {std::clamp(point.x(), grid.xmin + options.margin_cells * grid.step, grid.xmax - options.margin_cells * grid.step),
518 std::clamp(point.y(), grid.ymin + options.margin_cells * grid.step, grid.ymax - options.margin_cells * grid.step),
519 std::clamp(point.z(), options.zmin, options.zmax)};
520}
521
522/**
523 * @details Combine three nested XY layers at progressively higher requested
524 * altitudes. Snap XY and Z to their respective grid-step lattices, then remove
525 * exact duplicate points so a shared location cannot consume two greedy slots.
526 * Fixed-height settings collapse Z layers naturally without inventing a Z search.
527 * @note Enumeration order breaks equal-score ties in grid_search(). Changing
528 * it can change a deterministic route even when the candidate set is unchanged.
529 * @pre Altitude bounds are finite, ordered, and their step count fits int.
530 */
531std::vector<Point> lattice(const Grid& grid, const PlannerOptions& options) {
532 std::vector<Point> candidates;
533 // Broad 9-by-9 coverage comes first, then smaller 3-by-3 and 2-by-2
534 // central layers. This is a compact candidate policy, not an exhaustive
535 // uniformly spaced 3-D search; layer density and height change together.
536 struct Layer { int side_count; double inset_fraction; };
537 static constexpr Layer layers[] = {{9, 0.0}, {3, 0.30}, {2, 0.60}};
538 const int altitude_count = static_cast<int>(std::floor((options.zmax - options.zmin) / grid.step + grid_interval_tolerance));
539 for (std::size_t layer = 0; layer < std::size(layers); ++layer) {
540 const double inset = layers[layer].inset_fraction;
541 const int side_count = layers[layer].side_count;
542 const double half_x = 0.5 * (grid.xmax - grid.xmin - 2.0 * options.margin_cells * grid.step) * (1.0 - inset);
543 const double half_y = 0.5 * (grid.ymax - grid.ymin - 2.0 * options.margin_cells * grid.step) * (1.0 - inset);
544 const double lower_x = 0.5 * (grid.xmin + grid.xmax) - half_x;
545 const double lower_y = 0.5 * (grid.ymin + grid.ymax) - half_y;
546 const double requested_z = options.zmin + layer * 0.5 * (options.zmax - options.zmin);
547 const int altitude_index = std::clamp(static_cast<int>(std::ceil((requested_z - options.zmin) / grid.step - 0.5)), 0, altitude_count);
548 for (int column = 0; column < side_count; ++column) {
549 for (int row = 0; row < side_count; ++row) {
550 Point point(grid.x(grid.nearest_x(lower_x + 2.0 * half_x * column / (side_count - 1))),
551 grid.y(grid.nearest_y(lower_y + 2.0 * half_y * row / (side_count - 1))),
552 options.zmin + altitude_index * grid.step);
553 if (std::find(candidates.begin(), candidates.end(), point) == candidates.end()) candidates.push_back(point);
554 }
555 }
556 }
557 return candidates;
558}
559
560/**
561 * @details Parameterize by 3-D chord length rather than control index so unequal
562 * waypoint spacing affects interpolation geometrically. Natural endpoint
563 * conditions set endpoint second derivatives to zero; an in-place tridiagonal
564 * solve obtains the interior derivatives for all three coordinates together.
565 * @note Consecutive chords no longer than 1e-9 m are omitted. This avoids a
566 * singular system but intentionally differs from retaining coincident controls
567 * with an artificial minimum chord length in another reference implementation.
568 * @warning This is a sampled scoring path, not the flight controller's path
569 * implementation, a constant-speed parameterization, or a bound on continuous
570 * spline overshoot between emitted samples.
571 * @pre Input coordinates are finite and the requested sample storage is practical.
572 */
573std::vector<Point> spline(const Point& start, const std::vector<Point>& waypoints, int samples_per_segment) {
574 if (samples_per_segment <= 0) throw std::invalid_argument("Spline sampling must be positive");
575 std::vector<Point> knots{start};
576 std::vector<double> distances;
577 // Zero-length chords make the natural-spline system singular. Removing
578 // consecutive duplicates preserves the geometric path and its endpoint.
579 for (const auto& point : waypoints) {
580 const double length = (point - knots.back()).norm();
581 if (length > minimum_spline_chord_m) { distances.push_back(length); knots.push_back(point); }
582 }
583 if (knots.size() == 1) return knots;
584 const std::size_t count = knots.size();
585 std::vector<double> diagonal(count, 1.0);
586 std::vector<Point> second(count, Point::Zero());
587 // Solve the tridiagonal natural-spline equations in place: endpoint second
588 // derivatives remain zero, avoiding a general dense solve per objective.
589 for (std::size_t index = 1; index + 1 < count; ++index) {
590 diagonal[index] = 2.0 * (distances[index - 1] + distances[index]);
591 second[index] = 6.0 * ((knots[index + 1] - knots[index]) / distances[index]
592 - (knots[index] - knots[index - 1]) / distances[index - 1]);
593 if (index > 1) {
594 const double ratio = distances[index - 1] / diagonal[index - 1];
595 diagonal[index] -= ratio * distances[index - 1];
596 second[index] -= ratio * second[index - 1];
597 }
598 }
599 // Back-substitute only interior knots. The unsigned loop stops above zero;
600 // changing the condition to index >= 0 would underflow rather than terminate.
601 for (std::size_t index = count - 2; index > 0; --index) {
602 second[index] = (second[index] - distances[index] * second[index + 1]) / diagonal[index];
603 }
604 std::vector<Point> path;
605 path.reserve(distances.size() * static_cast<std::size_t>(samples_per_segment) + 1);
606 for (std::size_t segment = 0; segment < distances.size(); ++segment) {
607 const double length = distances[segment];
608 // Emit each segment's start, not its endpoint; the next segment owns
609 // that shared knot. Append the final endpoint once after all segments.
610 for (int sample = 0; sample < samples_per_segment; ++sample) {
611 const double fraction = static_cast<double>(sample) / samples_per_segment;
612 const double left = 1.0 - fraction;
613 path.push_back(left * knots[segment] + fraction * knots[segment + 1]
614 + length * length / 6.0 * ((left * left * left - left) * second[segment]
615 + (fraction * fraction * fraction - fraction) * second[segment + 1]));
616 }
617 }
618 path.push_back(knots.back());
619 return path;
620}
621
622/**
623 * @details Minimize boundary violation minus information gain per travel cost.
624 * Raw controls contribute a penalty even though scoring uses clipped controls;
625 * otherwise many out-of-bounds proposals would become the same cheap clipped
626 * path. Sampled spline overshoot adds a penalty of its own.
627 * @note Travel cost is the clipped control polyline's length, not spline arc
628 * length. Its grid-step floor prevents division by zero and infinite reward for
629 * hovering on coincident controls. Sensor stride and the 64-row budget bound the
630 * scoring solve at the cost of an approximate future-observation operator.
631 * @warning Soft penalties do not certify continuous-path containment. Changing
632 * samples_per_segment, stride, or row budget changes the objective, not just speed.
633 * @pre Inputs satisfy spline(), clip_point(), and expected_reduction() contracts.
634 */
635double trajectory_objective(const Grid& grid, const Matrix& covariance, const std::vector<bool>& mask,
636 const Point& start, const std::vector<Point>& raw_waypoints, const PlannerOptions& options) {
637 std::vector<Point> clipped;
638 clipped.reserve(raw_waypoints.size());
639 double overshoot = 0.0;
640 // Keep a penalty for the un-clipped proposal. Scoring only its clipped
641 // geometry would make out-of-bounds controls appear deceptively optimal.
642 // Each point contributes its largest coordinate violation, not the sum
643 // over axes. Preserve that norm when comparing optimizer objective values.
644 const auto violation = [&](const Point& point) {
645 return std::max({point.x() - (grid.xmax - options.margin_cells * grid.step), grid.xmin + options.margin_cells * grid.step - point.x(),
646 point.y() - (grid.ymax - options.margin_cells * grid.step), grid.ymin + options.margin_cells * grid.step - point.y(),
647 point.z() - options.zmax, options.zmin - point.z(), 0.0});
648 };
649 double distance = 0.0;
650 Point previous = start;
651 for (const auto& point : raw_waypoints) {
652 overshoot += violation(point);
653 clipped.push_back(clip_point(grid, point, options));
654 distance += (clipped.back() - previous).norm();
655 previous = clipped.back();
656 }
657 const auto path = spline(start, clipped);
658 for (const auto& point : path) overshoot += violation(point);
659 const double gain = expected_reduction(covariance, mask,
660 future_sensor(grid, path, options.angle_of_view, lookahead_stride, lookahead_measurement_budget, options.observation_variance));
661 return overshoot - gain / std::pow(std::max(distance, grid.step), options.cost_exponent);
662}
663
664/**
665 * @details Choose a gain-per-distance winner, condition a private hypothetical
666 * covariance, then score the next choice against reduced uncertainty. Reusing
667 * the original covariance every round would repeatedly reward already-covered
668 * regions instead of planning complementary observations.
669 * @note The mean remains the observed mean throughout look-ahead: future sensor
670 * values are unknown. Importance masks can still change as simulated uncertainty
671 * shrinks. One extra dense covariance is the principal storage cost of this search.
672 * @warning Cancellation returns an empty vector, not the partial prefix already
673 * selected. A short nonempty route can still result from exhausted candidates;
674 * the application must enforce its required eight-point publication contract.
675 * @pre Geometry, posterior, utility, and sensor settings satisfy the called helpers.
676 * @see PlannerOptions::keep_planning Cooperative polling, not preemptive interruption.
677 */
678std::vector<Point> grid_search(const Grid& grid, const Vector& mean, const Matrix& covariance,
679 const Point& start, const PlannerOptions& options) {
680 auto available = lattice(grid, options);
681 // Greedy look-ahead needs a hypothetical posterior after each selected
682 // sensor, but must not mutate the caller's observed posterior.
683 Matrix simulated = covariance;
684 Point pose = start;
685 std::vector<Point> waypoints;
686 for (int round = 0; round < options.horizon && !available.empty(); ++round) {
687 if (options.keep_planning && !options.keep_planning()) return {};
688 const auto mask = importance_mask(mean, simulated, options);
689 double best_score = -std::numeric_limits<double>::infinity();
690 std::size_t best_index = 0;
691 Sensor best_sensor;
692 for (std::size_t index = 0; index < available.size(); ++index) {
693 if (options.keep_planning && !options.keep_planning()) return {};
694 Sensor sensor = future_sensor(grid, {available[index]}, options.angle_of_view, 1, lookahead_measurement_budget,
695 options.observation_variance);
696 const double score = expected_reduction(simulated, mask, sensor)
697 / std::pow(std::max((available[index] - pose).norm(), grid.step), options.cost_exponent);
698 // Strict comparison keeps the first enumerated candidate on ties.
699 // Deterministic ordering is part of reproducible greedy behavior.
700 if (score > best_score) {
701 best_score = score;
702 best_index = index;
703 best_sensor = std::move(sensor);
704 }
705 }
706 pose = available[best_index];
707 waypoints.push_back(pose);
708 // Do not choose the same lattice point twice, and do not condition the
709 // caller's real posterior on an observation we have only imagined.
710 available.erase(available.begin() + static_cast<std::ptrdiff_t>(best_index));
711 condition(simulated, nullptr, best_sensor, nullptr);
712 }
713 return waypoints;
714}
715
716}
static bool project(const union dc_shot_union *shot, unsigned camera_id, double uncertainty, struct coverage_candidate *candidate)
Convert FC units and estimate geometric detail using this camera's calibration.
int cols() const noexcept
Signed column count.
Definition compat.hpp:59
static Matrix Zero(std::size_t rows, std::size_t columns)
Allocate zero storage.
Definition compat.hpp:65
int rows() const noexcept
Signed row count.
Definition compat.hpp:57
static Point Zero()
Construct the origin.
Definition compat.hpp:142
int size() const noexcept
Signed vector length.
Definition compat.hpp:80
static bool start(uint32_t session)
Admit a nonzero session and discard correlation from the previous one.
Native finite-grid GP sensor model and trajectory-planning API.
struct latest_measurement measurement
static bool simulated
static double limited(double value)
Clamp full-range JPEG/YUV color conversion without changing the raw source evidence.
uint8_t row[LWIR_MAX_WIDTH *3U]
Definition lwir_image.c:63
static uint8_t combined[256U *192U *4U]
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
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.
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.
double expected_reduction(const Matrix &covariance, const std::vector< bool > &mask, const Sensor &sensor)
Sum the masked posterior diagonal reduction without modifying belief.
Definition aigenlib.cpp:388
constexpr double pseudoinverse_relative_cutoff
Definition aigenlib.hpp:35
void condition(Matrix &covariance, Vector *mean, const Sensor &sensor, const Vector *measurement)
Condition a GP in place with optional measurement assimilation.
Definition aigenlib.cpp:409
constexpr std::size_t max_extent
Definition aigenlib.hpp:33
Matrix innovation(const Sensor &sensor, const Matrix &projected)
Construct innovation covariance S = H P H^T + R from B = H P.
Definition aigenlib.cpp:359
static pthread_cond_t available
Definition pose_log.c:73
static size_t count
Definition pose_log.c:79
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.
double & y() noexcept
Access north.
Definition aigenlib.cpp:190
double & x() noexcept
Access east.
Definition aigenlib.cpp:188
double & z() noexcept
Access altitude.
Definition aigenlib.cpp:192
std::vector< std::vector< int > > rows
Definition aigenlib.hpp:162
Validated capture and reporting policy, fixed before the sample loop.
Definition earcam.c:137
One timestamped geographic position for a replay annotation.
struct usher_control_result result
Definition usher.c:377