From a66fc4bbeeca89a6ffb5c34165673231ccfec901 Mon Sep 17 00:00:00 2001 From: brentyi Date: Sun, 13 Oct 2024 02:49:35 -0700 Subject: [PATCH] Fixes --- README.md | 19 +++++++++---------- examples/pose_graph_simple.py | 2 +- src/jaxls/_preconditioning.py | 6 +++--- 3 files changed, 13 insertions(+), 14 deletions(-) diff --git a/README.md b/README.md index 4d2e815..51a8e1f 100644 --- a/README.md +++ b/README.md @@ -11,26 +11,25 @@ problems. We accelerate optimization by analyzing the structure of graphs: repeated factor and variable types are vectorized, and the sparsity of adjacency in the graph is translated into sparse matrix operations. +Use cases are primarily in least squares problems that are (1) sparse and (2) +inefficient to solve with gradient-based methods. + Currently supported: - Automatic sparse Jacobians. -- Optimization on manifolds like SO(2), SO(3), SE(2), and SE(3). +- Optimization on manifolds. + - Examples provided for SO(2), SO(3), SE(2), and SE(3). - Nonlinear solvers: Levenberg-Marquardt and Gauss-Newton. -- Multiple solvers for linear subproblems: +- Linear subproblem solvers: - Sparse direct with Cholesky / CHOLMOD, on CPU. - Sparse iterative with Conjugate Gradient. - Preconditioning: block and point Jacobi. - Inexact Newton via Eisenstat-Walker. - Dense Cholesky for smaller problems. -Use cases are primarily in least squares problems that are inherently (1) -sparse and (2) inefficient to solve with gradient-based methods. These are -common in robotics. - -For the first iteration of this library, written for -[IROS 2021](https://github.com/brentyi/dfgo), see -[jaxfg](https://github.com/brentyi/jaxfg). `jaxls` is a rewrite that aims to be -faster and easier to use. For additional references, see inspirations like +For the first iteration of this library, written for [IROS 2021](https://github.com/brentyi/dfgo), see +[jaxfg](https://github.com/brentyi/jaxfg). `jaxls` is a rewrite that is faster +and easier to use. For additional references, see inspirations like [GTSAM](https://gtsam.org/), [Ceres Solver](http://ceres-solver.org/), [minisam](https://github.com/dongjing3309/minisam), [SwiftFusion](https://github.com/borglab/SwiftFusion), diff --git a/examples/pose_graph_simple.py b/examples/pose_graph_simple.py index 436c26c..b0829c9 100644 --- a/examples/pose_graph_simple.py +++ b/examples/pose_graph_simple.py @@ -48,7 +48,7 @@ graph = jaxls.FactorGraph.make(factors, vars) # Solve the optimization problem. -solution = graph.solve(linear_solver=jaxls.DenseLinearSolver()) +solution = graph.solve() print("All solutions", solution) print("Pose 0", solution[vars[0]]) print("Pose 1", solution[vars[1]]) diff --git a/src/jaxls/_preconditioning.py b/src/jaxls/_preconditioning.py index 9f9c9fe..2368740 100644 --- a/src/jaxls/_preconditioning.py +++ b/src/jaxls/_preconditioning.py @@ -25,9 +25,9 @@ def make_point_jacobi_precoditioner( block_l2_cols = jnp.sum(block_row.blocks_concat**2, axis=1).flatten() indices = jnp.concatenate( [ - (start_col[:, None] + jnp.arange(width)[None, :]) - for start_col, width in zip( - block_row.start_cols, block_row.block_widths + (start_col[:, None] + jnp.arange(block_cols)[None, :]) + for start_col, block_cols in zip( + block_row.start_cols, block_row.block_num_cols ) ], axis=1,