Loading...
Searching...
No Matches
dipla_gp_belief_planner.cpp
Go to the documentation of this file.
153 const double cutoff = dipla::aigenlib::pseudoinverse_relative_cutoff * fallback.eigenvalues().cwiseAbs().maxCoeff();
157 return fallback.eigenvectors() * inverse.asDiagonal() * fallback.eigenvectors().transpose() * right;
178 throw std::invalid_argument("Grid must be finite, positive, and wider than the safety margins");
193/** @details Use the grid's affine mapping everywhere; an array index is not a metre coordinate. */
195/** @details North is the row coordinate, consistent with field flattening and sensor indices. */
238int resolution_block_size(double altitude) { return altitude <= 20.0 ? 1 : altitude <= 30.0 ? 2 : 4; }
296Sensor build_sensor(const Grid& grid, const Point& pose, double angle, double observation_variance) {
316 sensor.variances.push_back(observation_variance >= 0.0 ? observation_variance : noise_model(pose.z()));
333 * @pre Path values, sensor geometry, and selected variances satisfy build_sensor()'s preconditions.
417std::vector<bool> importance_mask(const Vector& mean, const Matrix& covariance, const PlannerOptions& options) {
441double expected_reduction(const Matrix& covariance, const std::vector<bool>& mask, const Sensor& sensor) {
473void condition(Matrix& covariance, Vector* mean, const Sensor& sensor, const Vector* measurement) {
491 const int count = std::min(eigen_covariance_block_rows, static_cast<int>(covariance.rows()) - first);
517 return {std::clamp(point.x(), grid.xmin + options.margin_cells * grid.step, grid.xmax - options.margin_cells * grid.step),
538 const int altitude_count = static_cast<int>(std::floor((options.zmax - options.zmin) / grid.step + grid_interval_tolerance));
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);
547 const int altitude_index = std::clamp(static_cast<int>(std::ceil((requested_z - options.zmin) / grid.step - 0.5)), 0, altitude_count);
635double trajectory_objective(const Grid& grid, const Matrix& covariance, const std::vector<bool>& mask,
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(),
660 future_sensor(grid, path, options.angle_of_view, lookahead_stride, lookahead_measurement_budget, options.observation_variance));
694 Sensor sensor = future_sensor(grid, {available[index]}, options.angle_of_view, 1, lookahead_measurement_budget,
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.
Definition capture_arbiter.c:173
static Matrix Zero(std::size_t rows, std::size_t columns)
Allocate zero storage.
Definition compat.hpp:65
static bool start(uint32_t session)
Admit a nonzero session and discard correlation from the previous one.
Definition dipla_camera_backend.c:170
Native finite-grid GP sensor model and trajectory-planning API.
static double limited(double value)
Clamp full-range JPEG/YUV color conversion without changing the raw source evidence.
Definition inference_image.c:38
Definition compat.hpp:242
dipla::aigenlib::Sensor Sensor
Sparse block-averaging observation operator.
Definition dipla_gp_belief_planner.hpp:124
double fov_radius(double altitude, double angle)
Compute the ground-plane footprint radius from altitude and full FOV angle.
Definition dipla_gp_belief_planner.cpp:230
constexpr int lookahead_measurement_budget
Definition dipla_gp_belief_planner.hpp:40
int resolution_block_size(double altitude)
Select the research sensor's altitude-dependent aggregation block size.
Definition dipla_gp_belief_planner.cpp:238
double noise_model(double altitude, double minimum_altitude)
Compute modeled observation variance at an altitude.
Definition dipla_gp_belief_planner.cpp:248
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.
Definition dipla_gp_belief_planner.cpp:573
std::vector< bool > importance_mask(const Vector &mean, const Matrix &covariance, const PlannerOptions &options)
Select cells whose confidence bounds satisfy the utility threshold.
Definition dipla_gp_belief_planner.cpp:417
Matrix initialize_covariance(const Grid &grid, double sigma2, double lengthscale)
Initialize a dense Matérn-3/2 GP covariance matrix.
Definition dipla_gp_belief_planner.cpp:262
Vector sample_sensor_noise(const Sensor &sensor, std::mt19937_64 &generator)
Draw independent zero-mean noise using each sensor row's variance.
Definition dipla_gp_belief_planner.cpp:398
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.
Definition dipla_gp_belief_planner.cpp:635
double expected_reduction(const Matrix &covariance, const std::vector< bool > &mask, const Sensor &sensor)
Sum expected marginal-variance reduction over important cells.
Definition dipla_gp_belief_planner.cpp:441
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.
Definition dipla_gp_belief_planner.cpp:335
Sensor build_sensor(const Grid &grid, const Point &pose, double angle, double observation_variance)
Build one block-averaged sensor footprint at a pose.
Definition dipla_gp_belief_planner.cpp:296
void condition(Matrix &covariance, Vector *mean, const Sensor &sensor, const Vector *measurement)
Apply a Gaussian linear-observation update in place.
Definition dipla_gp_belief_planner.cpp:473
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.
Definition dipla_gp_belief_planner.cpp:678
std::vector< Point > lattice(const Grid &grid, const PlannerOptions &options)
Generate the deduplicated coarse-to-fine candidate lattice.
Definition dipla_gp_belief_planner.cpp:531
Vector observe(const Sensor &sensor, const Vector &field)
Apply the sensor's block averages to a full-grid field.
Definition dipla_gp_belief_planner.cpp:379
Point clip_point(const Grid &grid, const Point &point, const PlannerOptions &options)
Clamp a candidate to the map safety inset and altitude interval.
Definition dipla_gp_belief_planner.cpp:516
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
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
int nearest_y(double value) const
Find the nearest clamped row; half-cell ties round downward.
Definition dipla_gp_belief_planner.cpp:215
int nearest_x(double value) const
Find the nearest clamped column; half-cell ties round downward.
Definition dipla_gp_belief_planner.cpp:203
double x(int column) const
Convert a column index to its east coordinate.
Definition dipla_gp_belief_planner.cpp:194
double y(int row) const
Convert a row index to its north coordinate.
Definition dipla_gp_belief_planner.cpp:196
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.
Definition dipla_gp_belief_planner.cpp:173
Search, utility, and optimizer controls for the reference planner.
Definition dipla_gp_belief_planner.hpp:131
One timestamped geographic position for a replay annotation.
Definition ear_heatmap_overlay.c:79