最接近 3D 空间中多条线的 3d 点
Posted
技术标签:
【中文标题】最接近 3D 空间中多条线的 3d 点【英文标题】：3d point closest to multiple lines in 3D space 【发布时间】：2018-06-17 15:54:39 【问题描述】：我搜索非迭代、封闭形式的算法，以找到最接近 3d 线集的点的最小二乘解。它类似于 3d 点三角测量（以最小化重新投影）但似乎更简单更快？
线可以用任何形式描述，2点，点和单位方向或类似的。
【问题讨论】：
您最好在 math.stackexchange.com 上询问这个问题 - 如果您需要帮助编码，请回到这里。 您可以用全套线测试每个点。如果您对点和线进行排序，可以实现一些改进，因此可以快速丢弃其中的许多。对于点线 3D 距离，您可以使用例如 this 我的回答对你有用吗？我认为效果很好，你从来没有评论过。 抱歉，这个任务耽误了很久。生病回来时，a 会写出 cmets 和结果。抱歉暂停。 【参考方案1】：我需要这个作为 Processing 中的草图，所以我移植了 Gene 的答案。效果很好，并认为它可以为其他人节省一点时间。不幸的是，PVector/PMatrix 没有向量或矩阵的数组访问器，所以我不得不将它们添加为本地函数。
float getv(PVector v, int i)
if(i == 0) return v.x;
if(i == 1) return v.y;
return v.z;
void setv(PVector v, int i, float value)
if (i == 0) v.x = value;
else if (i == 1) v.y = value;
else v.z = value;
void incv(PVector v, int i, float value)
setv(v,i,getv(v,i) + value);
float getm(float[] mm, int r, int c) return mm[c + r*4];
void setm(float[] mm, int r, int c, float value) mm[c + r*4] = value;
void incm(float[] mm, int r, int c, float value) mm[c + r*4] += value;
PVector findNearestPoint(PVector a[], PVector d[])
var mm = new float[16];
var b = new PVector();
var n = a.length;
for (int i = 0; i < n; ++i)
var d2 = d[i].dot(d[i]);
var da = d[i].dot(a[i]);
for (int ii = 0; ii < 3; ++ii)
for (int jj = 0; jj < 3; ++jj)
incm(mm,ii,jj, getv(d[i],ii) * getv(d[i],jj));
incm(mm, ii,ii, -d2);
incv(b, ii, getv(d[i], ii) * da - getv(a[i], ii) * d2);
var p = solve(mm, new float[] b.x, b.y, b.z);
return new PVector(p[0],p[1],p[2]);
// Verifier
float dist2(PVector p, PVector a, PVector d)
PVector pa = new PVector( a.x-p.x, a.y-p.y, a.z-p.z );
float dpa = d.dot(pa);
return d.dot(d) * pa.dot(pa) - dpa * dpa;
//double sum_dist2(VEC p, VEC a[], VEC d[], int n)
float sum_dist2(PVector p, PVector a[], PVector d[])
int n = a.length;
float sum = 0;
for (int i = 0; i < n; ++i)
sum += dist2(p, a[i], d[i]);
return sum;
// Check 26 nearby points and verify the provided one is nearest.
boolean isNearest(PVector p, PVector a[], PVector d[])
float min_d2 = 3.4028235E38;
int ii = 2, jj = 2, kk = 2;
final float D = 0.1f;
for (int i = -1; i <= 1; ++i)
for (int j = -1; j <= 1; ++j)
for (int k = -1; k <= 1; ++k)
PVector pp = new PVector( p.x + D * i, p.y + D * j, p.z + D * k );
float d2 = sum_dist2(pp, a, d);
// Prefer provided point among equals.
if (d2 < min_d2 || i == 0 && j == 0 && k == 0 && d2 == min_d2)
min_d2 = d2;
ii = i; jj = j; kk = k;
return ii == 0 && jj == 0 && kk == 0;
void setup()
PVector a[] =
new PVector(-14.2, 17, -1),
new PVector(1, 1, 1),
new PVector(2.3, 4.1, 9.8),
new PVector(1,2,3)
;
PVector d[] =
new PVector(1.3, 1.3, -10),
new PVector(12.1, -17.2, 1.1),
new PVector(19.2, 31.8, 3.5),
new PVector(4,5,6)
;
int n = 4;
for (int i = 0; i < n; ++i)
d[i].normalize();
PVector p = findNearestPoint(a, d);
println(p);
if (!isNearest(p, a, d))
println("Woops. Not nearest.\n");
// From rosettacode (with bug fix: added a missing fabs())
int mat_elem(int y, int x) return y*4+x;
void swap_row(float[] a, float[] b, int r1, int r2, int n)
float tmp;
int p1, p2;
int i;
if (r1 == r2) return;
for (i = 0; i < n; i++)
p1 = mat_elem(r1, i);
p2 = mat_elem(r2, i);
tmp = a[p1];
a[p1] = a[p2];
a[p2] = tmp;
tmp = b[r1];
b[r1] = b[r2];
b[r2] = tmp;
float[] solve(float[] a, float[] b)
float[] x = new float[] 0,0,0;
int n = x.length;
int i, j, col, row, max_row, dia;
float max, tmp;
for (dia = 0; dia < n; dia++)
max_row = dia;
max = abs(getm(a, dia, dia));
for (row = dia + 1; row < n; row++)
if ((tmp = abs(getm(a, row, dia))) > max)
max_row = row;
max = tmp;
swap_row(a, b, dia, max_row, n);
for (row = dia + 1; row < n; row++)
tmp = getm(a, row, dia) / getm(a, dia, dia);
for (col = dia+1; col < n; col++)
incm(a, row, col, -tmp * getm(a, dia, col));
setm(a,row,dia, 0);
b[row] -= tmp * b[dia];
for (row = n - 1; row >= 0; row--)
tmp = b[row];
for (j = n - 1; j > row; j--)
tmp -= x[j] * getm(a, row, j);
x[row] = tmp / getm(a, row, row);
return x;
【讨论】：
【参考方案2】：让第i行由点ai和单位方向向量di给出>。我们需要找到最小化点到线距离平方和的单点。这是梯度为零向量的地方：
扩大渐变，
代数产生一个规范的 3x3 线性系统，
矩阵 M 的第 k 行（一个 3 元素行向量）在哪里
用向量ek各自的单位基向量，而
将其转换为代码并不难。我从 Rosettacode 借用（并修复了一个小错误）一个高斯消除函数来解决这个系统。感谢作者！
#include <stdio.h>
#include <math.h>
typedef double VEC[3];
typedef VEC MAT[3];
void solve(double *a, double *b, double *x, int n); // linear solver
double dot(VEC a, VEC b) return a[0]*b[0] + a[1]*b[1] + a[2]*b[2];
void find_nearest_point(VEC p, VEC a[], VEC d[], int n)
MAT m = 0, 0, 0, 0, 0, 0, 0, 0, 0;
VEC b = 0, 0, 0;
for (int i = 0; i < n; ++i)
double d2 = dot(d[i], d[i]), da = dot(d[i], a[i]);
for (int ii = 0; ii < 3; ++ii)
for (int jj = 0; jj < 3; ++jj) m[ii][jj] += d[i][ii] * d[i][jj];
m[ii][ii] -= d2;
b[ii] += d[i][ii] * da - a[i][ii] * d2;
solve(&m[0][0], b, p, 3);
void pp(VEC v, char *l, char *r)
printf("%s%.3lf, %.3lf, %.3lf%s", l, v[0], v[1], v[2], r);
void pv(VEC v) pp(v, "(", ")");
void pm(MAT m) for (int i = 0; i < 3; ++i) pp(m[i], "\n[", "]");
// Verifier
double dist2(VEC p, VEC a, VEC d)
VEC pa = a[0]-p[0], a[1]-p[1], a[2]-p[2] ;
double dpa = dot(d, pa);
return dot(d, d) * dot(pa, pa) - dpa * dpa;
double sum_dist2(VEC p, VEC a[], VEC d[], int n)
double sum = 0;
for (int i = 0; i < n; ++i) sum += dist2(p, a[i], d[i]);
return sum;
// Check 26 nearby points and verify the provided one is nearest.
int is_nearest(VEC p, VEC a[], VEC d[], int n)
double min_d2 = 1e100;
int ii = 2, jj = 2, kk = 2;
#define D 0.01
for (int i = -1; i <= 1; ++i)
for (int j = -1; j <= 1; ++j)
for (int k = -1; k <= 1; ++k)
VEC pp = p[0] + D * i, p[1] + D * j, p[2] + D * k ;
double d2 = sum_dist2(pp, a, d, n);
// Prefer provided point among equals.
if (d2 < min_d2 || i == 0 && j == 0 && k == 0 && d2 == min_d2)
min_d2 = d2;
ii = i; jj = j; kk = k;
return ii == 0 && jj == 0 && kk == 0;
void normalize(VEC v)
double len = sqrt(dot(v, v));
v[0] /= len;
v[1] /= len;
v[2] /= len;
int main(void)
VEC a[] = -14.2, 17, -1, 1, 1, 1, 2.3, 4.1, 9.8, 1,2,3;
VEC d[] = 1.3, 1.3, -10, 12.1, -17.2, 1.1, 19.2, 31.8, 3.5, 4,5,6;
int n = 4;
for (int i = 0; i < n; ++i) normalize(d[i]);
VEC p;
find_nearest_point(p, a, d, n);
pv(p);
printf("\n");
if (!is_nearest(p, a, d, n)) printf("Woops. Not nearest.\n");
return 0;
// From rosettacode (with bug fix: added a missing fabs())
#define mat_elem(a, y, x, n) (a + ((y) * (n) + (x)))
void swap_row(double *a, double *b, int r1, int r2, int n)
double tmp, *p1, *p2;
int i;
if (r1 == r2) return;
for (i = 0; i < n; i++)
p1 = mat_elem(a, r1, i, n);
p2 = mat_elem(a, r2, i, n);
tmp = *p1, *p1 = *p2, *p2 = tmp;
tmp = b[r1], b[r1] = b[r2], b[r2] = tmp;
void solve(double *a, double *b, double *x, int n)
#define A(y, x) (*mat_elem(a, y, x, n))
int i, j, col, row, max_row, dia;
double max, tmp;
for (dia = 0; dia < n; dia++)
max_row = dia, max = fabs(A(dia, dia));
for (row = dia + 1; row < n; row++)
if ((tmp = fabs(A(row, dia))) > max) max_row = row, max = tmp;
swap_row(a, b, dia, max_row, n);
for (row = dia + 1; row < n; row++)
tmp = A(row, dia) / A(dia, dia);
for (col = dia+1; col < n; col++)
A(row, col) -= tmp * A(dia, col);
A(row, dia) = 0;
b[row] -= tmp * b[dia];
for (row = n - 1; row >= 0; row--)
tmp = b[row];
for (j = n - 1; j > row; j--) tmp -= x[j] * A(row, j);
x[row] = tmp / A(row, row);
#undef A
这没有经过广泛的测试，但似乎工作正常。
【讨论】：
【参考方案3】：设直线的基点为p，单位方向向量为d。
然后可以计算点v到这条线的距离using cross product
SquaredDist = ((v - p) x d)^2
使用Maple包符号计算，可以得到
d := <dx, dy, dz>;
v := <vx, vy, vz>;
p := <px, py, pz>;
w := v - p;
cp := CrossProduct(d, w);
nrm := BilinearForm(cp, cp, conjugate=false); //squared dist
nr := expand(nrm);
//now partial derivatives
nrx := diff(nr, vx);
//results:
nrx := -2*dz^2*px-2*dy^2*px+2*dz^2*vx+2*dy^2*vx
+2*dx*py*dy-2*dx*vy*dy+2*dz*dx*pz-2*dz*dx*vz
nry := -2*dx^2*py-2*dz^2*py-2*dy*vz*dz+2*dx^2*vy
+2*dz^2*vy+2*dy*pz*dz+2*dx*dy*px-2*dx*dy*vx
nrz := -2*dy^2*pz+2*dy^2*vz-2*dy*dz*vy+2*dx^2*vz
-2*dx^2*pz-2*dz*vx*dx+2*dy*dz*py+2*dz*px*dx
为了最小化距离平方和，我们必须为零偏导数建立线性方程组，如下所示：
vx*2*(Sum(dz^2)+Sum(dy^2)) + vy * (-2*Sum(dx*dy)) + vz *(-2*Sum(dz*dx)) =
2*Sum(dz^2*px)-2*Sum(dy^2*px) -2*Sum(dx*py*dy)-2*Sum(dz*dx*pz)
where
Sum(dz^2) = Sumover all i in line indexes dz[i] * dz[i]
求解未知数 vx, vy, vz
编辑：飞机而不是线的旧错误答案，留作参考
如果我们使用一般的直线方程
A * x + B * y + C * z + D = 0
那么点 (x, y, z) 到这条线的距离是
Dist = Abs(A * x + B * y + C * z + D) / Sqrt(A^2 + B^2 + C^2)
为了简化 - 只需标准化除以Norm's 的所有线方程
Norm = Sqrt(A^2 + B^2 + C^2)
a = A / Norm
b = B / Norm
c = C / Norm
d = D / Norm
现在方程是
a * x + b * y + c * z + d = 0
和距离
Dist = Abs(a * x + b * y + c * z + d)
我们可以使用像 LS 方法这样的平方距离（ai, bi, ci, di 是第 i 行的系数）
F = Sum(ai*x + bi*y + ci * z + d)^2 =
Sum(ai^2*x^2 + bi^2*y^2 + ci^2*z^2 + d^2 +
2 * (ai*bi*x*y + ai*ci*x*z + bi*y*ci*z + ai*x*di + bi*y*di + ci*z*di))
partial derivatives
dF/dx = 2*Sum(ai^2*x + ai*bi*y + ai*ci*z + ai*di) = 0
dF/dy = 2*Sum(bi^2*y + ai*bi*x + bi*ci*z + bi*di) = 0
dF/dz = 2*Sum(ci^2*z + ai*ci*x + bi*ci*y + ci*di) = 0
so we have system of linear equation
x * Sum(ai^2) + y * Sum(ai*bi) + z * Sum(ai*ci)= - Sum(ai*di)
y * Sum(bi^2) + x * Sum(ai*bi) + z * Sum(bi*ci)= - Sum(bi*di)
z * Sum(ci^2) + x * Sum(ai*ci) + y * Sum(bi*ci)= - Sum(ci*di)
x * Saa + y * Sab + z * Sac = - Sad
x * Sab + y * Sbb + z * Sbc = - Sbd
x * Sac + y * Sbc + z * Scc = - Scd
where S** are corresponding sums
并且可以解决未知数x, y, z
【讨论】：
A * x + B * y + C * z + D = 0 不是 3D 中的线，而是平面。因此，以下计算可能也不符合 OP 的问题。
稍微考虑一下：到一条线的平方距离可以通过将平方距离添加到两个正交平面（在线相交）来计算。因此，如果您为每条线创建两个平面，您的方法会很有用。
@Ralf Kleberhoff OMG，是的 :( 我回答了 2D 案例，然后愚蠢地将其扩展到 3D 案例
我没有看到您的最终解决方案是如何工作的。可能有任意数量的点，所以你写的是任意数量的方程。但只有3个未知数。正如我在 cmets 中所说的（根据您的变量名进行调整），您需要求解 gradient( sum_i (d_i x (p_i - v))^2 ) = 0，即 3 个未知数中的 3 个方程。
@Gene 我考虑计算点的坐标，以最小化给定线集的距离平方和。它准确地解决了 3 个未知数中的 3 个方程（我只写了一个）。您是在谈论从点集中选择最佳点吗？以上是关于最接近 3D 空间中多条线的 3d 点的主要内容，如果未能解决你的问题，请参考以下文章