Amending the previous message: the question is about constrained optimization and the simplified real-valued objective function is as below:
where p and q are 3D points.
Indeed this problem should be copied K times, that is, we want to find K transformations (T) for K 3D points (p) to minimize their distance (after transformation) to the corresponding points (q). Here, the contribution of some residuals should be controlled using the weight (penalty) "w" as some of "p"s have a poor corresponding one in q. So we're dealing with SE(3)^K and K-element "w" per residual so we have T( : , : , i) and w( i ) where i=1:K.
The way I constructed the problem (except for the suitable factory for "w") in MATLAB is:
manifold = productmanifold(M);
problem.cost = @(x) CostFunction(x, p, q,K);
Given "w", the penalty coefficient per residual, I am wondering how I can constrain w to [0 1] in the problem construction (or should I handle it mathematically somehow inside the CostFunction)?