AXXB 问题引出

对于两个固定在同一刚体上的传感器（H，E）。以及外参\(T^h_e\). 有与传感器之间的相对位姿，也就是外参与时间无关，因此：

\[ T^h_e = T^{h_t}_{e_t} = T^{h_{t + 1}}_{e_{t + 1}} \]

两个传感器的相对位姿分别为,\(T^{h_t}_{h_{t+1}}, T^{e_t}_{e_{t+1}}\)可以得到：

\[ T^{h_t}_{h_{t+1}} * T^{h_{t + 1}}_{e_{t + 1}} = T^{h_t}_{e_t} * T^{e_t}_{e_{t+1}} \]

得到：

\[ AX = XB \]

李群

刚体运动可以用欧式群表示：

\[ \begin{bmatrix}{} \theta &b \\ \mathbf{0}&1 \end{bmatrix} \]

李群与李代数之间的转换可以由指数映射和对数映射完成。

指数映射

李代数到李群的转换满足指数映射关系，假设\(\left[ w \right] \in so\left( 3 \right)\),\(\exp \left[ w \right] \in SO\left( 3 \right)\)指数映射满足罗德里格斯公式:

\[ \exp \left[ w \right] = I + \frac{\sin \left\| w \right\|}{\left\| w \right\|}\left[ w \right] + \frac{1 - \cos \left\| w \right\|}{\left\| w \right\|^2}\left[ w \right]^2 \]

对数映射

李群到李代数的转换满足对数映射关系，假设\(\theta \in SO\left( 3 \right)\)对数映射为：

\[ \log \theta = \frac{\phi }{2\sin \phi }\left( \theta - \theta ^T \right) \\ \]

其中\(trace(T) \ne -1\):\(\phi = arccos(\frac{trace(R) - 1}{2})\) \(trace(T) = -1\):\(log(R) = \pm\pi\hat{e} , \hat{e} = \frac{R+I}2\)

求解AX=XB

手眼标定问题其实就是求解方程\(AX=XB\), 将\(X\)移到方程的右边，可以得到\(A=XBX^T\)根据上文介绍的对数映射关系，在方程的两边取对数，则可以得到:

\[ \log A = \log XBX^T \]

令\(\log A = \left[ \alpha \right],\log B = \left[ \beta \right]\)得到：

\[ \left[ \alpha \right] = X\left[ \beta \right]X^T = \left[ X\beta \right] \]

从而：

\[ \alpha = X\beta \]

对于手眼标定问题，\(AX=XB\)写成矩阵形式:

\[ \begin{bmatrix} \theta _A&b_A \\ \mathbf{0}&1 \end{bmatrix} \begin{bmatrix} \theta _X&b_X \\ \mathbf{0}&1 \end{bmatrix} = \begin{bmatrix} \theta _X&b_X \\ \mathbf{0}&1 \end{bmatrix} \begin{bmatrix} {\theta _B}&b_B \\ \mathbf{0}&1 \end{bmatrix} \]

展开得到：

\[ \begin{matrix} \theta _A \theta _X = \theta _X \theta _B \\ \theta _A b_X + b_A = \theta _X b_B + b_X \end{matrix} \]

采用“两步法”求解，先解算旋转矩阵，再求得平移向量。由上面推导:

\[ \theta _A \theta _X = \theta _X \theta _B \\ \alpha = \theta _X \beta \]

当存在多组观测值时，使用最小二乘：

\[ \min \sum\limits_{i = 1}^k \left\| \theta _X \beta _i - \alpha _i \right\| ^2 \]

使用SVD解法：

\[ R_X=VU^T \]

其中\(U,V\)为\(M=\sum b_ia_i^T\)的SVD分解：\(M=U\Sigma V^T\) 等价与：

\[ \begin{aligned}R_X=(M^TM)^{-\frac{1}{2}}M^T\end{aligned} \]

求解旋转之后求解平移，构建线性方程：

\[ \begin{bmatrix}R_{A_1}-I\\\ldots\\R_{A_n}-I\end{bmatrix}t_X=\begin{bmatrix}R_Xt_{B_1}-t_{A_1}\\\ldots\\R_Xt_{B_n}-t_{A_n}\end{bmatrix} \]

bool navyAxxb(manif::SE3d& the, std::vector<manif::SE3d> twhij, std::vector<manif::SE3d> tweij)
{
    if (twhij.size() != tweij.size()){
        return false;
    }

    size_t size = twhij.size();
    if(size <= 2){return false;}

    Eigen::Matrix3d I;
    I.setIdentity();
    Eigen::MatrixXd C(0,3), bA(0,1), bB(0,1);

    Eigen::Matrix3d M = Eigen::Matrix3d::Zero();
    for(auto pos = 0; pos < size; pos ++){
        Eigen::Vector3d alpha = twhij[pos].asSO3().log().coeffs();
        Eigen::Vector3d beta = tweij[pos].asSO3().log().coeffs();
        M = beta * alpha.transpose() + M;

        Eigen::MatrixXd C_tmp = C;
        C.resize(C.rows()+3, Eigen::NoChange);
        C << C_tmp, Eigen::Matrix3d::Identity() - twhij[pos].rotation();

        Eigen::MatrixXd V_tmp = bA;
        bA.resize(bA.rows()+3, Eigen::NoChange);
        bA << V_tmp, twhij[pos].translation();

        V_tmp = bB;
        bB.resize(bB.rows()+3, Eigen::NoChange);
        bB << V_tmp, tweij[pos].translation();
    }
    //std::cout << "M:" << std::endl << M << std::endl;
    Eigen::EigenSolver<Eigen::Matrix3d> es(M.transpose() * M);
    Eigen::Matrix3cd D = es.eigenvalues().asDiagonal();
    Eigen::Matrix3cd V = es.eigenvectors();

    Eigen::Matrix3cd lambda = D.inverse().array().sqrt();
    Eigen::Matrix3cd theta_x = V * lambda * V.inverse() * M.transpose();

    for(int i=0; i < bB.rows()/3; i++){
        bB.block(i*3,0,3,1) = theta_x.real()*bB.block(i*3,0,3,1);
    }
    bA = bA - bB;
    the.quat(Eigen::Quaterniond(theta_x.real()));
    the.translation((C.transpose()*C).inverse() * C.transpose() * bA);
    return true;
}

---

Reference

[0] Tsai, Roger Y., and Reimar K. Lenz. “A new technique for fully autonomous and efficient 3D robotics hand/eye calibration.” IEEE Transactions on robotics and automation 5.3 (1989): 345-358.
[1] Horaud, Radu, and Fadi Dornaika. “Hand-eye calibration.” The international journal of robotics research 14.3 (1995): 195-210.
[2] Park, Frank C., and Bryan J. Martin. “Robot sensor calibration: solving AX= XB on the Euclidean group.” IEEE Transactions on Robotics and Automation10.5 (1994): 717-721.
[3] Daniilidis, Konstantinos. “Hand-eye calibration using dual quaternions.” The International Journal of Robotics Research 18.3 (1999): 286-298.