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
aigenlib.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/*
19 * Portions of Jacobi rotation adapted from OpenCV's BSD-3-Clause JacobiImpl_:
20 * Copyright (C) 2000-2008, Intel Corporation, all rights reserved.
21 * Copyright (C) 2009, Willow Garage Inc., all rights reserved.
22 * Full original notice and modification details are in NOTICE.md.
23 */
24
25/**
26 * @file aigenlib.cpp
27 * @brief Bounded numerical storage and scaled small-system solvers.
28 * @details Contiguous right-hand-side row updates avoid strided inner loops.
29 * The fallback uses deterministic cyclic rotations rather than cached pivots.
30 */
31#include "aigenlib.hpp"
32
33#include <algorithm>
34#include <array>
35#include <cmath>
36#include <exception>
37#include <limits>
38#include <stdexcept>
39#include <thread>
40#include <utility>
41
42namespace dipla::aigenlib {
43namespace {
44constexpr unsigned jacobi_sweep_limit = 80;
45constexpr double jacobi_epsilon_multiplier = 0.25;
46constexpr double symmetry_epsilon_multiplier = 64.0;
47
48/** @brief Check extents before multiplication. @param rows Rows. @param columns Columns. @return Scalar count. */
49std::size_t element_count(std::size_t rows, std::size_t columns) {
50 if (rows > max_extent || columns > max_extent ||
51 (columns != 0 && rows > Vector{}.max_size() / columns)) {
52 throw std::length_error("aigenlib matrix exceeds the dense GP storage bound");
53 }
54 return rows * columns;
55}
56
57/** @brief Reject nonfinite vector values. @param values Input. @return Whether finite. */
58bool finite_vector(const Vector& values) {
59 return std::all_of(values.begin(), values.end(), [](double value) { return std::isfinite(value); });
60}
61
62/** @brief Validate the operator before indexed kernels run. @param sensor Operator. @param cells Cell count. */
63void validate_sensor(const Sensor& sensor, std::size_t cells) {
64 if (cells > max_extent || sensor.rows.size() > max_extent || sensor.variances.size() != sensor.rows.size()) {
65 throw std::invalid_argument("Invalid aigenlib sensor dimensions");
66 }
67 for (std::size_t row = 0; row < sensor.rows.size(); ++row) {
68 if (sensor.rows[row].empty() || sensor.rows[row].size() > max_extent ||
69 !std::isfinite(sensor.variances[row]) || sensor.variances[row] < 0.0) {
70 throw std::invalid_argument("Invalid aigenlib sensor row or variance");
71 }
72 for (const int index : sensor.rows[row]) {
73 if (index < 0 || static_cast<std::size_t>(index) >= cells) throw std::out_of_range("aigenlib sensor cell index");
74 }
75 }
76}
77
78/** @brief Check GP covariance shape. @param covariance Input. */
79void validate_covariance_shape(const Matrix& covariance) {
80 if (covariance.rows() != covariance.cols()) {
81 throw std::invalid_argument("Invalid aigenlib covariance");
82 }
83}
84
85/** @brief Factor the lower triangle in place. @param factor Scaled symmetric workspace. @return Whether positive definite. */
86bool cholesky(Matrix& factor) {
87 for (std::size_t row = 0; row < factor.rows(); ++row) {
88 double* current = factor.row_data(row);
89 for (std::size_t column = 0; column <= row; ++column) {
90 const double* previous = factor.row_data(column);
91 double value = current[column];
92 for (std::size_t inner = 0; inner < column; ++inner) {
93 value -= current[inner] * previous[inner];
94 }
95 if (column == row) {
96 if (!(value > 0.0) || !std::isfinite(value)) return false;
97 current[column] = std::sqrt(value);
98 } else {
99 current[column] = value / previous[column];
100 if (!std::isfinite(current[column])) return false;
101 }
102 }
103 }
104 return true;
105}
107/** @brief Apply two triangular solves. @param factor Lower Cholesky factor. @param result Scaled RHS, replaced by solution. */
108void triangular_solve(const Matrix& factor, Matrix& result) {
109 for (std::size_t row = 0; row < factor.rows(); ++row) {
110 double* destination = result.row_data(row);
111 const double* coefficients = factor.row_data(row);
112 for (std::size_t inner = 0; inner < row; ++inner) {
113 const double* source = result.row_data(inner);
114 for (std::size_t column = 0; column < result.cols(); ++column) {
115 destination[column] -= coefficients[inner] * source[column];
116 }
117 }
118 for (std::size_t column = 0; column < result.cols(); ++column) destination[column] /= coefficients[row];
119 }
120 for (std::size_t remaining = factor.rows(); remaining > 0; --remaining) {
121 const std::size_t row = remaining - 1;
122 double* destination = result.row_data(row);
123 for (std::size_t inner = row + 1; inner < factor.rows(); ++inner) {
124 const double coefficient = factor(inner, row);
125 const double* source = result.row_data(inner);
126 for (std::size_t column = 0; column < result.cols(); ++column) destination[column] -= coefficient * source[column];
127 }
128 const double diagonal = factor(row, row);
129 for (std::size_t column = 0; column < result.cols(); ++column) destination[column] /= diagonal;
130 }
131}
132
133/**
134 * @brief Diagonalize a scaled symmetric matrix with cyclic Jacobi rotations.
135 * @details The stable hypot rotation follows OpenCV JacobiImpl_; see NOTICE.md.
136 * Eigenvectors are columns. At most 80 sweeps, with an explicit convergence
137 * check, avoid returning an unfinished factorization as a valid result.
138 * @param diagonal Workspace, replaced by a near-diagonal matrix.
139 * @param vectors Output orthonormal eigenvector matrix.
140 */
141void jacobi(Matrix& diagonal, Matrix& vectors) {
142 for (std::size_t index = 0; index < vectors.rows(); ++index) vectors(index, index) = 1.0;
143 for (unsigned sweep = 0; sweep <= jacobi_sweep_limit; ++sweep) {
144 double largest = 0.0;
145 double scale = 0.0;
146 for (std::size_t row = 0; row < diagonal.rows(); ++row) {
147 scale = std::max(scale, std::abs(diagonal(row, row)));
148 for (std::size_t column = row + 1; column < diagonal.cols(); ++column) {
149 largest = std::max(largest, std::abs(diagonal(row, column)));
150 }
151 }
152 const double tolerance = jacobi_epsilon_multiplier * std::numeric_limits<double>::epsilon() * std::max(scale, 1.0);
153 if (largest <= tolerance) return;
154 if (sweep == jacobi_sweep_limit) break;
155 for (std::size_t first = 0; first < diagonal.rows(); ++first) {
156 for (std::size_t second = first + 1; second < diagonal.cols(); ++second) {
157 const double off = diagonal(first, second);
158 if (std::abs(off) <= tolerance) continue;
159 const double delta = 0.5 * (diagonal(second, second) - diagonal(first, first));
160 double tangent = off / (std::abs(delta) + std::hypot(off, delta));
161 if (delta < 0.0) tangent = -tangent;
162 const double cosine = 1.0 / std::hypot(1.0, tangent);
163 const double sine = tangent * cosine;
164 diagonal(first, first) -= tangent * off;
165 diagonal(second, second) += tangent * off;
166 diagonal(first, second) = diagonal(second, first) = 0.0;
167 for (std::size_t index = 0; index < diagonal.rows(); ++index) {
168 if (index != first && index != second) {
169 const double before_first = diagonal(index, first);
170 const double before_second = diagonal(index, second);
171 diagonal(index, first) = diagonal(first, index) = cosine * before_first - sine * before_second;
172 diagonal(index, second) = diagonal(second, index) = sine * before_first + cosine * before_second;
173 }
174 const double before_first = vectors(index, first);
175 const double before_second = vectors(index, second);
176 vectors(index, first) = cosine * before_first - sine * before_second;
177 vectors(index, second) = sine * before_first + cosine * before_second;
178 }
179 }
180 }
181 }
182 throw std::runtime_error("aigenlib symmetric eigensolver did not converge");
183}
184
185}
186
187Point::Point(double east, double north, double altitude) : coordinates{east, north, altitude} {}
188double& Point::x() noexcept { return coordinates[0]; }
189double Point::x() const noexcept { return coordinates[0]; }
190double& Point::y() noexcept { return coordinates[1]; }
191double Point::y() const noexcept { return coordinates[1]; }
192double& Point::z() noexcept { return coordinates[2]; }
193double Point::z() const noexcept { return coordinates[2]; }
194double& Point::operator[](std::size_t index) { return coordinates.at(index); }
195double Point::operator[](std::size_t index) const { return coordinates.at(index); }
196double Point::norm() const noexcept { return std::hypot(std::hypot(x(), y()), z()); }
197bool Point::finite() const noexcept { return std::isfinite(x()) && std::isfinite(y()) && std::isfinite(z()); }
198Point& Point::operator+=(const Point& other) noexcept {
199 for (std::size_t index = 0; index < coordinates.size(); ++index) coordinates[index] += other[index];
200 return *this;
201}
202Point& Point::operator-=(const Point& other) noexcept {
203 for (std::size_t index = 0; index < coordinates.size(); ++index) coordinates[index] -= other[index];
204 return *this;
205}
206Point operator+(Point left, const Point& right) noexcept { return left += right; }
207Point operator-(Point left, const Point& right) noexcept { return left -= right; }
208Point operator*(Point point, double scale) noexcept {
209 for (double& coordinate : point.coordinates) coordinate *= scale;
210 return point;
211}
212Point operator*(double scale, Point point) noexcept { return point * scale; }
213Point operator/(Point point, double divisor) {
214 if (divisor == 0.0 || !std::isfinite(divisor)) throw std::invalid_argument("Invalid aigenlib point divisor");
215 for (double& coordinate : point.coordinates) coordinate /= divisor;
216 return point;
217}
218bool operator==(const Point& left, const Point& right) noexcept { return left.coordinates == right.coordinates; }
219bool operator!=(const Point& left, const Point& right) noexcept { return !(left == right); }
220
221Matrix::Matrix(std::size_t rows, std::size_t columns, double value)
222 : rows_(rows), columns_(columns), values_(element_count(rows, columns), value) {}
224 if (this != &other) {
225 Matrix replacement(other);
226 *this = std::move(replacement);
227 }
228 return *this;
229}
230Matrix::Matrix(Matrix&& other) noexcept
231 : rows_(std::exchange(other.rows_, 0)), columns_(std::exchange(other.columns_, 0)), values_(std::move(other.values_)) {}
232Matrix& Matrix::operator=(Matrix&& other) noexcept {
233 if (this != &other) {
234 values_ = std::move(other.values_);
235 rows_ = std::exchange(other.rows_, 0);
236 columns_ = std::exchange(other.columns_, 0);
237 }
238 return *this;
239}
240std::size_t Matrix::rows() const noexcept { return rows_; }
241std::size_t Matrix::cols() const noexcept { return columns_; }
242std::size_t Matrix::size() const noexcept { return values_.size(); }
243double& Matrix::operator()(std::size_t row, std::size_t column) {
244 if (column >= columns_) throw std::out_of_range("aigenlib matrix column");
245 return row_data(row)[column];
246}
247double Matrix::operator()(std::size_t row, std::size_t column) const {
248 if (column >= columns_) throw std::out_of_range("aigenlib matrix column");
249 return row_data(row)[column];
250}
251double* Matrix::row_data(std::size_t row) {
252 if (row >= rows_) throw std::out_of_range("aigenlib matrix row");
253 return columns_ == 0 ? nullptr : values_.data() + row * columns_;
254}
255const double* Matrix::row_data(std::size_t row) const {
256 if (row >= rows_) throw std::out_of_range("aigenlib matrix row");
257 return columns_ == 0 ? nullptr : values_.data() + row * columns_;
258}
259bool Matrix::finite() const noexcept {
260 return std::all_of(values_.begin(), values_.end(), [](double value) { return std::isfinite(value); });
261}
262double Matrix::trace() const {
263 if (rows_ != columns_) throw std::invalid_argument("aigenlib trace requires a square matrix");
264 double result = 0.0;
265 for (std::size_t index = 0; index < rows_; ++index) result += (*this)(index, index);
266 return result;
267}
268
269Matrix solve_symmetric(const Matrix& system, const Matrix& right, SolveMethod* method) {
270 if (system.rows() != system.cols() || right.rows() != system.rows() || !system.finite() || !right.finite()) {
271 throw std::invalid_argument("Invalid aigenlib symmetric system");
272 }
273 double scale = 0.0;
274 for (std::size_t row = 0; row < system.rows(); ++row) {
275 for (std::size_t column = 0; column < system.cols(); ++column) scale = std::max(scale, std::abs(system(row, column)));
276 }
277 Matrix scaled(system.rows(), system.cols());
278 for (std::size_t row = 0; row < system.rows(); ++row) {
279 for (std::size_t column = 0; column <= row; ++column) {
280 const double lower = scale == 0.0 ? 0.0 : system(row, column) / scale;
281 const double upper = scale == 0.0 ? 0.0 : system(column, row) / scale;
282 if (std::abs(lower - upper) > symmetry_epsilon_multiplier * std::numeric_limits<double>::epsilon()) {
283 throw std::invalid_argument("aigenlib system is not symmetric");
284 }
285 scaled(row, column) = scaled(column, row) = lower;
286 }
287 }
288 Matrix factor = scaled;
289 Matrix result(right.rows(), right.cols());
290 if (cholesky(factor)) {
291 for (std::size_t row = 0; row < right.rows(); ++row) {
292 for (std::size_t column = 0; column < right.cols(); ++column) result(row, column) = right(row, column) / scale;
293 }
294 triangular_solve(factor, result);
295 if (method) *method = SolveMethod::cholesky;
296 } else {
297 Matrix vectors(system.rows(), system.cols());
298 jacobi(scaled, vectors);
299 double largest = 0.0;
300 for (std::size_t index = 0; index < system.rows(); ++index) largest = std::max(largest, std::abs(scaled(index, index)));
301 Vector coefficients(right.cols());
302 for (std::size_t mode = 0; mode < system.rows(); ++mode) {
303 const double eigenvalue = scaled(mode, mode);
304 if (std::abs(eigenvalue) <= pseudoinverse_relative_cutoff * largest) continue;
305 std::fill(coefficients.begin(), coefficients.end(), 0.0);
306 for (std::size_t row = 0; row < right.rows(); ++row) {
307 const double weight = vectors(row, mode);
308 const double* source = right.row_data(row);
309 for (std::size_t column = 0; column < right.cols(); ++column) coefficients[column] += weight * source[column];
310 }
311 for (double& coefficient : coefficients) coefficient = (coefficient / eigenvalue) / scale;
312 for (std::size_t row = 0; row < result.rows(); ++row) {
313 double* destination = result.row_data(row);
314 const double weight = vectors(row, mode);
315 for (std::size_t column = 0; column < result.cols(); ++column) destination[column] += weight * coefficients[column];
316 }
317 }
318 if (method) *method = SolveMethod::pseudoinverse;
319 }
320 if (!result.finite()) throw std::overflow_error("Nonfinite aigenlib symmetric solution");
321 return result;
322}
323
324Matrix matern32(const std::vector<Point>& positions, double sigma2, double lengthscale) {
325 if (positions.size() > max_extent || !(sigma2 > 0.0) || !(lengthscale > 0.0) ||
326 !std::isfinite(sigma2) || !std::isfinite(lengthscale) ||
327 !std::all_of(positions.begin(), positions.end(), [](const Point& point) { return point.finite(); })) {
328 throw std::invalid_argument("Invalid aigenlib Matern parameters");
329 }
330 Matrix result(positions.size(), positions.size());
331 for (std::size_t row = 0; row < positions.size(); ++row) {
332 for (std::size_t column = 0; column <= row; ++column) {
333 const double distance = (positions[row] - positions[column]).norm();
334 const double scaled = std::sqrt(3.0) * (distance / lengthscale);
335 const double value = std::isinf(scaled) ? 0.0 : sigma2 * ((1.0 + scaled) * std::exp(-scaled));
336 result(row, column) = result(column, row) = value;
337 }
338 }
339 return result;
340}
341
342Matrix project(const Sensor& sensor, const Matrix& covariance) {
343 validate_covariance_shape(covariance);
344 validate_sensor(sensor, covariance.rows());
345 Matrix result(sensor.rows.size(), covariance.cols());
346 for (std::size_t row = 0; row < sensor.rows.size(); ++row) {
347 double* destination = result.row_data(row);
348 for (const int index : sensor.rows[row]) {
349 const double* source = covariance.row_data(static_cast<std::size_t>(index));
350 for (std::size_t column = 0; column < covariance.cols(); ++column) destination[column] += source[column];
351 }
352 const double count = static_cast<double>(sensor.rows[row].size());
353 for (std::size_t column = 0; column < covariance.cols(); ++column) destination[column] /= count;
354 }
355 if (!result.finite()) throw std::overflow_error("Nonfinite aigenlib covariance projection");
356 return result;
357}
358
359Matrix innovation(const Sensor& sensor, const Matrix& projected) {
360 validate_sensor(sensor, projected.cols());
361 if (projected.rows() != sensor.rows.size() || !projected.finite()) throw std::invalid_argument("Invalid aigenlib projection");
362 Matrix result(sensor.rows.size(), sensor.rows.size());
363 for (std::size_t row = 0; row < result.rows(); ++row) {
364 for (std::size_t column = 0; column < result.cols(); ++column) {
365 double value = 0.0;
366 const double* source = projected.row_data(column);
367 for (const int index : sensor.rows[row]) value += source[index];
368 result(row, column) = value / static_cast<double>(sensor.rows[row].size());
369 }
370 result(row, row) += sensor.variances[row];
371 }
372 if (!result.finite()) throw std::overflow_error("Nonfinite aigenlib innovation covariance");
373 return result;
374}
375
376Vector observe(const Sensor& sensor, const Vector& field) {
377 validate_sensor(sensor, field.size());
378 if (!finite_vector(field)) throw std::invalid_argument("Nonfinite aigenlib field");
379 Vector result(sensor.rows.size());
380 for (std::size_t row = 0; row < sensor.rows.size(); ++row) {
381 for (const int index : sensor.rows[row]) result[row] += field[index];
382 result[row] /= static_cast<double>(sensor.rows[row].size());
383 }
384 if (!finite_vector(result)) throw std::overflow_error("Nonfinite aigenlib observation");
385 return result;
386}
387
388double expected_reduction(const Matrix& covariance, const std::vector<bool>& mask, const Sensor& sensor) {
389 validate_covariance_shape(covariance);
390 validate_sensor(sensor, covariance.rows());
391 if (mask.size() != covariance.rows()) throw std::invalid_argument("Invalid aigenlib importance mask");
392 if (sensor.rows.empty()) return 0.0;
393 const Matrix projected = project(sensor, covariance);
394 const Matrix solved = solve_symmetric(innovation(sensor, projected), projected);
395 double gain = 0.0;
396 for (std::size_t column = 0; column < covariance.cols(); ++column) {
397 if (!mask[column]) continue;
398 const double variance = covariance(column, column);
399 if (!std::isfinite(variance)) throw std::invalid_argument("Nonfinite aigenlib selected variance");
400 double reduction = 0.0;
401 for (std::size_t row = 0; row < projected.rows(); ++row) reduction += projected(row, column) * solved(row, column);
402 if (!std::isfinite(reduction)) throw std::overflow_error("Nonfinite aigenlib variance reduction");
403 gain += std::clamp(reduction, 0.0, std::max(0.0, variance));
404 }
405 if (!std::isfinite(gain)) throw std::overflow_error("Nonfinite aigenlib information gain");
406 return gain;
407}
408
409void condition(Matrix& covariance, Vector* mean, const Sensor& sensor, const Vector* measurement) {
410 validate_covariance_shape(covariance);
411 if (!covariance.finite()) throw std::invalid_argument("Nonfinite aigenlib covariance");
412 validate_sensor(sensor, covariance.rows());
413 if ((mean == nullptr) != (measurement == nullptr) ||
414 (mean && (mean->size() != covariance.rows() || measurement->size() != sensor.rows.size() ||
415 !finite_vector(*mean) || !finite_vector(*measurement)))) {
416 throw std::invalid_argument("Invalid aigenlib GP measurement or mean");
417 }
418 if (sensor.rows.empty()) return;
419 const Matrix projected = project(sensor, covariance);
420 const Matrix solved = solve_symmetric(innovation(sensor, projected), projected);
421 Vector updated_mean;
422 if (mean) {
423 Vector residual = observe(sensor, *mean);
424 for (std::size_t row = 0; row < residual.size(); ++row) residual[row] = (*measurement)[row] - residual[row];
425 updated_mean = *mean;
426 for (std::size_t row = 0; row < solved.rows(); ++row) {
427 const double* source = solved.row_data(row);
428 for (std::size_t column = 0; column < solved.cols(); ++column) updated_mean[column] += source[column] * residual[row];
429 }
430 if (!finite_vector(updated_mean)) throw std::overflow_error("Nonfinite aigenlib posterior mean");
431 }
432 constexpr std::size_t tile_size = 32;
433 if (projected.rows() == 1) {
434 const double* projection = projected.row_data(0);
435 const double* solution = solved.row_data(0);
436 for (std::size_t row = 0; row < covariance.rows(); ++row) {
437 covariance(row, row) -= 0.0 + projection[row] * solution[row];
438 }
439 for (std::size_t row_begin = 0; row_begin < covariance.rows(); row_begin += tile_size) {
440 const std::size_t row_end = std::min(row_begin + tile_size, covariance.rows());
441 for (std::size_t column_begin = 0; column_begin <= row_begin; column_begin += tile_size) {
442 const std::size_t column_end = std::min(column_begin + tile_size, covariance.cols());
443 for (std::size_t row = row_begin; row < row_end; ++row) {
444 double* destination = covariance.row_data(row);
445 double* opposite = covariance.row_data(column_begin);
446 for (std::size_t column = column_begin; column < std::min(column_end, row); ++column) {
447 const double lower = destination[column] - (0.0 + projection[row] * solution[column]);
448 const double upper = opposite[row] - (0.0 + projection[column] * solution[row]);
449 const double average = 0.5 * lower + 0.5 * upper;
450 destination[column] = opposite[row] = average;
451 opposite += covariance.cols();
452 }
453 }
454 }
455 }
456 } else {
457 constexpr std::size_t update_rows = 8;
458 constexpr std::size_t update_columns = 64;
459 const auto update = [&](std::size_t first_row, std::size_t last_row) {
460 double scratch[update_rows][update_columns];
461 for (std::size_t row_begin = first_row; row_begin < last_row; row_begin += update_rows) {
462 const std::size_t row_count = std::min(update_rows, last_row - row_begin);
463 for (std::size_t column_begin = 0; column_begin < covariance.cols(); column_begin += update_columns) {
464 const std::size_t column_count = std::min(update_columns, covariance.cols() - column_begin);
465 for (std::size_t row = 0; row < row_count; ++row)
466 std::fill_n(scratch[row], column_count, 0.0);
467 for (std::size_t inner = 0; inner < projected.rows(); ++inner) {
468 const double* weights = projected.row_data(inner) + row_begin;
469 const double* source = solved.row_data(inner) + column_begin;
470 for (std::size_t row = 0; row < row_count; ++row) {
471 const double weight = weights[row];
472 for (std::size_t column = 0; column < column_count; ++column)
473 scratch[row][column] += weight * source[column];
474 }
475 }
476 for (std::size_t row = 0; row < row_count; ++row) {
477 double* destination = covariance.row_data(row_begin + row) + column_begin;
478 for (std::size_t column = 0; column < column_count; ++column)
479 destination[column] -= scratch[row][column];
480 }
481 }
482 }
483 };
484 if (covariance.rows() >= 1024 && projected.rows() >= 16) {
485 constexpr std::size_t lane_count = 3;
486 std::array<std::thread, lane_count - 1> workers;
487 std::array<std::exception_ptr, lane_count> errors{};
488 const auto run_lane = [&](std::size_t lane) {
489 try { update(covariance.rows() * lane / lane_count, covariance.rows() * (lane + 1) / lane_count); }
490 catch (...) { errors[lane] = std::current_exception(); }
491 };
492 std::size_t launched = 0;
493 try {
494 for (; launched < workers.size(); ++launched)
495 workers[launched] = std::thread(run_lane, launched);
496 } catch (...) {}
497 for (std::size_t lane = launched; lane < lane_count; ++lane) run_lane(lane);
498 for (auto& worker : workers) if (worker.joinable()) worker.join();
499 for (const auto& error : errors) if (error) std::rethrow_exception(error);
500 } else update(0, covariance.rows());
501 for (std::size_t row_begin = 0; row_begin < covariance.rows(); row_begin += tile_size) {
502 const std::size_t row_end = std::min(row_begin + tile_size, covariance.rows());
503 for (std::size_t column_begin = 0; column_begin <= row_begin; column_begin += tile_size) {
504 const std::size_t column_end = std::min(column_begin + tile_size, covariance.cols());
505 for (std::size_t row = row_begin; row < row_end; ++row) {
506 double* destination = covariance.row_data(row);
507 for (std::size_t column = column_begin; column < std::min(column_end, row); ++column) {
508 double* opposite = covariance.row_data(column);
509 const double average = 0.5 * destination[column] + 0.5 * opposite[row];
510 destination[column] = opposite[row] = average;
511 }
512 }
513 }
514 }
515 }
516 if (!covariance.finite()) throw std::overflow_error("Nonfinite aigenlib posterior covariance");
517 if (mean) mean->swap(updated_mean);
518}
519
520}
Bounded, owning numerical types and Gaussian-process operations.
aigenlib storage with the planner's signed dimensions and finite predicate.
Definition compat.hpp:51
int cols() const noexcept
Signed column count.
Definition compat.hpp:59
int rows() const noexcept
Signed row count.
Definition compat.hpp:57
Matrix(dipla::aigenlib::Matrix value)
Accept an owning kernel result.
Definition compat.hpp:55
Bounded owning vector with the planner's small Eigen-style value API.
Definition compat.hpp:72
Owning, contiguous, row-major double matrix.
Definition aigenlib.hpp:95
double & operator()(std::size_t row, std::size_t column)
Checked access.
Definition aigenlib.cpp:243
std::size_t cols() const noexcept
Column count.
Definition aigenlib.cpp:241
Matrix & operator=(const Matrix &other)
Deep assignment with strong exception guarantee.
Definition aigenlib.cpp:223
std::size_t size() const noexcept
Allocated scalar count.
Definition aigenlib.cpp:242
double * row_data(std::size_t row)
Borrow a row.
Definition aigenlib.cpp:251
double trace() const
Sum a square matrix's diagonal.
Definition aigenlib.cpp:262
Matrix()=default
Construct an empty matrix.
bool finite() const noexcept
Validate all stored scalars.
Definition aigenlib.cpp:259
std::size_t rows() const noexcept
Row count.
Definition aigenlib.cpp:240
struct latest_measurement measurement
uint8_t row[LWIR_MAX_WIDTH *3U]
Definition lwir_image.c:63
struct jpeg_error_mgr error
Definition lwir_image.c:60
Point operator/(Point point, double divisor)
Divide coordinates.
Definition aigenlib.cpp:213
bool operator!=(const Point &left, const Point &right) noexcept
Exact inequality.
Definition aigenlib.cpp:219
bool operator==(const Point &left, const Point &right) noexcept
Exact equality.
Definition aigenlib.cpp:218
Vector observe(const Sensor &sensor, const Vector &field)
Average field values through a sensor.
Definition aigenlib.cpp:376
Matrix matern32(const std::vector< Point > &positions, double sigma2, double lengthscale)
Construct a symmetric Matern-3/2 covariance over supplied positions.
Definition aigenlib.cpp:324
Point operator-(Point left, const Point &right) noexcept
Subtract points.
Definition aigenlib.cpp:207
Point operator*(Point point, double scale) noexcept
Scale coordinates.
Definition aigenlib.cpp:208
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
Matrix project(const Sensor &sensor, const Matrix &covariance)
Project covariance through sparse averaging rows: B = H P.
Definition aigenlib.cpp:342
std::vector< double > Vector
Definition aigenlib.hpp:37
Matrix solve_symmetric(const Matrix &system, const Matrix &right, SolveMethod *method)
Solve a finite symmetric system with multiple right-hand sides.
Definition aigenlib.cpp:269
constexpr double pseudoinverse_relative_cutoff
Definition aigenlib.hpp:35
Point operator+(Point left, const Point &right) noexcept
Add points.
Definition aigenlib.cpp:206
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
constexpr std::size_t cells
static size_t count
Definition pose_log.c:79
Three-dimensional value type for route arithmetic.
Definition aigenlib.hpp:43
double & y() noexcept
Access north.
Definition aigenlib.cpp:190
Point & operator-=(const Point &other) noexcept
Subtract coordinates.
Definition aigenlib.cpp:202
std::array< double, 3 > coordinates
Definition aigenlib.hpp:44
bool finite() const noexcept
Check every coordinate.
Definition aigenlib.cpp:197
Point(double east=0.0, double north=0.0, double altitude=0.0)
Construct a point.
Definition aigenlib.cpp:187
double & x() noexcept
Access east.
Definition aigenlib.cpp:188
double & z() noexcept
Access altitude.
Definition aigenlib.cpp:192
double & operator[](std::size_t index)
Checked coordinate access.
Definition aigenlib.cpp:194
Point & operator+=(const Point &other) noexcept
Add coordinates.
Definition aigenlib.cpp:198
double norm() const noexcept
Euclidean length.
Definition aigenlib.cpp:196
Sparse uniform-averaging measurement operator.
Definition aigenlib.hpp:161
std::vector< std::vector< int > > rows
Definition aigenlib.hpp:162
One timestamped geographic position for a replay annotation.
struct usher_control_result result
Definition usher.c:377