1mod graph;
8
9use std::sync::Mutex;
10
11use nalgebra::DMatrix;
12
13use crate::autodiff::{Dual, Scalar, Tape, Value};
14use crate::error::{Error, Result};
15use crate::linalg::RMat;
16use crate::optim::{Eval, Objective};
17use crate::setup::Problem;
18
19pub struct CostModel {
21 problem: Problem,
22 pool: crate::parallel::Pool,
23 cache: Mutex<Option<Cached>>,
26}
27
28struct Cached {
29 x: Vec<f64>,
30 eval: Eval,
31 grad: Option<Vec<f64>>,
32 hess: Option<DMatrix<f64>>,
33}
34
35#[derive(Clone, Debug, PartialEq)]
37pub struct Waveforms {
38 pub cx: RMat<f64>,
40 pub cy: RMat<f64>,
42 pub amp: RMat<f64>,
44 pub phase: RMat<f64>,
46}
47
48impl CostModel {
49 pub fn new(problem: Problem) -> Self {
51 CostModel {
52 problem,
53 pool: crate::parallel::Pool::default(),
54 cache: Mutex::new(None),
55 }
56 }
57
58 pub fn with_threads(mut self, threads: usize) -> Result<Self> {
61 self.pool = crate::parallel::Pool::with_threads(threads)?;
62 Ok(self)
63 }
64
65 pub fn problem(&self) -> &Problem {
67 &self.problem
68 }
69
70 pub fn dim(&self) -> usize {
72 self.problem.n_params()
73 }
74
75 fn check_len(&self, n: usize) -> Result<()> {
76 if n != self.dim() {
77 return Err(Error::Dimension(format!(
78 "{n} parameters for a problem with {}",
79 self.dim()
80 )));
81 }
82 Ok(())
83 }
84
85 pub fn eval_generic<T: Scalar>(&self, x: &[T], grad: bool) -> Result<(T, T, T, Option<Vec<T>>)> {
87 self.check_len(x.len())?;
88 let mut tape: Tape<'_, T> = if grad { Tape::new() } else { Tape::no_grad() };
89 let xv = if grad {
90 tape.leaf(Value::R(RMat::column(x.to_vec())))
91 } else {
92 tape.constant(Value::R(RMat::column(x.to_vec())))
93 };
94 let problem = &self.problem;
95 let (cost, fid, pen) = self.pool.install(|| graph::build_cost(problem, &mut tape, xv))?;
96 let values = (tape.scalar(cost)?, tape.scalar(fid)?, tape.scalar(pen)?);
97 let g = if grad {
98 Some(tape.backward(cost)?.wrt_real(xv, x.len()))
99 } else {
100 None
101 };
102 Ok((values.0, values.1, values.2, g))
103 }
104
105 pub fn value(&self, x: &[f64]) -> Result<Eval> {
107 let (cost, fidelity, penalty, _) = self.eval_generic(x, false)?;
108 finite(Eval {
109 cost,
110 fidelity,
111 penalty,
112 })
113 }
114
115 pub fn value_and_gradient(&self, x: &[f64]) -> Result<(Eval, Vec<f64>)> {
117 let (cost, fidelity, penalty, g) = self.eval_generic(x, true)?;
118 let g = g.unwrap_or_default();
119 if g.iter().any(|v| !v.is_finite()) {
120 return Err(Error::Numerical("the gradient is not finite".into()));
121 }
122 Ok((
123 finite(Eval {
124 cost,
125 fidelity,
126 penalty,
127 })?,
128 g,
129 ))
130 }
131
132 pub fn hvp(&self, x: &[f64], v: &[f64]) -> Result<Vec<f64>> {
134 self.check_len(v.len())?;
135 let xd: Vec<Dual> = x.iter().zip(v).map(|(&a, &d)| Dual::new(a, d)).collect();
136 let (_, _, _, g) = self.eval_generic(&xd, true)?;
137 Ok(g.unwrap_or_default().iter().map(|d| d.eps).collect())
138 }
139
140 pub fn hessian_matrix(&self, x: &[f64]) -> Result<DMatrix<f64>> {
143 let n = self.dim();
144 self.check_len(x.len())?;
145 let columns = self.pool.install(|| {
146 crate::parallel::map(n, |i| {
147 let mut e = vec![0.0; n];
148 e[i] = 1.0;
149 self.hvp(x, &e)
150 })
151 });
152 let mut h = DMatrix::zeros(n, n);
153 for (i, col) in columns.into_iter().enumerate() {
154 for (r, v) in col?.into_iter().enumerate() {
155 h[(r, i)] = v;
156 }
157 }
158 let sym = (&h + h.transpose()) * 0.5;
159 if sym.iter().any(|v| !v.is_finite()) {
160 return Err(Error::Numerical("the Hessian is not finite".into()));
161 }
162 Ok(sym)
163 }
164
165 pub fn waveforms(&self, x: &[f64]) -> Result<Waveforms> {
167 self.check_len(x.len())?;
168 let mut tape: Tape<'_, f64> = Tape::no_grad();
169 let xv = tape.constant(Value::R(RMat::column(x.to_vec())));
170 let w = graph::build_waveforms(&self.problem, &mut tape, xv)?;
171 let cx = tape.value(w.cx).r("waveforms")?.clone();
172 let cy = tape.value(w.cy).r("waveforms")?.clone();
173 let amp = RMat::from_fn(cx.rows, cx.cols, |r, c| cx.get(r, c).hypot(cy.get(r, c)));
174 let phase = RMat::from_fn(cx.rows, cx.cols, |r, c| cy.get(r, c).atan2(cx.get(r, c)));
175 Ok(Waveforms { cx, cy, amp, phase })
176 }
177}
178
179impl CostModel {
180 fn cached<R>(&self, x: &[f64], f: impl FnOnce(&Cached) -> Option<R>) -> Option<R> {
181 let guard = self.cache.lock().unwrap_or_else(|p| p.into_inner());
182 guard.as_ref().filter(|c| c.x == x).and_then(f)
183 }
184
185 fn store(&self, x: &[f64], eval: Eval, grad: Option<Vec<f64>>, hess: Option<DMatrix<f64>>) {
186 let mut guard = self.cache.lock().unwrap_or_else(|p| p.into_inner());
187 match guard.as_mut().filter(|c| c.x == x) {
188 Some(c) => {
189 c.grad = grad.or(c.grad.take());
190 c.hess = hess.or(c.hess.take());
191 }
192 None => {
193 *guard = Some(Cached {
194 x: x.to_vec(),
195 eval,
196 grad,
197 hess,
198 })
199 }
200 }
201 }
202}
203
204impl Objective for CostModel {
205 fn dim(&self) -> usize {
206 CostModel::dim(self)
207 }
208
209 fn value(&self, x: &[f64]) -> Result<Eval> {
210 if let Some(e) = self.cached(x, |c| Some(c.eval)) {
211 return Ok(e);
212 }
213 let e = CostModel::value(self, x)?;
214 self.store(x, e, None, None);
215 Ok(e)
216 }
217
218 fn gradient(&self, x: &[f64]) -> Result<(Eval, Vec<f64>)> {
219 if let Some(hit) = self.cached(x, |c| c.grad.clone().map(|g| (c.eval, g))) {
220 return Ok(hit);
221 }
222 let (e, g) = self.value_and_gradient(x)?;
223 self.store(x, e, Some(g.clone()), None);
224 Ok((e, g))
225 }
226
227 fn hessian(&self, x: &[f64]) -> Result<(Eval, Vec<f64>, DMatrix<f64>)> {
228 if let Some(hit) = self.cached(x, |c| match (&c.grad, &c.hess) {
229 (Some(g), Some(h)) => Some((c.eval, g.clone(), h.clone())),
230 _ => None,
231 }) {
232 return Ok(hit);
233 }
234 let (e, g) = Objective::gradient(self, x)?;
235 let h = self.hessian_matrix(x)?;
236 self.store(x, e, None, Some(h.clone()));
237 Ok((e, g, h))
238 }
239
240 fn hvp(&self, x: &[f64], v: &[f64]) -> Result<Vec<f64>> {
241 CostModel::hvp(self, x, v)
242 }
243}
244
245fn finite(e: Eval) -> Result<Eval> {
246 if e.cost.is_finite() && e.fidelity.is_finite() && e.penalty.is_finite() {
247 Ok(e)
248 } else {
249 Err(Error::Numerical("the objective is not finite".into()))
250 }
251}
252
253#[cfg(test)]
254mod tests;