How can I compute the optimal convergence point for a set of inverted ray vectors using numpy?

Below is an explanation of what went wrong and how you can fix it.

What’s the Issue?

Your derivation is based on minimizing the sum of the squared distances from a point X to each line. For each line (with a point pᵢ and unit direction vᵢ), the squared distance is

  || (I − vᵢ vᵢᵀ) (X − pᵢ) ||².

When you set the derivative with respect to X to zero you obtain the normal equations

  (Σᵢ (I − vᵢ vᵢᵀ)) X = Σᵢ (I − vᵢ vᵢᵀ) pᵢ.

In your code you correctly form

  A = N•I – Σᵢ (vᵢ vᵢᵀ).

However, the way you compute b is not doing what you expect. In your code you write

PYTHONCopied
b = np.sum(position - np.dot(direction, np.dot(direction.T, position)), axis=0)

Here, the inner product
  np.dot(direction.T, position)
does a matrix multiplication using the entirety of the arrays rather than applying the projection on a per–ray (row) basis. In effect, you are computing something like

  Σᵢ pᵢ – (global projection),

instead of summing over the individual residuals
  pᵢ – (vᵢ • pᵢ) vᵢ.

How to Fix It

You want for each ray to subtract its projection onto the direction. A vectorized way of doing that is to compute the dot product row‑by‑row. For example:

PYTHONCopied
# Compute the projection scalar for each ray (row)
proj_scalars = np.sum(position * direction, axis=1, keepdims=True)
# Compute the projected component for each ray
proj = proj_scalars * direction
# Compute the residual for each ray (i.e. the part orthogonal to the ray)
residuals = position - proj
# Sum the residuals to form the right-hand side vector b
b = np.sum(residuals, axis=0)

The matrix A can be computed as

PYTHONCopied
A = len(direction) * np.eye(3) - np.dot(direction.T, direction)

since
  np.dot(direction.T, direction)
sums the outer products of the direction vectors.

Revised Function

Here is a corrected version of your function:

PYTHONCopied
import numpy as np

def FindConvergingPoint(position, direction):
    # Compute the matrix A = Σᵢ (I – vᵢ vᵢᵀ)
    A = len(direction) * np.eye(3) - np.dot(direction.T, direction)
    
    # For each ray compute pᵢ - (vᵢ • pᵢ) vᵢ
    proj_scalars = np.sum(position * direction, axis=1, keepdims=True)
    residuals = position - proj_scalars * direction
    
    # Sum the residuals to construct the vector b = Σᵢ (I – vᵢ vᵢᵀ) pᵢ
    b = np.sum(residuals, axis=0)
    
    # Solve for the point X
    return np.linalg.pinv(A) @ b

This version applies the projection on a per–row basis so that you correctly compute
  (I – vᵢ vᵢᵀ) pᵢ
for each ray, and the sum over i gives the correct vector b.

Summary

Your algorithm was conceptually correct, but the problem was with the use of np.dot for the projection. Instead of trying to “matrix-multiply” the whole arrays (which does the wrong aggregation), you need to compute the dot product for each ray individually. Once fixed, the function should return a point that is much closer to what you expected (around [0, 0, 20] for your data).