1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
use crate::prelude::*;
use serde::{Deserialize, Serialize};
use std::default::Default;
#[derive(Serialize, Deserialize)]
pub struct GaussNewton {
gamma: f64,
}
impl GaussNewton {
pub fn new() -> Self {
GaussNewton { gamma: 1.0 }
}
pub fn gamma(mut self, gamma: f64) -> Result<Self, Error> {
if gamma <= 0.0 || gamma > 1.0 {
return Err(ArgminError::InvalidParameter {
text: "Gauss-Newton: gamma must be in (0, 1].".to_string(),
}
.into());
}
self.gamma = gamma;
Ok(self)
}
}
impl Default for GaussNewton {
fn default() -> GaussNewton {
GaussNewton::new()
}
}
impl<O> Solver<O> for GaussNewton
where
O: ArgminOp,
O::Param: Default
+ ArgminScaledSub<O::Param, f64, O::Param>
+ ArgminSub<O::Param, O::Param>
+ ArgminMul<f64, O::Param>,
O::Output: ArgminNorm<f64>,
O::Jacobian: ArgminTranspose
+ ArgminInv<O::Jacobian>
+ ArgminDot<O::Jacobian, O::Jacobian>
+ ArgminDot<O::Output, O::Param>
+ ArgminDot<O::Param, O::Param>,
O::Hessian: Default,
{
const NAME: &'static str = "Gauss-Newton method";
fn next_iter(
&mut self,
op: &mut OpWrapper<O>,
state: &IterState<O>,
) -> Result<ArgminIterData<O>, Error> {
let param = state.get_param();
let residuals = op.apply(¶m)?;
let jacobian = op.jacobian(¶m)?;
let p = jacobian
.clone()
.t()
.dot(&jacobian)
.inv()?
.dot(&jacobian.t().dot(&residuals));
let new_param = param.sub(&p.mul(&self.gamma));
Ok(ArgminIterData::new()
.param(new_param)
.cost(residuals.norm()))
}
fn terminate(&mut self, state: &IterState<O>) -> TerminationReason {
if (state.get_prev_cost() - state.get_cost()).abs() < std::f64::EPSILON.sqrt() {
return TerminationReason::NoChangeInCost;
}
TerminationReason::NotTerminated
}
}
#[cfg(test)]
mod tests {
use super::*;
use crate::send_sync_test;
send_sync_test!(gauss_newton_method, GaussNewton);
}