Marginalization Utils

[wxliuu]

How to use symforce to generate marginalization prior factors?
In ther words, how to use symforce to add marginalization factors to the optimizer?

[aaron-skydio]

The tools are all there, but we don't currently have them bundled together into a single function or set of functions to call for this. The process would with the current API would be something like:

1. Collect the sym::Factors touching the variables you want to marginalize
2. Use a sym::Linearizer to linearize those factors (you could do this manually also, but using the Linearizer is probably easier)
3. Eliminate the marginalized variables in the resulting linearization. The sym::SchurComplementSolver doesn't quite expose what you'd need to do this, so for now you'd do this just with e.g. linear algebra ops from Eigen
4. Create a new sym::Factor with the resulting linearization after elimination, and use that in the next optimization

[empty-spacebar]

The tools are all there, but we don't currently have them bundled together into a single function or set of functions to call for this. The process would with the current API would be something like:

1. Collect the sym::Factors touching the variables you want to marginalize
2. Use a sym::Linearizer to linearize those factors (you could do this manually also, but using the Linearizer is probably easier)
3. Eliminate the marginalized variables in the resulting linearization. The sym::SchurComplementSolver doesn't quite expose what you'd need to do this, so for now you'd do this just with e.g. linear algebra ops from Eigen
4. Create a new sym::Factor with the resulting linearization after elimination, and use that in the next optimization

Symforce doesn't have an official implementation of marginalization, is it because that the manipulation of matrix which is no longer sparse after being marginalized, can't get noticeable accelerated with symforce?

[wxliuu]

Thanks!

[aaron-skydio]

Symforce doesn't have an official implementation of marginalization, is it because that the manipulation of matrix which is no longer sparse after being marginalized, can't get noticeable accelerated with symforce?

We do plan to add some utilities to make this nicer at some point, there are lots of reasons you’d want to do this with SymForce. Symforce supports both dense factors (most factors are dense by default) and full dense problems (with the DenseLinearizer). Even with dense problems, you benefit from common subexpression elimination, function flattening, and zero-overhead ops in SymForce, as described in sections IV.A and IV.C in the paper.

[empty-spacebar]

looking forward to symforce being the BEST Solver. Cheers

[asa]

The tools are all there, but we don't currently have them bundled together into a single function or set of functions to call for this. The process would with the current API would be something like:

1. Collect the sym::Factors touching the variables you want to marginalize
2. Use a sym::Linearizer to linearize those factors (you could do this manually also, but using the Linearizer is probably easier)
3. Eliminate the marginalized variables in the resulting linearization. The sym::SchurComplementSolver doesn't quite expose what you'd need to do this, so for now you'd do this just with e.g. linear algebra ops from Eigen
4. Create a new sym::Factor with the resulting linearization after elimination, and use that in the next optimization

Would it make sense to have a version of a Factor, that would take a Linearized{Dense|Sparse}Factor ( linearized_sparse_factor_t and linearized_dense_factor_t) and use it directly during the Linearize() call? From my read there isn't a type that can jump past the Linearize(values, linearized_factor, maybe_index_entry_cache) step for factor. I have a version working with the Hessian Functor type, that copies over the linearization elements, but seems like it would be a little more straightforward to just support this existing type directly?

I believe what you are proposing is that the linearization wouldn't ever change, this linearized factor is fixed at its elimination linearization.

Does that make sense?

[asa]

I believe this may be what Hayk is referring to here: https://github.com/symforce-org/symforce/blob/main/symforce/opt/factor.cc#L92 and https://github.com/symforce-org/symforce/blob/main/symforce/opt/factor.cc#L106

[aaron-skydio]

I believe this may be what Hayk is referring to here: https://github.com/symforce-org/symforce/blob/main/symforce/opt/factor.cc#L92 and https://github.com/symforce-org/symforce/blob/main/symforce/opt/factor.cc#L106

The comments there are just about function signature, and whether those functions should take four parameters like they do now, vs a struct as a single parameter

Would it make sense to have a version of a Factor, that would take a Linearized{Dense|Sparse}Factor ( linearized_sparse_factor_t and linearized_dense_factor_t) and use it directly during the Linearize() call? From my read there isn't a type that can jump past the Linearize(values, linearized_factor, maybe_index_entry_cache) step for factor. I have a version working with the Hessian Functor type, that copies over the linearization elements, but seems like it would be a little more straightforward to just support this existing type directly?

So, there needs to be something that computes at least the updated residual and rhs for the current Values, given the fixed linearization and the linearization point? Presumably that's what you've implemented as the thing you're handing to the existing constructor? We could add something that takes a Linearized{Dense | Sparse}Factor, and the linearization point, and returns a sym::Factor object though, that would be nice

[asa]

Yep, that is along the lines of what I was thinking. I'll see about adding what you describe in a generic way where the linearization point is a tangent vector member of the new factor. Could have std::vector and std::vector<index_entry_t> interface versions as well I think. I'll play with it.

[asa]

I have it boiled down to a hessian lambda like this:

auto gen_factor = [](const Eigen::VectorXd& linearization_point,
                       const std::vector<sym::Key>& linearized_keys,
                       const auto& linearized_factor) -> sym::Factord {
     return sym::Factord::Hessian(
         [linearization_point = std::move(linearization_point),
          // cache Jt, J and hess as they don't change
          jacobian_transpose = Eigen::MatrixXd(linearized_factor.jacobian.transpose()),
          jacobian = std::move(linearized_factor.jacobian),
          hessian = std::move(linearized_factor.hessian)](const sym::Pose3d& x,
                                                          Eigen::VectorXd* res,
                                                          Eigen::MatrixXd* jac,
                                                          Eigen::MatrixXd* hess,
                                                          Eigen::VectorXd* rhs) {
             // assume simple pose state. This needs to be improved to handle polymorphic keys / more complicated tangent_vec
             Eigen::VectorXd tangent_vec(6);
             tangent_vec.head(6) = x.ToTangent();

             // update residual based on current iteration state
             *res = jacobian * (tangent_vec - linearization_point);
             *rhs = jacobian_transpose * (*res);

             // Fixed by the linearization at which the linearized_factor was
             // marginalized
             *jac = jacobian;
             *hess = hessian;
         },
         // the same keys used in linearization are the keys to optimize
         linearized_keys);
};

However this doesn't have a general interface for pulling out the optimized keys and turning them into the tangent_vec for the internal loop of the factor. I could use some guidance on what you think the best path forward on getting that generic through the substantial machinery available in Factor. I started in on a constructor for sym::Factor that would produce this hessian_func, but I'll have to revisit again when I have some bandwidth. Feedback welcome!