RobustCostFunction weighted error is different when vectorization is on/off.

[luisenp]

🐛 Bug

Reported by @joeaortiz

When using RobustCostFunction we get different results in Objective.error() if vectorization is turned on/off. The reason is that when vectorization is off we get the result from cost_function.weighted_error(), and when it's turned on we get it (indirectly) from cost_function.weighted_jacobians_and_error(). As explained in the code (and to be documented), RobustCostFunction has different behavior for each of this methods; the jacobians version of the error is rescaled so that it can be used by the optimizers, while the other one is the actual error value (not rescaled).

My proposal (using input from @joeaortiz) is to find a way for the Vectorization to serve the result of weighted_error when _CostFunctionWrapper is called, rather than the one from weighted_jacobians_and_error.

[fantaosha]

That seems what I did in the experimental branch where only error() is vectorized and RobustLoss further processes the data.