ICP weights of point correspondences

Question

For estimating an affine transformation between two 3D point sets, where corresponding point pairs have been established, I'm using single value decomposition on a correlation matrix calculated like this (pseudocode):

vector3 fixed_center = (0, 0, 0)
vector3 loose_center = (0, 0, 0)
n = 0
foreach (fixed_pt, loose_pt) in correspondences
    fixed_center += fixed_pt
    loose_center += loose_pt
    n++
fixed_center /= n
loose_center /= n
matrix3x3 correlation = ((0,0,0), (0,0,0), (0,0,0))
foreach (fixed_pt, loose_pt) in correspondences
     correlation += (fixed_pt - fixed_center) * (loose_pt - loose_center)

If weights are assigned to the correspondences, how does the correlation matrix have to be calculated then? Do the weights have to be normalized, and do they have to be included in the calculation of the centers?

Answer 1

If the meaning of your weights is that you want to minimise

Q = Sum{ i | w[i]*||fixed[i] - (R*loose[i] + t)||^2 }

(where w are the weights, x -> R*x + t your affine map)

Then yes, the centres should both be weighted averages and the 'correlation' (really covariance) should be weighted too. If your weights are not normalised you should normalise them first.

That is

f_centre = Sum{ i | w[i]*fixed[i] }
l_centre = Sum{ i | w[i]*loose[i] }
covar = Sum{ i | w[i] * (fixed[i]-f_centre)*(loose[i]-l_centre)' }