When using an M-Estimator, the wrong cost is used

[furgalep]

We are not using \rho(x) as the cost. We are using w(x) x^T x.

This should be changed.

[furgalep]

See here for a possibility of how we could implement M-Estimators. They have to return p(x), p'(x), p''(x).

http://homes.cs.washington.edu/~sagarwal/ceres-solver/stable/modeling.html#LossFunction

[furgalep]

Here is where the M-Estimator is applied:

https://github.com/ethz-asl/aslam_optimizer/blob/master/aslam_backend/src/ErrorTerm.cpp#L20-L24

[furgalep]

These also need attention:

https://github.com/ethz-asl/aslam_optimizer/blob/master/aslam_backend/include/aslam/backend/implementation/ErrorTerm.hpp#L169-L192

[furgalep]

And

https://github.com/ethz-asl/aslam_optimizer/blob/master/aslam_backend/src/ErrorTermDs.cpp#L144-L167

[furgalep]

Maybe other places as well. Thanks!