[FIXED] How to fit a curved surface to 3d vectors with their normals?

Issue

I have 3d position vectors as follows:

A=np.array([-773.58, 645.41, -101.986])
B=np.array([841.01, -205.0, 400.9])
C=np.array([1000.91, 805.45, 745.10])

and their corresponding normal vectors

NORM_A = np.array([0.89,  -0.031, 0.44])
NORM_B = np.array([0.87,  -0.14, -0.46])
NORM_C = np.array([0.83,  -0.23, -0.48])

The positions A, B, and C are just points in 3d space, but the normal vectors NORM_A, NORM_B, and NORM_C indicate normal vectors that are perpendicular to a surface.

So, how to fit a 3d surface (maybe curved)?

Solution

You need to do some soul searching and choose the geometry of an ideal surface to which you want to attempt a fit. You claim to be able to accept a paraboloid. The equation of a generalised elliptic paraboloid in (y, z) is

x = ((y - a)/b)² + ((z - c)/d)² - e

or

f(x, y, z) = e = -x + ((y - a)/b)² + ((z - c)/d)²

The gradient is

∇f = -x̂ + ŷ 2(y-a)/b² + ẑ 2(z-c)/d²

The unit normal is the gradient divided by its Frobenius norm,

√( 4(y-a)²/b⁴ + 4(z-c)²/d⁴ + 1 ) 

Write functions finding the paraboloid x and the unit normal function like above. Then write a cost function that applies parameters a through e and calculates least-squares between the ideal and actual quantities.

The result is a very crude, but roughly appropriate, fit:

import numpy as np
import scipy.optimize


def paraboloid(y: np.ndarray, z: np.ndarray, params: np.ndarray) -> np.ndarray:
    a, b, c, d, e = params
    return ((y - a) / b)**2 + ((z - c) / d)**2 - e


def paraboloid_norm(y: np.ndarray, z: np.ndarray, params: np.ndarray) -> np.ndarray:
    a, b, c, d, e = params
    grad = np.stack((
        np.full_like(y, -1),
        2*(y - a)/b**2,
        2*(z - c)/d**2,
    ))
    gradlen = np.linalg.norm(grad, axis=0)
    return grad/gradlen


def main() -> None:
    points = np.array((
        (-773.58,  645.41, -101.986),
        ( 841.01, -205.00,  400.900),
        (1000.91,  805.45,  745.100),
    ))
    x, y, z = points.T

    norms = -np.array((  # Negative to take interior surface
        (0.89, -0.031,  0.44),
        (0.87, -0.140, -0.46),
        (0.83, -0.230, -0.48),
    ))

    def paraboloid_estimate(params: np.ndarray) -> float:
        xact = paraboloid(y, z, params)
        xerr = x - xact
        normact = paraboloid_norm(y, z, params)
        normerr = np.ravel(norms - normact.T)
        return normerr.dot(normerr) + xerr.dot(xerr)/1e6

    res = scipy.optimize.minimize(
        fun=paraboloid_estimate,
        x0=(1000, 100, -10, -10, 100),
    )

    print(res)
    print()

    print('Parameters')
    print('\n'.join(
        f'{var}={param:10.2f}'
        for var, param in zip('abcde', res.x)
    ))
    print()

    print('X: ideal vs. estimated')
    print(np.stack((x, paraboloid(y, z, res.x)), axis=1))
    print()

    normest = paraboloid_norm(y, z, res.x)
    print('Norms: ideal vs. estimated')
    print('\n'.join(
        f'{n0:6.3f} {n1:6.3f}'
        for n0, n1 in zip(norms.ravel(), normest.T.ravel())
    ))


if __name__ == '__main__':
    main()

Output:

      fun: 1.5277971465545992
 hess_inv: array([[ 8.84698190e+05,  4.64761816e+04,  9.37195785e+03,
        -7.46447476e+02, -5.97381612e+04],
       [ 4.64761816e+04,  9.18469098e+03,  1.29540661e+03,
        -1.17925626e+03, -9.18563946e+04],
       [ 9.37195785e+03,  1.29540661e+03,  2.54264547e+04,
         1.17320705e+02, -3.61699455e+04],
       [-7.46447476e+02, -1.17925626e+03,  1.17320705e+02,
         3.03472348e+02,  1.97447972e+04],
       [-5.97381612e+04, -9.18563946e+04, -3.61699455e+04,
         1.97447972e+04,  1.58708638e+06]])
      jac: array([-2.98023224e-08,  6.25848770e-07, -5.96046448e-08,  1.31130219e-06,
        1.49011612e-08])
  message: 'Optimization terminated successfully.'
     nfev: 648
      nit: 101
     njev: 108
   status: 0
  success: True
        x: array([714.97308176,  48.97163217, -28.38546403, -22.28868237,
       292.05566874])

Parameters
a=    714.97
b=     48.97
c=    -28.39
d=    -22.29
e=    292.06

X: ideal vs. estimated
[[-773.58       -279.13372976]
 [ 841.01        431.80899381]
 [1000.91        915.66004437]]

Norms: ideal vs. estimated
-0.890 -0.957
 0.031 -0.056
-0.440 -0.284
-0.870 -0.468
 0.140 -0.359
 0.460  0.808
-0.830 -0.306
 0.230  0.023
 0.480  0.952

Visualise result in 3D (surface and contours):

Visualise a normal:

Better fit is possible with more data points and a more complicated function that has more degrees of freedom, but you need to decide what that means.

Answered By - Reinderien