Missing demontration of spline optimization and/or lie group normalize function

[cedricpradalier]

Hello, I have been trying to port a bunch of helper functions to build optimisation problems in continuous time from Sophus 1 to Sophus 2. In Sophus 1, there is a PR here: strasdat/Sophus#557 even though the message says it won't be applied.

The sophus 2 code is here https://github.com/cedricpradalier/farm-ng-core/tree/porting_spline_error

The goal of these helper functions is to apply seemlessly functors f(g) with g a Lie Group object on a spline sampled at t, without having to manage the knot selection at the user level. Example:
addResidualFunction0<Functor,kDof>(problem,t,spline,functor);

The Sophus 1 code is working well. The Sophus 2 code for cartesian splines is also working ok, but in the lie group spline code, I end up having groups with invalid parameters. In Sophus 1, I solved that by calling the normalize function after each ceres iteration, but this does not exist in Sophus 2 (and theoretically, we should not need that with the manifold parametrization).

Is there a code example somewhere showing continuous-time optimisation with Sophus 2 to check what I'm doing wrong?
Thanks

[cedricpradalier]

I solved some of my problems, which were mostly due to the manifold parametrization not being applied. For some reason, the following did not work:

            for (auto v: spline->parentFromsControlPoint()) {
                problem.AddParameterBlock(v.ptr(), Group::kNumParams, parametrization);
            }

but this worked:

            for (size_t i=0;i<spline->parentFromsControlPoint().size();i++) {
                problem.AddParameterBlock(spline->parentFromsControlPoint()[i].ptr(), Group::kNumParams, parametrization);
            }

Now all the tests are working except for scaling, where the optimization seems to get away from a valid representation.

[cedricpradalier]

The last commit in https://github.com/cedricpradalier/farm-ng-core/tree/porting_spline_error solves the last errors. Scalings were not working because I was sampling random elements that were too small or too large.

This commit demonstrates the application of generic ceres functors to splines without expliciting the knot indices. In the test, this is used in a spline approximation for all the Sophus 2 lie groups, and cartesian vectors up to 4d.

Given that the last commit on this repo is fairly old, I am wondering if there is an interest for a PR. Please let me know.