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!

Marginalization Utils #395

Description

Metadata

Metadata

Assignees

Labels

Type

Projects

Milestone

Relationships

Development

Issue actions