手眼标定

　　在基于位置的视觉控制中，手眼标定起着关键性作用，手眼标定是解决相机与机器人之间位置关系的理论，手眼关系一般有两种形式，一种是相机固定在机器人执行器末端随机器人运动，称为Eye-in-Hand,另一种为相机固定在机器人附近的地面或设备上，不随机器人运动，称为Eye-to-Hand.无论是怎样的形式，最终解决的问题都是同样的，即看到目标物在什么位置，告诉机器人去什么位置执行加工动作。Eye-in-Hand的形式中，相机随机器人运动视野比较宽不会受到机械臂的阻挡，但是存在运动过量可能看不到目标物体，导致目标丢失。Eye-to-Hand的形式中，相机不随机器人运动，固定在一处，能够看到在视野中目标，但是在机械臂移动的过程中容易受到阻挡。两种形式各有利弊，下面是每种形式求取手眼关系的过程。

　　在上图Eye-in-Hand关系图中，机械臂末端带着相机从{C1}移动到{C2}的过程中相机和机械臂末端执行器的固定关系保持不变即：

[{}^{C1}{T_{H1}} = {}^{C2}{T_{H2}}]

　　同时带有Marker的目标物在机器人世界坐标系中的关系也不变。

[{}^B{T_O} = {}^{ m{B}}{T_{H1}} ullet {}^{C1}T_{H1}^{ - 1} ullet {}^{C1}{T_O} = {}^{ m{B}}{T_{H2}} ullet {}^{C2}T_{H2}^{ - 1} ullet {}^{C2}{T_O}]

[({}^{ m{B}}T_{H2}^{ - 1} ullet {}^{ m{B}}{T_{H1}}) ullet {}^{C1}T_{H1}^{ - 1} = {}^{C2}T_{H2}^{ - 1} ullet ({}^{C2}{T_O} ullet {}^{C1}T_O^{ - 1})]

　　写作：$AX = XB$，求解X即可得出手眼之间的关系。

　　在上图Eye-to-Hand关系图中对于机器人末端执行器夹着Marker从{O1}移动到和{O2}执行任务时，执行器末端和Marker之间的位置关系保持不变，因此在两个位置存在如下的转换关系：

[{}^{O1}{T_{H1}} = {}^CT_{O1}^{ - 1} ullet {}^C{T_B} ullet {}^{ m{B}}{T_{H1}}]

[{}^{O2}{T_{H2}} = {}^CT_{O2}^{ - 1} ullet {}^C{T_B} ullet {}^{ m{B}}{T_{H2}}]

[{}^CT_{O1}^{ - 1} ullet {}^C{T_B} ullet {}^{ m{B}}{T_{H1}} = {}^CT_{O2}^{ - 1} ullet {}^C{T_B} ullet {}^{ m{B}}{T_{H2}}]

[({}^C{T_{O2}} ullet {}^CT_{O1}^{ - 1}) ullet {}^C{T_B} = {}^C{T_B} ullet ({}^{ m{B}}{T_{H2}} ullet {}^{ m{B}}T_{H1}^{ - 1})]

　　写作：$AX = XB$，求解X即可得出手眼之间的关系。

注意在求取的过程中多采集几组A，B的数据结果才会准确，不然无法求解。

//向量转反对称矩阵

cv::Mat Calculate::skew(cv::Mat vec)
{

    cv::Mat skewM(3, 3, CV_64FC1);

    double vx = vec.at<double>(0, 0);

    double vy = vec.at<double>(1, 0);

    double vz = vec.at<double>(2, 0);

    skewM.at<double>(0, 0) = 0.0; skewM.at<double>(0, 1) = -vz; skewM.at<double>(0, 2) = vy;

    skewM.at<double>(1, 0) = vz; skewM.at<double>(1, 1) = 0.0; skewM.at<double>(1, 2) = -vx;

    skewM.at<double>(2, 0) = -vy; skewM.at<double>(2, 1) = vx; skewM.at<double>(2, 2) = 0.0;

    return skewM;

}
Solve AX=XB, A-RT_Tcpij, B-RT_Camij, B2A

Tsai_HandEye(cv::Mat Hcg, vector<cv::Mat> Hgij, vector<cv::Mat> Hcij)

{

    CV_Assert(Hgij.size() == Hcij.size());

    int nStatus = Hgij.size();

    cv::Mat Rgij(3, 3, CV_64FC1);

    cv::Mat Rcij(3, 3, CV_64FC1);

    cv::Mat rgij(3, 1, CV_64FC1);

    cv::Mat rcij(3, 1, CV_64FC1);

    double theta_gij;

    double theta_cij;

    cv::Mat rngij(3, 1, CV_64FC1);

    cv::Mat rncij(3, 1, CV_64FC1);

    cv::Mat Pgij(3, 1, CV_64FC1);

    cv::Mat Pcij(3, 1, CV_64FC1);

    cv::Mat tempA(3, 3, CV_64FC1);

    cv::Mat tempb(3, 1, CV_64FC1);

    cv::Mat A;

    cv::Mat b;

    cv::Mat pinA;

    cv::Mat Pcg_prime(3, 1, CV_64FC1);

    cv::Mat Pcg(3, 1, CV_64FC1);

    cv::Mat PcgTrs(1, 3, CV_64FC1);

    cv::Mat Rcg(3, 3, CV_64FC1);

    cv::Mat eyeM = cv::Mat::eye(3, 3, CV_64FC1);

    cv::Mat Tgij(3, 1, CV_64FC1);

    cv::Mat Tcij(3, 1, CV_64FC1);

    cv::Mat tempAA(3, 3, CV_64FC1);

    cv::Mat tempbb(3, 1, CV_64FC1);

    cv::Mat AA;

    cv::Mat bb;

    cv::Mat pinAA;

    cv::Mat Tcg(3, 1, CV_64FC1);

    for (int i = 0; i < nStatus; i++)

    {

        Hgij[i](cv::Rect(0, 0, 3, 3)).copyTo(Rgij);

        Hcij[i](cv::Rect(0, 0, 3, 3)).copyTo(Rcij);

        Rodrigues(Rgij, rgij);

        Rodrigues(Rcij, rcij);

        theta_gij = norm(rgij);

        theta_cij = norm(rcij);

        rngij = rgij / theta_gij;

        rncij = rcij / theta_cij;

        Pgij = 2 * sin(theta_gij / 2)*rngij;

        Pcij = 2 * sin(theta_cij / 2)*rncij;

        tempA = skew(Pgij + Pcij);

        tempb = Pcij - Pgij;

        A.push_back(tempA);

        b.push_back(tempb);

    }

    //Compute rotation

    invert(A, pinA, cv::DECOMP_SVD);

    Pcg_prime = pinA * b;

    Pcg = 2 * Pcg_prime / sqrt(1 + norm(Pcg_prime) * norm(Pcg_prime));

    PcgTrs = Pcg.t();

    Rcg = (1 - norm(Pcg) * norm(Pcg) / 2) * eyeM + 0.5 * (Pcg * PcgTrs + sqrt(4 - norm(Pcg)*norm(Pcg))*skew(Pcg));

    //Compute Translation

    for (int i = 0; i < nStatus; i++)

    {

        Hgij[i](cv::Rect(0, 0, 3, 3)).copyTo(Rgij);

        Hcij[i](cv::Rect(0, 0, 3, 3)).copyTo(Rcij);

        Hgij[i](cv::Rect(3, 0, 1, 3)).copyTo(Tgij);

        Hcij[i](cv::Rect(3, 0, 1, 3)).copyTo(Tcij);

        tempAA = Rgij - eyeM;

        tempbb = Rcg * Tcij - Tgij;

        AA.push_back(tempAA);

        bb.push_back(tempbb);

    }

    invert(AA, pinAA, cv::DECOMP_SVD);

    Tcg = pinAA * bb;

    Rcg.copyTo(Hcg(cv::Rect(0, 0, 3, 3)));

    Tcg.copyTo(Hcg(cv::Rect(3, 0, 1, 3)));

    Hcg.at<double>(3, 0) = 0.0;

    Hcg.at<double>(3, 1) = 0.0;

    Hcg.at<double>(3, 2) = 0.0;

    Hcg.at<double>(3, 3) = 1.0;


}