Fixed handling of residuals in GaussNewton

argmin/src/solver/gaussnewton/gaussnewton_method.rs

@@ -124,29 +124,50 @@ where
 {
     const NAME: &'static str = "Gauss-Newton method";
 
-    fn next_iter(
+    fn init(
         &mut self,
         problem: &mut Problem<O>,
-        state: IterState<P, (), J, (), R, F>,
+        mut state: IterState<P, (), J, (), R, F>,
     ) -> Result<(IterState<P, (), J, (), R, F>, Option<KV>), Error> {
-        let param = state.get_param().ok_or_else(argmin_error_closure!(
+        let init_param = state.take_param().ok_or_else(argmin_error_closure!(
             NotInitialized,
             concat!(
                 "`GaussNewton` requires an initial parameter vector. ",
                 "Please provide an initial guess via `Executor`s `configure` method."
             )
         ))?;
-        let residuals = problem.apply(param)?;
+        let residuals = problem.apply(&init_param)?;
+        let cost = residuals.l2_norm();
+        Ok((
+            state.param(init_param).residuals(residuals).cost(cost),
+            None,
+        ))
+    }
+
+    fn next_iter(
+        &mut self,
+        problem: &mut Problem<O>,
+        state: IterState<P, (), J, (), R, F>,
+    ) -> Result<(IterState<P, (), J, (), R, F>, Option<KV>), Error> {
+        let param = state.get_param().ok_or_else(argmin_error_closure!(
+            PotentialBug,
+            "`GaussNewton`: `param` not set"
+        ))?;
+        let residuals = state.get_residuals().ok_or_else(argmin_error_closure!(
+            PotentialBug,
+            "`GaussNewton`: `residuals` not set"
+        ))?;
         let jacobian = problem.jacobian(param)?;
 
         let p = jacobian
             .clone()
             .t()
             .dot(&jacobian)
             .inv()?
-            .dot(&jacobian.t().dot(&residuals));
+            .dot(&jacobian.t().dot(residuals));
 
         let new_param = param.sub(&p.mul(&self.gamma));
+        let residuals = problem.apply(&new_param)?;
 
         let cost = residuals.l2_norm();