[SymForce] Add test for LinearDeltaError

Also stores the update in the stats when debug_stats is on, makes the
stored residual and jacobian double precision, and clarifies when some
of the debug stats are present.

Topic: sf-linear-test
GitOrigin-RevId: 841fc05473dd8b68123efd8d528237d65db45965

lcmtypes/symforce.lcm

@@ -206,16 +206,19 @@ struct optimization_iteration_t {
   // Angle between previous update and current update
   double update_angle_change;
 
-  // The values, residual, and jacobian are only populated when debug_stats is true,
+  // The update, values, residual, and jacobian are only populated when debug_stats is true,
   // otherwise they are size 0
 
+  // The update at this step
+  eigen_lcm.VectorXd update;
+
   // The Values at this step
   values_t values;
 
   // The problem residual
-  eigen_lcm.VectorXf residual;
+  eigen_lcm.VectorXd residual;
   // The problem jacobian exactly if dense, or as CSC format sparse data column vector if sparse
-  eigen_lcm.MatrixXf jacobian_values;
+  eigen_lcm.MatrixXd jacobian_values;
 }
 
 // The structure of a sparse matrix in CSC format, not including the numerical values
@@ -232,7 +235,6 @@ struct sparse_matrix_structure_t {
   // The shape (M, N) of the sparse matrix
   int64_t shape[2];
 }
-
 #protobuf
 enum optimization_status_t {
   // Uninitialized enum value
@@ -270,13 +272,26 @@ struct optimization_stats_t {
   // for the nonlinear solver you used.
   int32_t failure_reason;
 
-  // The sparsity pattern of the jacobian, filled out if debug_stats=true
+  /// The sparsity pattern of the problem jacobian
+  ///
+  /// Only filled if Optimizer created with debug_stats = true and include_jacobians = true,
+  /// otherwise default constructed.
+  ///
+  /// If using a dense linearization, only the shape field will be filled.
   sparse_matrix_structure_t jacobian_sparsity;
 
-  // The ordering used for the linear solver, filled out if debug_stats=true
+  /// The permutation used by the linear solver
+  ///
+  /// Only filled if using an Optimizer created with debug_stats = true and a linear solver that
+  /// exposes Permutation() (such as the default SparseCholeskySolver).  Otherwise, will be default
+  /// constructed.
   eigen_lcm.VectorXi linear_solver_ordering;
 
-  // The sparsity pattern of the cholesky factor L, filled out if debug_stats=true
+  /// The sparsity pattern of the cholesky factor L
+  ///
+  /// Only filled if using an Optimizer created with debug_stats = true and a linear solver that
+  /// exposes L() (such as the default SparseCholeskySolver).  Otherwise, will be default
+  /// constructed.
   sparse_matrix_structure_t cholesky_factor_sparsity;
 }
 

symforce/opt/assert.h

@@ -36,8 +36,6 @@ inline std::string FormatFailure(const char* error, const char* func, const char
  *
  * Inspiration taken from:
  *     http://cnicholson.net/2009/02/stupid-c-tricks-adventures-in-assert/
- *
- * TODO(hayk): Improve with _EQ variant, etc.
  */
 #ifndef SYMFORCE_DISABLE_ASSERT
 #define SYM_ASSERT(expr, ...)                                                                   \
@@ -47,9 +45,25 @@ inline std::string FormatFailure(const char* error, const char* func, const char
           sym::FormatFailure((#expr), __PRETTY_FUNCTION__, __FILE__, __LINE__, ##__VA_ARGS__)); \
     }                                                                                           \
   } while (0)
+
+#define SYM_ASSERT_EQ(a, b, ...)                                                          \
+  do {                                                                                    \
+    if ((a) != (b)) {                                                                     \
+      throw std::runtime_error(                                                           \
+          sym::FormatFailure(fmt::format((#a " == " #b " ({} == {})"), (a), (b)).c_str(), \
+                             __PRETTY_FUNCTION__, __FILE__, __LINE__, ##__VA_ARGS__));    \
+    }                                                                                     \
+  } while (0)
+
 #else
 #define SYM_ASSERT(expr, ...) \
   do {                        \
     (void)sizeof(expr);       \
   } while (0)
+
+#define SYM_ASSERT_EQ(a, b, ... /*fmt, message args*/) \
+  do {                                                 \
+    (void)sizeof(a);                                   \
+    (void)sizeof(b);                                   \
+  } while (0)
 #endif

symforce/opt/levenberg_marquardt_solver.tcc

@@ -121,11 +121,12 @@ void LevenbergMarquardtSolver<ScalarType, LinearSolverType>::PopulateIterationSt
   }
 
   if (p_.debug_stats) {
+    iteration_stats.update = update_.template cast<double>();
     iteration_stats.values = state.New().values.template Cast<double>().GetLcmType();
     const VectorX<Scalar> residual_vec = state.New().GetLinearization().residual;
-    iteration_stats.residual = residual_vec.template cast<float>();
+    iteration_stats.residual = residual_vec.template cast<double>();
     const MatrixX<Scalar> jacobian_vec = JacobianValues(state.New().GetLinearization().jacobian);
-    iteration_stats.jacobian_values = jacobian_vec.template cast<float>();
+    iteration_stats.jacobian_values = jacobian_vec.template cast<double>();
   }
 }
 
@@ -191,10 +192,10 @@ LevenbergMarquardtSolver<ScalarType, LinearSolverType>::Iterate(
     if (p_.debug_stats) {
       iteration_stats.values = state_.Init().values.template Cast<double>().GetLcmType();
       const VectorX<Scalar> residual_vec = state_.Init().GetLinearization().residual;
-      iteration_stats.residual = residual_vec.template cast<float>();
+      iteration_stats.residual = residual_vec.template cast<double>();
       const MatrixX<Scalar> jacobian_vec =
           JacobianValues(state_.Init().GetLinearization().jacobian);
-      iteration_stats.jacobian_values = jacobian_vec.template cast<float>();
+      iteration_stats.jacobian_values = jacobian_vec.template cast<double>();
     }
 
     if (!std::isfinite(state_.Init().Error())) {
@@ -225,8 +226,9 @@ LevenbergMarquardtSolver<ScalarType, LinearSolverType>::Iterate(
     SYM_ASSERT(success, "Internal Error: Damped hessian factorization failed");
 
     // NOTE(aaron): This has to happen after the first factorize, since L_inner is not filled out
-    // by ComputeSymbolicSparsity
-    if (p_.debug_stats && stats.linear_solver_ordering.size() == 0) {
+    // by ComputeSymbolicSparsity.  The linear_solver may return an empty result for either of
+    // these, so the only way to know we haven't filled it out yet is the iteration number.
+    if (p_.debug_stats && iteration_ == 0) {
       stats.linear_solver_ordering = linear_solver_.Permutation().indices();
       stats.cholesky_factor_sparsity = GetSparseStructure(linear_solver_.L());
     }

symforce/opt/linearization.h

@@ -10,6 +10,7 @@
 
 #include <lcmtypes/sym/sparse_matrix_structure_t.hpp>
 
+#include <sym/util/type_ops.h>
 #include <sym/util/typedefs.h>
 
 #include "./assert.h"
@@ -102,8 +103,68 @@ sparse_matrix_structure_t GetSparseStructure(const Eigen::SparseMatrix<Scalar>&
  * Return a default initialized sparse structure because arg is dense.
  */
 template <typename Derived>
-sparse_matrix_structure_t GetSparseStructure(const Eigen::EigenBase<Derived>&) {
-  return {};
+sparse_matrix_structure_t GetSparseStructure(const Eigen::EigenBase<Derived>& matrix) {
+  return {{}, {}, {matrix.rows(), matrix.cols()}};
+}
+
+/**
+ * Returns the reconstructed matrix from the stored values and sparse_matrix_structure_t
+ *
+ * Useful for reconstructing the jacobian for an iteration in the OptimizationStats
+ *
+ * The result contains references to structure and values, so its lifetime must be shorter than
+ * those.
+ *
+ * @param rows The number of rows in the matrix (must match structure.shape for sparse matrices)
+ * @param cols The number of columns in the matrix (must match structure.shape for sparse matrices)
+ * @param structure The sparsity pattern of the matrix, as obtained from GetSparseStructure or
+ * stored in OptimizationStats::iterations.  For dense matrices, this should be empty.
+ * @param values The coefficients of the matrix; flat for sparse matrices, 2d for dense
+ * @tparam MatrixType The type of the matrix used to decide if the result is sparse or dense.  This
+ * is not otherwise used
+ * @tparam Scalar The scalar type of the result (must match the scalar type of values)
+ */
+template <typename MatrixType, typename Scalar,
+          typename std::enable_if_t<kIsSparseEigenType<MatrixType>, bool> = true>
+Eigen::Map<const Eigen::SparseMatrix<Scalar>> MatrixViewFromSparseStructure(
+    const sparse_matrix_structure_t& structure, const MatrixX<Scalar>& values) {
+  SYM_ASSERT_EQ(structure.shape.size(), 2, "Invalid shape for sparse matrix: {}", structure.shape);
+  SYM_ASSERT_EQ(structure.column_pointers.size(), structure.shape.at(1));
+  SYM_ASSERT_EQ(structure.row_indices.size(), values.rows());
+  SYM_ASSERT_EQ(values.cols(), 1);
+  return {structure.shape.at(0),        structure.shape.at(1),
+          structure.row_indices.size(), structure.column_pointers.data(),
+          structure.row_indices.data(), values.data()};
+}
+
+/**
+ * Returns the reconstructed matrix from the stored values and sparse_matrix_structure_t
+ *
+ * Useful for reconstructing the jacobian for an iteration in the OptimizationStats
+ *
+ * The result contains references to structure and values, so its lifetime must be shorter than
+ * those.
+ *
+ * @param rows The number of rows in the matrix (must match structure.shape for sparse matrices)
+ * @param cols The number of columns in the matrix (must match structure.shape for sparse matrices)
+ * @param structure The sparsity pattern of the matrix, as obtained from GetSparseStructure or
+ * stored in OptimizationStats::iterations.  For dense matrices, this should be empty.
+ * @param values The coefficients of the matrix; flat for sparse matrices, 2d for dense
+ * @tparam MatrixType The type of the matrix used to decide if the result is sparse or dense.  This
+ * is not otherwise used
+ * @tparam Scalar The scalar type of the result (must match the scalar type of values)
+ */
+template <typename MatrixType, typename Scalar,
+          typename std::enable_if_t<kIsEigenType<MatrixType>, bool> = true>
+Eigen::Map<const MatrixX<Scalar>> MatrixViewFromSparseStructure(
+    const sparse_matrix_structure_t& structure, const MatrixX<Scalar>& values) {
+  SYM_ASSERT_EQ(structure.shape.size(), 2, "Invalid shape for dense matrix: {}", structure.shape);
+  SYM_ASSERT_EQ(structure.column_pointers.size(), 0,
+                "column_pointers must be empty for dense matrix");
+  SYM_ASSERT_EQ(structure.row_indices.size(), 0, "row_indices must be empty for dense matrix");
+  SYM_ASSERT_EQ(values.rows(), structure.shape.at(0));
+  SYM_ASSERT_EQ(values.cols(), structure.shape.at(1));
+  return {values.data(), values.rows(), values.cols()};
 }
 
 /**

symforce/opt/optimization_stats.h

@@ -33,35 +33,46 @@ struct OptimizationStats {
   int32_t failure_reason{};
 
   /// The linearization at best_index (at optimized_values), filled out if
-  /// populate_best_linearization=true
+  /// populate_best_linearization = true
   optional<Linearization<MatrixType>> best_linearization{};
 
   /// The sparsity pattern of the problem jacobian
   ///
-  /// Only filled if using sparse linear solver and Optimizer created with debug_stats = true.
-  /// If not filled, row_indices field of sparse_matrix_structure_t and linear_solver_ordering
-  /// will have size() = 0.
-  sparse_matrix_structure_t jacobian_sparsity;
+  /// Only filled if Optimizer created with debug_stats = true and include_jacobians = true,
+  /// otherwise default constructed.
+  ///
+  /// If using a dense linearization, only the shape field will be filled.
+  sparse_matrix_structure_t jacobian_sparsity{};
 
   /// The permutation used by the linear solver
   ///
-  /// Only filled if using sparse linear solver and Optimizer created with debug_stats = true.
-  /// If not filled, row_indices field of sparse_matrix_structure_t and linear_solver_ordering
-  /// will have size() = 0.
-  Eigen::VectorXi linear_solver_ordering;
+  /// Only filled if using an Optimizer created with debug_stats = true and a linear solver that
+  /// exposes Permutation() (such as the default SparseCholeskySolver).  Otherwise, will be default
+  /// constructed.
+  Eigen::VectorXi linear_solver_ordering{};
 
-  /// The sparsity pattern of the cholesky factor
+  /// The sparsity pattern of the cholesky factor L
   ///
-  /// Only filled if using sparse linear solver and Optimizer created with debug_stats = true.
-  /// If not filled, row_indices field of sparse_matrix_structure_t and linear_solver_ordering
-  /// will have size() = 0.
-  sparse_matrix_structure_t cholesky_factor_sparsity;
+  /// Only filled if using an Optimizer created with debug_stats = true and a linear solver that
+  /// exposes L() (such as the default SparseCholeskySolver).  Otherwise, will be default
+  /// constructed.
+  sparse_matrix_structure_t cholesky_factor_sparsity{};
 
   optimization_stats_t GetLcmType() const {
     return optimization_stats_t(iterations, best_index, status, failure_reason, jacobian_sparsity,
                                 linear_solver_ordering, cholesky_factor_sparsity);
   }
 
+  /// Get a view of the Jacobian at a particular iteration
+  ///
+  /// The lifetime of the result is tied to the lifetime of the OptimizationStats object
+  auto JacobianView(const optimization_iteration_t& iteration) const {
+    SYM_ASSERT(
+        jacobian_sparsity.shape.size() == 2,
+        "Jacobian sparsity is empty, did you set debug_stats = true and include_jacobians = true?");
+    return MatrixViewFromSparseStructure<MatrixType>(jacobian_sparsity, iteration.jacobian_values);
+  }
+
   /**
    * Reset the optimization stats
    *

symforce/opt/optimizer.py

@@ -143,20 +143,21 @@ class Result:
                 What was the result of the optimization? (did it converge, fail, etc.)
 
             failure_reason:
-                If status == FAILED, why?
+                If ``status == FAILED``, why?
 
             best_linearization:
                 The linearization at best_index (at optimized_values), filled out if
-                populate_best_linearization=True
+                ``populate_best_linearization=True``
 
             jacobian_sparsity:
-                The sparsity pattern of the jacobian, filled out if debug_stats=True
+                The sparsity pattern of the jacobian, filled out if ``debug_stats=True`` and
+                ``include_jacobians=True``
 
             linear_solver_ordering:
-                The ordering used for the linear solver, filled out if debug_stats=True
+                The ordering used for the linear solver, filled out if ``debug_stats=True``
 
             cholesky_factor_sparsity:
-                The sparsity pattern of the cholesky factor L, filled out if debug_stats=True
+                The sparsity pattern of the cholesky factor L, filled out if ``debug_stats=True``
         """
 
         initial_values: Values

symforce/opt/optimizer.tcc

@@ -327,7 +327,7 @@ void Optimizer<ScalarType, NonlinearSolverType>::IterateToConvergence(
     }
   }
 
-  if (debug_stats_) {
+  if (debug_stats_ && include_jacobians_) {
     const auto& linearization = nonlinear_solver_.GetBestLinearization();
     stats.jacobian_sparsity = GetSparseStructure(linearization.jacobian);
   }

symforce/pybind/cc_optimization_stats.cc

@@ -30,7 +30,8 @@ void AddOptimizationStatsWrapper(pybind11::module_ module) {
                      "If status == FAILED, why?  This should be cast to the "
                      "NonlinearSolver::FailureReason enum for the nonlinear solver you used.")
       .def_readwrite("jacobian_sparsity", &sym::OptimizationStatsd::jacobian_sparsity,
-                     "Sparsity pattern of the problem jacobian (filled out if debug_stats=True)")
+                     "Sparsity pattern of the problem jacobian (filled out if debug_stats=True and "
+                     "include_jacobians=True)")
       .def_readwrite("linear_solver_ordering", &sym::OptimizationStatsd::linear_solver_ordering,
                      "Ordering used by the linear solver (filled out if debug_stats=True)")
       .def_readwrite("cholesky_factor_sparsity", &sym::OptimizationStatsd::cholesky_factor_sparsity,

symforce/pybind/cc_sym.pyi

@@ -343,14 +343,14 @@ class OptimizationStats:
     @property
     def jacobian_sparsity(self) -> sparse_matrix_structure_t:
         """
-        Sparsity pattern of the problem jacobian (filled out if debug_stats=True)
+        Sparsity pattern of the problem jacobian (filled out if debug_stats=True and include_jacobians=True)
 
         :type: sparse_matrix_structure_t
         """
     @jacobian_sparsity.setter
     def jacobian_sparsity(self, arg0: sparse_matrix_structure_t) -> None:
         """
-        Sparsity pattern of the problem jacobian (filled out if debug_stats=True)
+        Sparsity pattern of the problem jacobian (filled out if debug_stats=True and include_jacobians=True)
         """
     @property
     def linear_solver_ordering(self) -> typing.Any:

symforce/test_util/check_linear_error.h

@@ -0,0 +1,40 @@
+/* ----------------------------------------------------------------------------
+ * SymForce - Copyright 2022, Skydio, Inc.
+ * This source code is under the Apache 2.0 license found in the LICENSE file.
+ * ---------------------------------------------------------------------------- */
+
+#pragma once
+
+#include <catch2/catch_approx.hpp>
+#include <catch2/catch_test_macros.hpp>
+
+#include <symforce/opt/optimization_stats.h>
+
+namespace sym {
+
+/// Check that the linear error in the optimization stats matches the linear error computed from the
+/// Jacobian
+///
+/// Use from tests, adds Catch2 checks.  Requires debug_stats and include_jacobians to be turned on.
+template <typename OptimizationStats>
+void CheckLinearError(const OptimizationStats& stats) {
+  const sym::optimization_iteration_t* last_accepted_iteration = &stats.iterations.at(0);
+  for (int i = 1; i < static_cast<int>(stats.iterations.size()); ++i) {
+    const auto& iteration = stats.iterations.at(i);
+
+    const auto J = stats.JacobianView(*last_accepted_iteration).template cast<double>().eval();
+    CAPTURE(J);
+    const Eigen::VectorXd new_residual = J * iteration.update + last_accepted_iteration->residual;
+
+    const auto new_cost = 0.5 * new_residual.squaredNorm();
+
+    CAPTURE(i, last_accepted_iteration->iteration - 1);
+    CHECK(new_cost == Catch::Approx(iteration.new_error_linear).epsilon(1e-6));
+
+    if (iteration.update_accepted) {
+      last_accepted_iteration = &iteration;
+    }
+  }
+}
+
+}  // namespace sym