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
cmaes_optimizer.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 cmaes_optimizer.cpp
22 * @brief Bounded CMA-ES refinement for the native reference planner.
23 * @details Starts from the greedy lattice route, searches in CMA-ES latent
24 * coordinates, transforms candidates into flight bounds, and retains the best
25 * feasible route seen. Fixed-altitude tasks omit Z from the optimizer vector.
26 */
28#include <cmaes_interface.h>
29extern "C" {
31}
32#include <cmath>
33#include <stdexcept>
34#include <utility>
35
36namespace aipp {
37namespace {
38/** RAII guard for the C optimizer and its separately allocated boundary transform. */
39struct Optimizer {
40 cmaes_t state{}; /**< Optimizer-owned population/covariance resources. */
41 cmaes_boundary_transformation_t bounds{}; /**< Bounded-coordinate transform, separate from optimizer state. */
42 bool initialized = false; /**< Exit only after successful final initialization. */
43 bool bounds_initialized = false; /**< Transform cleanup remains valid if optimizer setup fails. */
44 /**
45 * @brief Release initialized C resources in reverse dependency order.
46 * @details Partial initialization is possible when the C API throws through
47 * an error path, so each resource has an independent readiness flag.
48 */
49 ~Optimizer() {
50 if (initialized) cmaes_exit(&state);
51 if (bounds_initialized) cmaes_boundary_transformation_exit(&bounds);
52 }
53};
54}
55std::vector<Point> refine(const Grid& grid, const Vector& mean, const Matrix& covariance,
56 const Point& start, const std::vector<Point>& initial, const PlannerOptions& options) {
57 if (initial.empty() || (options.keep_planning && !options.keep_planning())) return initial;
58 // Excluding fixed Z is more than an optimization: it prevents numerical
59 // search from wasting evaluations on a coordinate that cannot change.
60 const int coordinates = options.zmin == options.zmax ? 2 : 3;
61 const int dimension = static_cast<int>(coordinates * initial.size());
62 std::vector<double> lower(dimension), upper(dimension), deviations(dimension), latent(dimension), bounded(dimension);
63 auto best = initial;
64 for (std::size_t waypoint = 0; waypoint < initial.size(); ++waypoint) {
65 best[waypoint] = clip_point(grid, initial[waypoint], options);
66 for (int coordinate = 0; coordinate < coordinates; ++coordinate) {
67 const auto index = coordinates * waypoint + coordinate;
68 lower[index] = coordinate == 0 ? grid.xmin + options.margin_cells * grid.step : coordinate == 1 ? grid.ymin + options.margin_cells * grid.step : options.zmin;
69 upper[index] = coordinate == 0 ? grid.xmax - options.margin_cells * grid.step : coordinate == 1 ? grid.ymax - options.margin_cells * grid.step : options.zmax;
70 deviations[index] = coordinate == 2 ? options.step_z : options.step_xy;
71 bounded[index] = best[waypoint][coordinate];
72 }
73 }
74 Optimizer optimizer;
75 cmaes_boundary_transformation_init(&optimizer.bounds, lower.data(), upper.data(), lower.size());
76 optimizer.bounds_initialized = true;
77 // CMA-ES samples an unbounded Gaussian. Inverse-transform the warm start so
78 // the corresponding bounded point remains the center of its first search.
79 cmaes_boundary_transformation_inverse(&optimizer.bounds, bounded.data(), latent.data(), latent.size());
80 char silent_filename[] = "non";
81 cmaes_init_para(&optimizer.state, dimension, latent.data(), deviations.data(), options.seed, options.popsize, silent_filename);
82 optimizer.state.sp.filename = silent_filename;
83 double* fitness = cmaes_init_final(&optimizer.state);
84 optimizer.state.sp.filename = nullptr;
85 optimizer.initialized = true;
86 const auto mask = importance_mask(mean, covariance, options);
87 double best_value = trajectory_objective(grid, covariance, mask, start, best, options);
88 int evaluations = 0;
89 // Consume complete populations only: the C library updates its covariance
90 // from a generation as a unit, so a partial final batch is not meaningful.
91 for (int generation = 0; generation < options.maxiter && evaluations + options.popsize <= options.maxfevals; ++generation) {
92 double* const* population = cmaes_SamplePopulation(&optimizer.state);
93 for (int member = 0; member < options.popsize; ++member) {
94 if (options.keep_planning && !options.keep_planning()) return best;
95 cmaes_boundary_transformation(&optimizer.bounds, population[member], bounded.data(), bounded.size());
96 // Start from the best full route and replace only searched axes;
97 // in fixed-altitude mode this preserves every waypoint's Z exactly.
98 auto proposal = best;
99 for (std::size_t waypoint = 0; waypoint < proposal.size(); ++waypoint) {
100 for (int coordinate = 0; coordinate < coordinates; ++coordinate)
101 proposal[waypoint][coordinate] = bounded[coordinates * waypoint + coordinate];
102 }
103 fitness[member] = trajectory_objective(grid, covariance, mask, start, proposal, options);
104 if (!std::isfinite(fitness[member])) throw std::runtime_error("Nonfinite CMA-ES objective");
105 if (fitness[member] < best_value) { best_value = fitness[member]; best = std::move(proposal); }
106 }
107 evaluations += options.popsize;
108 cmaes_UpdateDistribution(&optimizer.state, fitness);
109 if (cmaes_TestForTermination(&optimizer.state)) break;
110 }
111 return best;
112}
113
114std::vector<Point> plan(const Grid& grid, const Vector& mean, const Matrix& covariance,
115 const Point& start, const PlannerOptions& options) {
116 auto initial = grid_search(grid, mean, covariance, start, options);
117 return options.refine ? refine(grid, mean, covariance, start, initial, options) : initial;
118}
119}
void cmaes_boundary_transformation_exit(cmaes_boundary_transformation_t *t)
void cmaes_boundary_transformation_init(cmaes_boundary_transformation_t *t, double const *lower_bounds, double const *upper_bounds, unsigned long len_of_bounds)
void cmaes_boundary_transformation_inverse(cmaes_boundary_transformation_t *t, double const *x, double *y, unsigned long len)
void cmaes_boundary_transformation(cmaes_boundary_transformation_t *t, double const *x, double *y, unsigned long len)
double * cmaes_UpdateDistribution(cmaes_t *t, const double *rgFunVal)
Definition cmaes.c:810
double *const * cmaes_SamplePopulation(cmaes_t *t)
Definition cmaes.c:625
double * cmaes_init_final(cmaes_t *t)
Definition cmaes.c:291
void cmaes_init_para(cmaes_t *t, int dimension, double *inxstart, double *inrgstddev, long int inseed, int lambda, const char *input_parameter_filename)
Definition cmaes.c:277
void cmaes_exit(cmaes_t *t)
Definition cmaes.c:565
const char * cmaes_TestForTermination(cmaes_t *t)
Definition cmaes.c:1548
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.
static atomic_uint_fast64_t generation
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.
std::vector< bool > importance_mask(const Vector &mean, const Matrix &covariance, const PlannerOptions &options)
Select cells whose confidence bounds satisfy the utility threshold.
math::Point Point
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
math::Vector Vector
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.
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.
Search, utility, and optimizer controls for the reference planner.
Validated capture and reporting policy, fixed before the sample loop.
Definition earcam.c:137