Adding penalty terms to the error variable in LeastSquaresAlgorithm class

[Mechazo11]

Hello @Rookfighter,

I am opening this thread to inquire about a separate feature relevant to the problem we are discussing in the other thread.

Is it possible to add additional terms to the error method found in the minimize method in the LeastSquaresAlgorithm class? This penalty term is a function of the current estimate of x_val.

For example the library current uses error = fval.squaredNorm() / 2;. Can I add a penalty term fpenalty s.t $fpenalty = f(x_{val})$ i.e. enforce orthonormal constraint when optimizing one of the direction vectors of a rotation matrix.

[Mechazo11]

@Rookfighter Hi, when it is convenient for you, could you share you thoughts on this feature?