AX=XB李群李代数知识求解方程

文章转载自：经典手眼标定算法之Navy的OpenCV实现 ：http://blog.csdn.net/yunlinwang/article/details/51871520

在我的上一篇博客中已经介绍了Tsai的手眼标定算法，下面主要介绍Frank C. Park and Bryan J. Martin在文献Robot sensor calibration: solving AX=XB on the Euclidean group中提出的手眼标定算法，该算法也被称为Navy手眼标定算法，该算法的主要创新点为利用李群理论的知识来求解手眼标定经典方程。该算法基于OpenCV的C++版本程序可去CSDN资源下载，MATLAB版本作者为苏黎世理工的Christian Wengert，也可在此处下载。

 

 
 

   
   

    
    

   
   
   
   

    
    

     
     void Navy_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 alpha1(3, 1, CV_64FC1);
    
    
   
   

   
   

    
    

   
   
   
   

    
    
    
     
     Mat beta1(3, 1, CV_64FC1);
    
    
   
   

   
   

    
    

   
   
   
   

    
    
    
     
     Mat alpha2(3, 1, CV_64FC1);
    
    
   
   

   
   

    
    

   
   
   
   

    
    
    
     
     Mat beta2(3, 1, CV_64FC1);
    
    
   
   

   
   

    
    

   
   
   
   

    
    
    
     
     Mat A(3, 3, CV_64FC1);
    
    
   
   

   
   

    
    

   
   
   
   

    
    
    
     
     Mat B(3, 3, CV_64FC1);
    
    
   
   

   
   

    
    

   
   
   
   

    
    
 
    
    
   
   

   
   

    
    

   
   
   
   

    
    
    
     
     Mat alpha(3, 1, CV_64FC1);
    
    
   
   

   
   

    
    

   
   
   
   

    
    
    
     
     Mat beta(3, 1, CV_64FC1);
    
    
   
   

   
   

    
    

   
   
   
   

    
    
    
     
     Mat M(3, 3, CV_64FC1, Scalar(0));
    
    
   
   

   
   

    
    

   
   
   
   

    
    
 
    
    
   
   

   
   

    
    

   
   
   
   

    
    
    
     
     Mat MtM(3, 3, CV_64FC1);
    
    
   
   

   
   

    
    

   
   
   
   

    
    
    
     
     Mat veMtM(3, 3, CV_64FC1);
    
    
   
   

   
   

    
    

   
   
   
   

    
    
    
     
     Mat vaMtM(3, 1, CV_64FC1);
    
    
   
   

   
   

    
    

   
   
   
   

    
    
    
     
     Mat pvaM(3, 3, CV_64FC1, Scalar(0));
    
    
   
   

   
   

    
    

   
   
   
   

    
    
 
    
    
   
   

   
   

    
    

   
   
   
   

    
    
    
     
     Mat Rx(3, 3, CV_64FC1);
    
    
   
   

   
   

    
    

   
   
   
   

    
    
 
    
    
   
   

   
   

    
    

   
   
   
   

    
    
    
     
     Mat Tgij(3, 1, CV_64FC1);
    
    
   
   

   
   

    
    

   
   
   
   

    
    
    
     
     Mat Tcij(3, 1, CV_64FC1);
    
    
   
   

   
   

    
    

   
   
   
   

    
    
 
    
    
   
   

   
   

    
    

   
   
   
   

    
    

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

   
   

    
    

   
   
   
   

    
    
 
    
    
   
   

   
   

    
    

   
   
   
   

    
    
    
     
     Mat tempCC(3, 3, CV_64FC1);
    
    
   
   

   
   

    
    

   
   
   
   

    
    
    
     
     Mat tempdd(3, 1, CV_64FC1);
    
    
   
   

   
   

    
    

   
   
   
   

    
    
 
    
    
   
   

   
   

    
    

   
   
   
   

    
    

     
         Mat C;
    
    
   
   

   
   

    
    

   
   
   
   

    
    

     
         Mat d;
    
    
   
   

   
   

    
    

   
   
   
   

    
    
    
     
     Mat Tx(3, 1, CV_64FC1);
    
    
   
   

   
   

    
    

   
   
   
   

    
    
 
    
    
   
   

   
   

    
    

   
   
   
   

    
    
    
     
     //Compute rotation
    
    
   
   

   
   

    
    

   
   
   
   

    
    
    
     
     if (Hgij.size() == 
     
     2) 
     
     // Two (Ai,Bi) pairs
    
    
   
   

   
   

    
    

   
   
   
   

    
    

     
         {
    
    
   
   

   
   

    
    

   
   
   
   

    
    

     
             Rodrigues(Hgij[
     
     0](Rect(
     
     0, 
     
     0, 
     
     3, 
     
     3)), alpha1);
    
    
   
   

   
   

    
    

   
   
   
   

    
    

     
             Rodrigues(Hgij[
     
     1](Rect(
     
     0, 
     
     0, 
     
     3, 
     
     3)), alpha2);
    
    
   
   

   
   

    
    

   
   
   
   

    
    

     
             Rodrigues(Hcij[
     
     0](Rect(
     
     0, 
     
     0, 
     
     3, 
     
     3)), beta1);
    
    
   
   

   
   

    
    

   
   
   
   

    
    

     
             Rodrigues(Hcij[
     
     1](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](Rect(
     
     0, 
     
     0, 
     
     3, 
     
     3)).copyTo(Rgij);
    
    
   
   

   
   

    
    

   
   
   
   

    
    

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

   
   

    
    

   
   
   
   

    
    
 
    
    
   
   

   
   

    
    

   
   
   
   

    
    

     
                 Rodrigues(Rgij, alpha);
    
    
   
   

   
   

    
    

   
   
   
   

    
    

     
                 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](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);
    
    
   
   

   
   

    
    

   
   
   
   

    
    
 
    
    
   
   

   
   

    
    

   
   
   
   

    
    

     
             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(Rect(
     
     0, 
     
     0, 
     
     3, 
     
     3)));
    
    
   
   

   
   

    
    

   
   
   
   

    
    

     
         Tx.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;
    
    
   
   

   
   

    
    

   
   
   
   

    
    
 
    
    
   
   

   
   

    
    

   
   
   
   

    
    

     
     }