CODEWITHSUNDEEP
algorithmmathcomputer-visionlinear-algebragraph-algorithm
NadavRub
NadavRub

Reputation: 2610

Triangulation: Find a 3D point minimizing the Distance from N 3D Lines/Rays

Given Multiple (N) lines in 3D space, find the 3D Point minimizing the distance to all lines using Least Squares.

enter image description here

How can I express this in Least Square Matrix Form? Thus, Having:
[A] - Variables matrix ( known )
[x] - Unknown vector
[b] - dependent variables vector

Ax=b => Least-Square

Having the distance formula in mind, What should go in A, x, and 'b' ?

Upvotes: 3

Views: 1469

Answers (1)

Salix alba
Salix alba

Reputation: 7824

If you expand the formula for d^2 above you find it is of the form

d^2 = Cxx Px Px + Cxy Px Py + ... + Ex Px + Ey Py + Ez Pz + F

where Cxx, Ex, F etc are constants depending on A and B vectors. Now take the sum of these for each line, lets call this sum S which is a quadratic in Px, Py and Pz.

S = Sxx Px Px + Sxy Px Py + ... +  Rx Px + Ry Py + Rz Pz + T

Now differentiate

dS/dx = 2 Sxx Px +   Sxy Py +   Sxz Pz + Rx
dS/dy =   Syx Px + 2 Syy Py +   Syz Pz + Ry
dS/dz =   Szx Px +   Szy Py + 2 Szz Pz + Rz

As S is a minimum each of these must be zero. You can write this in matrix form A x = b. [A] is the matrix of the Sxx etc , b is the vector -[Rx,Ry,Rz] and x is the vector [Px,Py,Pz].

I've now implemented a 2D version as a jsfiddle http://jsfiddle.net/SalixAlba/Y3yT9/1/ it suprisingly simple to do the guts of the algorithm is

var sxx = 0;
var sxy = 0;
var syy = 0;
var rx = 0;
var ry = 0;
var t = 0;
// each line is defined by a x + b y + c = 0
lines.forEach(function(line, index, array) {
    var div = line.a * line.a + line.b * line.b;
    sxx += line.a * line.a / div;        
    sxy += line.a * line.b / div;        
    syy += line.b * line.b / div;        
    rx += line.a * line.c / div;        
    ry += line.b * line.c / div;        
    t += line.c * line.c / div;        
});
// Derivative of S wrt x and y is
//  2 sxx x + 2 sxy y + 2 rx = 0
//  2 sxy x + 2 syy y + 2 ry = 0

// Solve this pair of linear equations
var det = sxx * syy - sxy * sxy;
if(Math.abs(det) < 1e-6) {
    sol = { x: -10, y: -10 };
    return;
}
// (x) =  1  ( syy    -sxy ) ( -rx )
// ( ) = --- (             ) (     )
// (y)   det ( -sxy    sxx ) ( -ry )
var x = ( - syy * rx + sxy * ry ) / det;
var y = (   sxy * rx - sxx * ry ) / det;
console.log("x "+x+" y "+y);
sol = { x: x, y: y };
return;

Upvotes: 4

Related Questions