Eureka!

After days of struggle, I think I have struck oil. The algorithm I mentioned works like a charm, but still needs to be tested. I shall be posting the code soon for anyone like me (Lol, I hardly think it'll happen). Anyways, I will be creating a Kinect Vision Library, which will allow .NET devs to easily access whatever I have without  the struggle....

Here is the function I use, any questions, please comment below.
Code: VB


Function estimateDistBWCentroid(ByVal srcIn As PointCloud, ByVal dstIn As PointCloud) As Double
        Dim src As PointCloud = srcIn
        Dim dst As PointCloud = dstIn
        If src.data.Count < 3 Then
            Throw New Exception("Error: at least 3 correspondences must be provided")
        End If

        Dim cL, cR As New Point3D(0, 0, 0)
        Dim S, N As MatrixA
        Dim Z, D As MatrixA

        Dim v As New List(Of Double)

        Dim nMatches As Integer = src.data.Count
       
        'Compute centroid
        For i = 0 To nMatches - 1
            cL.x_incr(dst.data(i).x)
            cL.y_incr(dst.data(i).y)
            cL.z_incr(dst.data(i).z)

            cR.x_incr(src.data(i).x)
            cR.y_incr(src.data(i).y)
            cR.z_incr(src.data(i).z)
        Next

        Dim F As Double = 1.0 / nMatches

        cL *= F
        cR *= F
'This will give the transitional vector (just change z to x or y to get the corresponding vector)

        Return cL.z - cR.z 'System.Math.Sqrt(((cL.x - cR.x) ^ 2) + ((cL.y - cR.y) ^ 2) + ((cL.z - cR.z) ^ 2)) '
End Function