Feature request

The constrained smoother has a cost function in ceres, that is not aligned with the documentation of ceres and causes poor convergence. The documentation can be found on the ceres page. The cost function squares all residuals even though ceres does that internally and it should not be done by the user. This causes the linearization being bad which in turn makes the trust region smaller and slows down the convergence a lot in scenarios where the costmap is causing the highest residual values.

Feature description

Squaring of residuals need to be removed in all residuals. For example remove the extra multiplication by value here:

Implementation considerations

Since this changes the shape of the cost function, the weights also need to be adjusted. Is there an example that can be used to tune the parameters?