使用加权最小二乘法的3D矢量交集

Question

我想在3D空间中找到与多个线/矢量最接近的交点，但我想根据每个矢量的长度来加权每个矢量的影响（它的长度越大，其影响越大）。我正在使用R. The answer to this question完美地找到与多个向量最接近的交点，但缺少加权分量。我在哪里需要插入权重矩阵？

This（第4张幻灯片）暗示了哪里，但我没有成功找到Dan的代码中的这个部分（我很少使用线性代数来完全理解正在发生的事情）。

关于线 - 线交叉的理论可以在这个answer和Wikipedia找到。

这是我在R中对Dan的MATLAB code的翻译：

XYZ_intersect <- function(XYZ_start, XYZ_end)
{
# XYZ_start: matrix n x 3 containing starting XYZ coordinates
# XYZ_end: matrix n x 3 containing ending XYZ coordinates

direction_vectors <- XYZ_end - XYZ_start
length_vectors <- matrix(sqrt(.rowSums(direction_vectors^2, dim(direction_vectors)[1], 3)))
normalized_vectors <- direction_vectors / (length_vectors %*% matrix(1, ncol = 3))    
weights <- length_vectors / sum(length_vectors)

Nx <- normalized_vectors[,1, drop = FALSE]
Ny <- normalized_vectors[,2, drop = FALSE]
Nz <- normalized_vectors[,3, drop = FALSE]

Sxx <- sum(Nx^2 - 1)
Syy <- sum(Ny^2 - 1)
Szz <- sum(Nz^2 - 1)
Sxy <- sum(Nx*Ny)
Sxz <- sum(Nx*Nz)
Syz <- sum(Ny*Nz)

symmetric <- matrix(c(Sxx,Sxy,Sxz,Sxy,Syy,Syz,Sxz,Syz,Szz), nrow = 3)

Cx <- sum(XYZ_start[,1, drop = FALSE]*(Nx^2 - 1) + XYZ_start[,2, drop = FALSE]*(Nx*Ny) + XYZ_start[,3, drop = FALSE]*(Nx*Nz))
Cy <- sum(XYZ_start[,1, drop = FALSE]*(Nx*Ny) + XYZ_start[,2, drop = FALSE]*(Ny^2 - 1) + XYZ_start[,3, drop = FALSE]*(Ny*Nz))
Cz <- sum(XYZ_start[,1, drop = FALSE]*(Nx*Nz) + XYZ_start[,2, drop = FALSE]*(Ny*Nz)  + XYZ_start[,3, drop = FALSE]*(Nz^2 - 1))

C <- matrix(c(Cx,Cy,Cz))
XYZ_intersect <- t(solve(symmetric,C))
return(XYZ_intersect)
}

所以，如果我有四个向量描述为：

#     (X0,Y0,Z0,X1,Y1,Z1)
vec1 <- c(0,0,1,3,3,6)
vec2 <- c(3,0,2,3,4,8)
vec3 <- c(0,3,3,4,3,9)
vec4 <- c(2,3,2,4,3,7)

XYZ_start <- matrix(c(vec1[1:3], vec2[1:3], vec3[1:3], vec4[1:3]), ncol = 3, byrow = TRUE)
XYZ_end <- matrix(c(vec1[4:6], vec2[4:6], vec3[4:6], vec4[4:6]), ncol = 3, byrow = TRUE)

XYZ_intersect(XYZ_start, XYZ_end)
#         [,1]     [,2]     [,3]
#[1,] 3.132048 3.063237 6.522456

在这种情况下，我想给出以下权重：

weights
#          [,1]
#[1,] 0.2487194
#[2,] 0.2735124
#[3,] 0.2735124
#[4,] 0.2042558

Answer 1

最后，有血有肉的人帮助了我。必须在每个总和内添加权重矩阵：

XYZ_intersect <- function(XYZ_start, XYZ_end)
{
  # XYZ_start: matrix n x 3 containing starting XYZ coordinates
  # XYZ_end: matrix n x 3 containing ending XYZ coordinates

  direction_vectors <- XYZ_end - XYZ_start
  length_vectors <- matrix(sqrt(.rowSums(direction_vectors^2, dim(direction_vectors)[1], 3)))
  normalized_vectors <- direction_vectors / (length_vectors %*% matrix(1, ncol = 3))    
  weights <- length_vectors / sum(length_vectors)

  Nx <- normalized_vectors[,1, drop = FALSE]
  Ny <- normalized_vectors[,2, drop = FALSE]
  Nz <- normalized_vectors[,3, drop = FALSE]

  Wxx <- weights * (Nx^2 - 1)
  Wyy <- weights * (Ny^2 - 1)
  Wzz <- weights * (Nz^2 - 1)
  Wxy <- weights * Nx * Ny
  Wxz <- weights * Nx * Nz
  Wyz <- weights * Ny * Nz

  Sxx <- sum(Wxx)
  Syy <- sum(Wyy)
  Szz <- sum(Wzz)
  Sxy <- sum(Wxy)
  Sxz <- sum(Wxz)
  Syz <- sum(Wyz)

  symmetric <- matrix(c(Sxx,Sxy,Sxz,Sxy,Syy,Syz,Sxz,Syz,Szz), nrow = 3)

  Cx <- sum(XYZ_start[,1, drop = FALSE] * Wxx + XYZ_start[,2, drop = FALSE] * Wxy + XYZ_start[,3, drop = FALSE] * Wxz)
  Cy <- sum(XYZ_start[,1, drop = FALSE] * Wxy + XYZ_start[,2, drop = FALSE] * Wyy + XYZ_start[,3, drop = FALSE] * Wyz)
  Cz <- sum(XYZ_start[,1, drop = FALSE] * Wxz + XYZ_start[,2, drop = FALSE] * Wyz + XYZ_start[,3, drop = FALSE] * Wzz)

  C <- matrix(c(Cx,Cy,Cz))
  XYZ_intersect <- t(solve(symmetric,C))
  return(XYZ_intersect)
}

因此，使用我的四个向量，结果是：

XYZ_intersect(XYZ_start, XYZ_end)
#         [,1]     [,2]     [,3]
#[1,] 3.092853 3.069695 6.556273