symforce-org / symforce

Fast symbolic computation, code generation, and nonlinear optimization for robotics
https://symforce.org
Apache License 2.0
1.41k stars 145 forks source link

Add diagonal & isotropic sqrt_infos in geo factors #294

Open bradley-solliday-skydio opened 1 year ago

bradley-solliday-skydio commented 1 year ago

Often times the square root information matrix is a diagonal or isotropic matrix. However, in our generated geo factors we always assume they are full dense matrices leading to many more operations than needed.

This commit modifies geo_factors_codegen.py to create 3 variants for each geo factor, one which assumes sqrt_info is a square matrix (the existing behavior), one which assumes it is a diagonal matrix, and one which assumes it is an isotropic matrix.

These variants will have the same base name, with nothing appended to the square variant, Diagonal appended to the diaognal variant, and Isotropic appended to the isotropic covariant. They cannot have the same name, as even though we only generate the factors in C++ which has function overloading, disambiguating the overloads is a pain when constructing sym::Factors. The square version takes a square eigen matrix, the diagonal version a eigen vector whose entries are those of the diagonal of sqrt_info, and a single scalar for the isotropic version.

The header that the user imports remains the same, except now rather than containing the implementation directly, it re-imports the headers for the 3 different versions (which are stored in sub-directories).

Topic: sqrt_info_variants Relative: geo_factors_use_skip_directory_nesting

bradley-solliday-skydio commented 1 year ago

Reviews in this chain: └https://github.com/symforce-org/symforce/pull/295 Simplify geo_factors_codegen w/ skip_dir_nesting  └https://github.com/symforce-org/symforce/pull/294 Add diagonal & isotropic sqrt_infos in geo factors

bradley-solliday-skydio commented 1 year ago
# head base diff date summary
0 50c4a3af f9a6f2d0 diff Jan 20 16:14 PM 57 files changed, 9704 insertions(+), 3915 deletions(-)
1 cf820425 f9a6f2d0 diff Jan 20 17:06 PM 43 files changed, 1 insertion(+), 1 deletion(-)
2 8e775eea f9a6f2d0 diff Feb 1 10:39 AM 57 files changed, 211 insertions(+), 196 deletions(-)