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
