Fix Ceres 2.0.0 API support

[efernandez]

This fixes the following compilation error when building with Ceres Solver 2.0.0 or newer:

In file included from /build/source/test/test_orientation_3d_stamped.cpp:35:
include/fuse_core/autodiff_local_parameterization.h: In instantiation of bool fuse_core::AutoDiffLocalParameterization<PlusFunctor, MinusFunctor, kGlobalSize, kLocalSize>::ComputeJacobian(const double*, double*) const [with PlusFunctor = Orientation3DPlus; MinusFunctor = Orientation3DMinus; int kGlobalSize = 4; int kLocalSize = 3]:
/build/source/test/test_orientation_3d_stamped.cpp:203:53:   required from here
include/fuse_core/autodiff_local_parameterization.h:193:107: error: no matching function for call to AutoDifferentiate<ceres::internal::StaticParameterDims<4, 3> >(Orientation3DPlus&, const double* [2], int, double [4], double* [2])
  193 |   return ceres::internal::AutoDifferentiate<ceres::internal::StaticParameterDims<kGlobalSize, kLocalSize>>(
      |          ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~^
  194 |       *plus_functor_, parameter_ptrs, kGlobalSize, x_plus_delta, jacobian_ptrs);
      |       ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~                            
In file included from include/fuse_core/autodiff_local_parameterization.h:41,
                 from /build/source/test/test_orientation_3d_stamped.cpp:35:
include/ceres/internal/autodiff.h:309:13: note: candidate: template<int kNumResiduals, class ParameterDims, class Functor, class T> bool ceres::internal::AutoDifferentiate(const Functor&, const T* const*, int, T*, T**)
  309 | inline bool AutoDifferentiate(const Functor& functor,
      |             ^~~~~~~~~~~~~~~~~
include/ceres/internal/autodiff.h:309:13: note:   template argument deduction/substitution failed:
In file included from /build/source/test/test_orientation_3d_stamped.cpp:35:
include/fuse_core/autodiff_local_parameterization.h: In instantiation of bool fuse_core::AutoDiffLocalParameterization<PlusFunctor, MinusFunctor, kGlobalSize, kLocalSize>::ComputeMinusJacobian(const double*, double*) const [with PlusFunctor = Orientation3DPlus; MinusFunctor = Orientation3DMinus; int kGlobalSize = 4; int kLocalSize = 3]:
/build/source/test/test_orientation_3d_stamped.cpp:242:58:   required from here
include/fuse_core/autodiff_local_parameterization.h:220:108: error: no matching function for call to AutoDifferentiate<ceres::internal::StaticParameterDims<4, 4> >(Orientation3DMinus&, const double* [2], int, double [3], double* [2])
  220 |   return ceres::internal::AutoDifferentiate<ceres::internal::StaticParameterDims<kGlobalSize, kGlobalSize>>(
      |          ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~^
  221 |       *minus_functor_, parameter_ptrs, kLocalSize, delta, jacobian_ptrs);
      |       ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~                                    
In file included from include/fuse_core/autodiff_local_parameterization.h:41,
                 from /build/source/test/test_orientation_3d_stamped.cpp:35:
include/ceres/internal/autodiff.h:309:13: note: candidate: template<int kNumResiduals, class ParameterDims, class Functor, class T> bool ceres::internal::AutoDifferentiate(const Functor&, const T* const*, int, T*, T**)
  309 | inline bool AutoDifferentiate(const Functor& functor,
      |             ^~~~~~~~~~~~~~~~~
include/ceres/internal/autodiff.h:309:13: note:   template argument deduction/substitution failed:

The internal::AutoDifferentiate API was changed in ceres-solver/ceres-solver@e7a3035, first released in 2.0.0

It's actually an internal API, so technically we shouldn't be using it. Either way, this fixes the compilation error.

@iwanders found and fixed this. He's also pointed out that this code is only used in unit tests, but I believe that's indeed intended, since it looks like fuse_core::AutoDiffLocalParameterization is used to compute the expected values to compared the actual ones against them, as it's done in

fuse/fuse_variables/test/test_orientation_2d_stamped.cpp

Line 170 in 3825e48

reference.ComputeJacobian(x, expected);

.

[svwilliams]

I don't remember that far back as to why exactly I needed to use the internal API. I'll review the code and Ceres public API to see if there is a cleaner work-around.

[methylDragon]

I don't remember that far back as to why exactly I needed to use the internal API. I'll review the code and Ceres public API to see if there is a cleaner work-around.

Pinging @svwilliams for visibility

This issue came up during the porting effort. I will be including this change in the PRs I'm making to port/fix the tests for fuse_core.

I don't know if AutoDiffCostFunction might be what you're looking for? But I don't have enough context to understand if that function is doing what you need it to be doing

[svwilliams]

I'll take a look at this tomorrow and see if I can remember exactly what is going on.

[svwilliams]

@methylDragon I've reviewed the AutoDiffLocalParameterization and its history. Here' is what is going on:
The Ceres Solver library defines a ceres::LocalParameterization interface class. Ceres Solver then defines a few derived classes to make it easier to implement custom LocalParameterization classes. One of these helper classes is the ceres::AutoDiffLocalParameterization.

However, the fuse project needed to add some additional capabilities to the LocalParameterization class. So fuse derived the fuse_core::LocalParameterization from the ceres::LocalParameterization base class. All custom fuse LocalParameterization classes must derive from the fuse_core::LocalParameterization base class instead of the ceres::LocalParameterization base class.

Because of this, the Ceres Solver helper classes, like ceres::AutoDiffLocalParameterization, cannot be used directly in fuse. Basically we run into the diamond inheritance problem. To fix that issue, I reimplemented the features of the ceres::AutoDiffLocalParameterization helper class inside a fuse_core::AutoDiffLocalParameterization helper class. This creates some code duplication between the ceres and fuse_core versions. And more importantly, reimplementing the functionality inside of fuse required the use of the internal Ceres Solver API.

I'm not actually using the fuse_core::AutoDiffLocalParameterization within the fuse project (other than inside some unit tests), but it is provided as a convenience for others if they wish to implement their own LocalParameterization classes. So even though it is not really used, I'd rather not removed it.

I'll merge this PR now. And if this is sufficient for the ROS2 port to move forward, I likely just continue to ignore my use of the internal API. 🙂

And it looks like Ceres Solver 2.2 will be making drastic changes to the LocalParameterization system. Replacing the LocalParameterization class with a Manifold class. The Manifold version appears to included the added functionality that fuse_core added to ceres::LocalParameterization class. So I might be able to drop all of the fuse_core::LocalParameterization classes in the future, which would solve my internal API usage issues.