I have two degrees of freedom robot with a camera mounted on the tool center point where the system is modeled as
$^wT_c = {^wT_z}(\theta_1){^zT_m}(\theta_2) {^mT_c}(\textbf{r})$, $\textbf{r} \in SO3$
I have to calibrate mount to camera transformation ${^mT_c}$ without the robot joint encoder value which means robot joint angles are parameters to estimate as well.
I assume that the following values are given.
The rotation axis of each joint ${^wT_z}(\theta_1){^zT_m}(\theta_2)$ is known.
The world reference frame to camera $^wT_c$ is also known from the camera observation.
The rotation axis of each motor in the camera frame $\textbf{c}_{1,2}$ and in the world frame $\textbf{v}_{1,2}$ is known.
The constraints that I have tried to estimate $\textbf{r}$ are
$\textbf{e}_{w_i} = {^wT_c}({^wT_z}(\theta_{1_i}){^zT_m}(\theta_2) {^mT_c}(\textbf{r}))^{-1}$
$\textbf{e}_{v1} = {^wT_z}(\theta_{1_i})_{[1:3,2]}-\textbf{v}_1$
$\textbf{e}_{v2} = ({^wT_z}(\theta_{1_i}){^zT_m}(\theta_{2_i}))_{[1:3,3]}-\textbf{v}_2$
$\textbf{e}_{c1} = {^wT_z}(\theta_{1_i}){^zT_m}(\theta_{2_i}) {^mT_c}(\textbf{r})\textbf{c}_1-\textbf{v}_1$
$\textbf{e}_{c2} = {^wT_z}(\theta_{1_i}){^zT_m}(\theta_{2_i}) {^mT_c}(\textbf{r})\textbf{c}_2-\textbf{v}_2$
The result of the optimization with simulation data is a bit weird. The estimation of $\theta_{1_i}$ are correct but not $\theta_{2_i}$. $\theta_{2_i}$ are slightly different from the ground truth values. As a result, $\textbf{r}$ is slightly different from the ground truth value. The residual of the constraints drops to the reasonable level which was around 5.6179e-17 after 7 iterations.
So, I guess this is an underconstrained problem but I am not sure what to add to fix the problem. Any idea or thought will be appreciated.
Note: Since the robot joint angles are not known it is not possible to use AX=XB kind of closed-form solution.
