如何用牛顿法求解n个球体相交的最优3D位置

问题描述 #

我尝试用牛顿法求解n个球体相交的最优位置：

• 球体方程：(x - a)² + (y - b)² + (z - c)² = r²
  • a,b,c：球心坐标
  • x,y,z：采样点位置
  • r²：半径平方

若一个点要位于（或接近）多个球面上，需满足所有对应球体的方程。我设计的代价函数为：
sum((x - a_i)² + (y - b_i)² + (z - c_i)² - r_i²) = 0

• a_i,b_i,c_i：第i个球的球心坐标
• x,y,z：采样点位置
• r_i：第i个球的半径

等价于sum(dot(pos - center_i) - r_i²) = 0，其中dot(pos - center_i)表示采样点到第i个球心的距离平方。

牛顿法的更新公式为：
s2 = s1 - f(s1)/f’(s1)

• s2：更新后的采样点位置
• f(s1)：采样点s1对应的代价函数值
• f’(s1)：代价函数在s1处的导数

现有代码及问题 #

我编写了如下C#函数，输入2个球体时可正常求解，但输入3个球体时失效，推测是方程变为非线性导致：

// 迭代循环
for (int i = 0; i < maxIterations; i++)
{
    float3 vDist = float3.zero;
    float errorFunction = 0f;
    float3 derivative = float3.zero;

    // 遍历每个球体
    for (int j = 0; j < centers.Length; j++)
    {
        // 计算采样点到球心的距离平方与半径平方的差值，累加得到代价函数值
        vDist = sampleLocation - centers[j];
        errorFunction += math.dot(vDist, vDist) - (radii[j] * radii[j]);

        // 计算导数用于更新采样点
        derivative += new float3(
            (sampleLocation.x - centers[j].x) * (sampleLocation.x - centers[j].x),
            (sampleLocation.y - centers[j].y) * (sampleLocation.y - centers[j].y),
            (sampleLocation.z - centers[j].z) * (sampleLocation.z - centers[j].z)
        );
    }

    // 误差在阈值内则停止迭代
    if (errorFunction >= -threshold && errorFunction <= threshold)
    {
        print("Found position: " + sampleLocation + " in " + i + ", error: " + errorFunction);
        break;
    }

    // 根据代价函数和导数更新采样点
    sampleLocation = sampleLocation - (errorFunction / derivative);
}

我需要扩展该函数，使其支持多个球体的求解。

内容的提问来源于stack exchange，提问作者Tea-F-Tea