How to fit for a parametric 3D shape into data points, allowing for translation and rotation I have a list of points in 3D space $(x_{Ci}, y_{Ci}, z_{Ci})$. These points are in the "C" coordinate system.
I want to fit a shape (an egg) into these points, the shape is described by the following implicit equation:
$x_E^2+y_E^2+z_E^2=b^2-a^2(x_E^2+y_E^2+z_E^2)+2*a*z_E*\sqrt{x_E^2+y_E^2+z_E^2}$
(the subscript E indicating that this is the Egg coordinate system)
The shape has two parameters, $a$ and $b$.
The goal is to determine the "best" $a$ and $b$.
("Best" as in least square fit for example, but other approximations are also possible).
This is where it gets complicated.
The "best" fit should respect translation and also rotation.
That means that the implicit equation cannot be used as-is, but also has to include translation (expressed through $x_{C0}$, $y_{C0}$ and $z_{C0}$ and rotation around the X-axis and Z-Axis (I'll call the rotation angles $k_x$ and $k_z$).
I came up with the following equations for calculating a point in the E coordinate system from a point in the C system.
$x_E = x_{C} \cos{\left(k_{Z} \right)} - \left(y_{C} \cos{\left(k_{X} \right)} - z_{C} \sin{\left(k_{X} \right)}\right) \sin{\left(k_{Z} \right)} + x_{C0}$
$y_E = x_{C} \sin{\left(k_{Z} \right)} + \left(y_{C} \cos{\left(k_{X} \right)} - z_{C} \sin{\left(k_{X} \right)}\right) \cos{\left(k_{Z} \right)} + y_{C0} $
$z_E = y_{C} \sin{\left(k_{X} \right)} + z_{C} \cos{\left(k_{X} \right)} + z_{C0}$
For doing the actual fitting, I tried following the approach described in this article by Charles Jekel, where he fitted a sphere into data points https://jekel.me/2015/Least-Squares-Sphere-Fit/
After inserting the equations for $x_E$, $y_E$ and $z_E$ into the first implicit equation, I brought it up to the matrix form $f=A*c$ (as done in the article).
These are the values I came up with:
$f=\begin{bmatrix}x_{Ci}^2+y_{Ci}^2+z_{Ci}^2 \\ x_ {Ci+1}^2+y_{Ci+1}^2+z_{Ci+1}^2 \\ ...\end{bmatrix}$
$A=\begin{bmatrix}
x_{Ci} & y_{Ci} & z_{Ci} & 1 \\ 
x_{Ci+1} & y_{Ci+1} & z_{Ci+1} & 1 \\ 
... & ... & ... & ...
\end{bmatrix}$
$c=\begin{bmatrix}
-2*x_{C0}*\cos{k_z}-2*y_{c0}*\sin{k_z} \\
-2*y_{C0}*\cos{k_x}*\cos{k_z}+2*x_{C0}*\sin{k_z}*cos{k_x}-2*z_{C0}*\sin{k_x} \\
-2*z_{C0}*\cos{k_x}-2*x_{C0}*\sin{k_x}*\sin{k_z}+2*y_{C0}*\sin{k_x}*cos{k_z} \\
-x_{C0}^2-y_{C0}^2-z_{C0}^2+b^2-a^2*(x_E^2+y_E^2+z_E^2)+2*a*z_E*\sqrt{(x_E^2+y_E^2+z_E^2)}
\end{bmatrix}$
While this will give me numerical values for the entries of $c$, I see no way to calculate the unknowns from that. Four equations, but seven unknowns: $a$, $b$, $k_x$, $k_z$, $x_{C0}$, $y_{C0}$ and $z_{C0}$ (although I am only interested in knowing $a$ and $b$.
I'd appreciate any ideas how I could make this work.
 A: The first step is to identify the closest ellipsoid that fits the given points.
The equation for the ellipsoid is
$ A x^2 + B y^2 + C z^2 + D xy + E xz + F yz + G x + H y + I z + J = 0 $
We can take $A = 1$, and this will result in
$ - x_i^2 = B y_i^2 + C z_i^2 + D x_i y_i + E x_i z_i + F y_i z_i + G x_i + H y_i + I z_i + J, \hspace{15pt} i = 1, \dots , n  $
And this is the linear regression model, for which standard identification results apply.
If the $i$-th row of matrix $M$ is
$M_i = [y_i^2 , z_i^2, x_i y_i, x_i z_i, y_i z_i, x_i, y_i, z_i, 1 ]$
And the $i$-th entry in the output vector $Y$ is
$Y_i = - x_i^2 $
And the parameter vector is
$ X = [B, C, D, E, F, G , H, I, J]^T $
Then we now have the following system of equations
$ Y = M X $
The solution for the 9 unknowns of vector $X$ is given by
$ X = (M^T M)^{-1} M Y $
Now we have all the parameters.  And the equation of the ellipsoid, when written in matrix-vector notation is
$ r^T Q r + b^T r + c = 0 $
where
$ Q = \begin{bmatrix} 1 && \frac{1}{2} D && \frac{1}{2} E \\ \frac{1}{2} D && B && \frac{1}{2} F \\ \frac{1}{2} E && \frac{1}{2} F && C \end{bmatrix} $
$ b = [G , H, I ]^T$
$ c = J$
From this point, we can identify the three axes of the ellipsoid and their orientation (directions), as well the location of the center of the ellipsoid.
Let the three semi-axes of the identified ellipsoid be $\ell_1, \ell_2, \ell_3$ where $\ell_1 \lt \ell_2 \lt \ell_3 $, then we'll take
$r_1 = \ell_2$
$ r_2 = \ell_3 $
And we can now compute the corresponding values of the parameters $a$ and $b$ as follows
$\hat{a} = \sqrt{\dfrac{r_2}{ r_2 - r_1}}$
$\hat{b} = \hat{a}  r_1 $
Now from the model of the egg surface we have
$r^2 + z^2 = b^2 - a^2 (r^2 + z^2) + 2 a z \sqrt{ r^2 + z^2 } $
where $r^2 = x^2 + y^2$
The above equation can be re-written as follows
$r^2 = b^2 - (a \sqrt{r^2 + z^2} - z )^2 $
So that
$(a \sqrt{r^2 + z^2} - z )^2 - b^2 + r^2 =0$
The variables $x, y, z$ in this equation are the coordinates relative to the coordinate frame attached to the translated and rotated egg.  The initial guess of this coordinate frame is available from the identified ellipsoid.
If $P$ is the coordinate of a point with respect to the world frame, and $Q$ is the coordinate of a point with respect to the egg frame, then
$ P = d + R Q $
where $ d = [C_x, C_y, C_z]^T $ is the center identified above, and
$R = [u_1, u_2, u_3 ] $
is a rotation matrix, with
$ u_3 = [\sin \theta \cos \phi, \sin \theta \sin \phi, \cos \theta ]^T $
is the axis of the egg, and
$u_1 = [\cos \theta \cos \phi, \cos \theta \sin \phi, - \sin \theta ]^T$
$u_2 = [-\sin \phi, \cos \phi , 0 ]^T $
$u_1 $ and $u_2$ are the transverse axes.
Next, we have to fine-tune the location of the center of the egg (3 parameters), and the direction of the axis, which is determined by two angles $\theta, \phi$  (2 parameters), and in addition we need to fine-tune the estimates of the parameters $a$ and $b$ of the model (2 parameters).
To implement this identification, we define an error function
$E = \displaystyle \sum E_i^2 = \sum \bigg((a \sqrt{r^2 + z^2} - z )^2 - b^2 + r^2\bigg)^2$
The gradient of this function $E$ is found with respect to the all the parameters (which are $C_x, C_y, C_z, \theta , \phi, a , b $ ).  This can be done numerically by choose a suitable step size of the differential $h$, or exactly (analytically).  The effort in the second form (the analytic form) is probably worth as it leads to much more reliable results.
From here, we want to find the solution of the vector equation
$ \nabla E = 0 $
This a vector equation of seven entries , in seven variables.  This suggests using the Newton-Raphson method, especially that we have a very good guess of the solution from the identification of the ellipsoid.
The guess for the parameters is given by (by the Newton-Raphson method)
$ X_{n+1} =X_{n} - J^{-1}(X_n) Y_{n} $
where $J(X_n) $ is the Jacobian evaluated at $X_n$.  $Y_n$ is the gradient vector ($\nabla E$) at the $n$-th iteration.
The egg model proved to be difficult to identify if all $7$ parameters are subjected to the above update formula together.
So, instead, I did the iteration in two stages.  In the first stage, I kept the last two parameters $a$ and $b$ fixed at $\hat{a}, \hat{b}$, and updated the other $5$ parameters.  After convergence, I let all parameters change.
The results were amazing.  For my simulations, the first stage converges in less than $10$ iterations, and the second stage in less than $5$ iterations.
