Warning - it's been awhile since I've dived into matrix+optimization math, so bear with me.
I'm trying to determine the center of rotation of a single 3D link (think a rod attached to a ball joint) using a camera. Similiar to Ehrig, the camera is tracking a marker on the rod and can determine its relative rotation from local frame to ground frame ($R_i$) and position in ground frame ($t_i$) at any given sample.
Define $c$ as the vector from a ground origin to the ball joint and $\hat{c}$ as the vector from the marker on the rod to the ball joint such that $c = R_i\hat{c} + t_i$. Then the resultant cost function
$$f_c(c,\hat{c}) = \sum_{i=1}^n \lVert R_i\hat{c} +t_i - c\rVert^2 $$
Can be written as the linear least squares problem with I as identity:
$$ \begin{pmatrix} R_1 & -I \\\\ \vdots & \vdots \\\\ R_n & -I \end{pmatrix} \begin{pmatrix} \hat{c} \\\\ c \end{pmatrix} = - \begin{pmatrix} t_1 \\\\ \vdots \\\\ t_n \end{pmatrix} $$
this can be solved using normal equations (full confession, haven't worked this out myself):
$$ \begin{pmatrix} nI & -\sum_{i=1}^n R_i^T \\\\ -\sum_{i=1}^n R_i & nI \end{pmatrix} \begin{pmatrix} \hat{c} \\\\ c \end{pmatrix} = \begin{pmatrix} -\sum_{i=1}^n R_i^Tt_i \\\\ \sum_{i=1}^n t_i \end{pmatrix} $$
which then can yield a closed solution for c as follows:
$$ \left( n \left( n-1 \right) I -\sum_{i,j=1,i \ne j}^n R_iR_j^T \right)c = (n-1)\sum_{i=1}^nt_i -\sum_{i,j=1,i\ne j}^nR_iR_j^Tt_j $$
This can be solved for c once the left and right expressions are developed.
The problem is that the $R_i$ matrices are noisy due to sensor noise. The $t_i$ vectors are not. I'm hoping a weighted least squares approach could help by letting me de-weight the contribution from the $R$ matrix in the solution, but not sure if the weights come out in the close form easily.
All of that was to minimize $\sum_{i=1}^n \left( R_i\hat{c} +t_i - c \right)$
Would an appropriate cost function assuming $w_r$ and $w_t$ are weights for $R$ and $t$ respectively, and $w_r + w_t = 1$ holds be as follows?
$$\sum_{i=1}^n \left( w_r \left( R_i\hat{c} +t_i -c\right)^2 + w_t \left( t_i -c\right)^2 \right)$$
And if so, is there a clear closed form development from there? It seems that the weighting values should shoehorn into the original closed form solution.
Thanks!