algorithm - 三角测量 : Find a 3D point minimizing the Distance from N 3D Lines/Rays

Question

给定 3D 空间中的多 (N) 条线，使用最小二乘法找到 3D 点到所有线的距离最小。

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 

考虑到距离公式，A、x 和 'b' 中应该包含什么？

Answer 1

如果你展开上面的 d^2 的公式，你会发现它的形式是

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

其中 Cxx、Ex、F 等是取决于 A 和 B 向量的常量。现在对每一行求和，我们称这个和为 S，它是 Px、Py 和 Pz 的二次方。

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

现在微分

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

由于 S 是最小值，因此每个都必须为零。你可以把它写成矩阵形式一个 x = b。 [A]是Sxx等的矩阵，b是向量-[Rx,Ry,Rz]，x是向量[Px,Py,Pz]。

我现在已经将 2D 版本实现为 jsfiddle http://jsfiddle.net/SalixAlba/Y3yT9/1/执行算法的核心是出奇的简单

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;