C++求解AX=XB（手眼标定用）

描述

C++求解AX = XB

以下的两个代码，虽然可以直接运行，但是我本人使用它们得到的手眼矩阵，旋转矩阵R值的大小看起来是合理的，但位置向量t的三个数值大概都是大于10万的，显然是错误的。

但没有时间去研究错误的原因，但是找到这些代码已属不易，因此还是贴在这里，供以后研究

代码

C++求解AX = XB

代码网上有很多，这里贴出来两个。Tsai_HandEye和
Navy_HandEye

Tsai_HandEye

Mat skew( Mat res )
{
	Mat result = (Mat_<double>(3, 3) << 0, -res.at<double>(2), res.at<double>(1),
		res.at<double>(2), 0, -res.at<double>(0),
		-res.at<double>(1), res.at<double>(0), 0);
		
	return result;
}
void Tsai_HandEye(Mat Hcg, vector<Mat> Hgij, vector<Mat> Hcij)
{
	CV_Assert(Hgij.size() == Hcij.size());
	int nStatus = Hgij.size();

	Mat Rgij(3, 3, CV_64FC1);
	Mat Rcij(3, 3, CV_64FC1);

	Mat rgij(3, 1, CV_64FC1);
	Mat rcij(3, 1, CV_64FC1);

	double theta_gij;
	double theta_cij;

	Mat rngij(3, 1, CV_64FC1);
	Mat rncij(3, 1, CV_64FC1);

	Mat Pgij(3, 1, CV_64FC1);
	Mat Pcij(3, 1, CV_64FC1);

	Mat tempA(3, 3, CV_64FC1);
	Mat tempb(3, 1, CV_64FC1);

	Mat A;
	Mat b;
	Mat pinA;

	Mat Pcg_prime(3, 1, CV_64FC1);
	Mat Pcg(3, 1, CV_64FC1);
	Mat PcgTrs(1, 3, CV_64FC1);

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

	Mat Tgij(3, 1, CV_64FC1);
	Mat Tcij(3, 1, CV_64FC1);

	Mat tempAA(3, 3, CV_64FC1);
	Mat tempbb(3, 1, CV_64FC1);
	Mat AA;
	Mat bb;
	Mat pinAA;
	Mat Tcg(3, 1, CV_64FC1);

	for (int i = 0; i < nStatus; i++)
	{
		Hgij[i](Rect(0, 0, 3, 3)).copyTo(Rgij);
		Hcij[i](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, 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));

	//Computer Translation 
	for (int i = 0; i < nStatus; i++)
	{
		Hgij[i](Rect(0, 0, 3, 3)).copyTo(Rgij);
		Hcij[i](Rect(0, 0, 3, 3)).copyTo(Rcij);
		Hgij[i](Rect(3, 0, 1, 3)).copyTo(Tgij);
		Hcij[i](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, DECOMP_SVD);
	Tcg = pinAA * bb;
	cout << Rcg << endl;

    // std::cout<<pinAA<<std::endl;
	Rcg.copyTo(Hcg(Rect(0, 0, 3, 3)));
	Tcg.copyTo(Hcg(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;

}

• Navy_HandEye

void Navy_HandEye(cv::Mat Hcg, std::vector<cv::Mat> Hgij, std::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 alpha1(3, 1, CV_64FC1);
    cv::Mat beta1(3, 1, CV_64FC1);
    cv::Mat alpha2(3, 1, CV_64FC1);
    cv::Mat beta2(3, 1, CV_64FC1);
    cv::Mat A(3, 3, CV_64FC1);
    cv::Mat B(3, 3, CV_64FC1);

    cv::Mat alpha(3, 1, CV_64FC1);
    cv::Mat beta(3, 1, CV_64FC1);
    cv::Mat M(3, 3, CV_64FC1, cv::Scalar(0));

    cv::Mat MtM(3, 3, CV_64FC1);
    cv::Mat veMtM(3, 3, CV_64FC1);
    cv::Mat vaMtM(3, 1, CV_64FC1);
    cv::Mat pvaM(3, 3, CV_64FC1, cv::Scalar(0));

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

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

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

    cv::Mat tempCC(3, 3, CV_64FC1);
    cv::Mat tempdd(3, 1, CV_64FC1);

    cv::Mat C;
    cv::Mat d;
    cv::Mat Tx(3, 1, CV_64FC1);
    
    //Compute rotation
    if (Hgij.size() == 2) // Two (Ai,Bi) pairs
    {
        cv::Rodrigues(Hgij[0](cv::Rect(0, 0, 3, 3)), alpha1);
        cv::Rodrigues(Hgij[1](cv::Rect(0, 0, 3, 3)), alpha2);
        cv::Rodrigues(Hcij[0](cv::Rect(0, 0, 3, 3)), beta1);
        cv::Rodrigues(Hcij[1](cv::Rect(0, 0, 3, 3)), beta2);

        alpha1.copyTo(A.col(0));
        alpha2.copyTo(A.col(1));
        (alpha1.cross(alpha2)).copyTo(A.col(2));

        beta1.copyTo(B.col(0));
        beta2.copyTo(B.col(1));
        (beta1.cross(beta2)).copyTo(B.col(2));

        Rx = A*B.inv();

    }
    else // More than two (Ai,Bi) pairs
    {
        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);

            cv::Rodrigues(Rgij, alpha);
            cv::Rodrigues(Rcij, beta);

            M = M + beta*alpha.t();
        }

        MtM = M.t()*M;
        eigen(MtM, vaMtM, veMtM);

        pvaM.at<double>(0, 0) = 1 / sqrt(vaMtM.at<double>(0, 0));
        pvaM.at<double>(1, 1) = 1 / sqrt(vaMtM.at<double>(1, 0));
        pvaM.at<double>(2, 2) = 1 / sqrt(vaMtM.at<double>(2, 0));

        Rx = veMtM*pvaM*veMtM.inv()*M.t();
    }
    
    //Computer 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);
        
        tempCC = eyeM - Rgij;
        tempdd = Tgij - Rx * Tcij;

        C.push_back(tempCC);
        d.push_back(tempdd);
    }
    
    Tx = (C.t()*C).inv()*(C.t()*d);
    
    Rx.copyTo(Hcg(cv::Rect(0, 0, 3, 3)));
    Tx.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;
}