Question

我有2个已知的3d点OC1和OC2，它们是空间中2轴绘图的原点，我需要计算它们之间的3D旋转矩阵。 enter image description here

我知道使用R1＆amp; T1我可以到达Oc1并使用R2＆amp; T2我可以到达Oc2但是我需要计算Oc1和Oc2之间的3D旋转矩阵。我刚考虑过这条规则：

oMc1=(R1 | T1) and oMc2=(R2 | T2) and what I want is:
c1Mc2 = (oMc1)^-1 oMc2

所以我尝试实现它，这是我的代码：

    vector <Point3f> listOfPointsOnTable;
    cout << "******** DATA *******" << endl;
    listOfPointsOnTable.push_back(Point3f(0,0,0));
    listOfPointsOnTable.push_back(Point3f(100,0,0));
    listOfPointsOnTable.push_back(Point3f(100,100,0));
    listOfPointsOnTable.push_back(Point3f(0,100,0));

    cout << endl << "Scene points :" << endl;
    for (int i = 0; i < listOfPointsOnTable.size(); i++)
    {
        cout << listOfPointsOnTable[i] << endl;
    }

    //Define the optical center of each camera
    Point3f centreOfC1 = Point3f(23,0,50);
    Point3f centreOfC2 = Point3f(0,42,20);
    cout << endl << "Center Of C1: " << centreOfC1 << " , Center of C2 : " << centreOfC2 << endl;

    //Define the translation and rotation between main axis and the camera 1 axis
    Mat translationOfC1 = (Mat_<double>(3, 1) << (0-centreOfC1.x), (0-centreOfC1.y), (0-centreOfC1.z));
    float rotxC1 = 0, rotyC1 = 0, rotzC1 = -45;
    int focaleC1 = 2;
    Mat rotationOfC1 = rotation3D(rotxC1, rotyC1,rotzC1);
    cout << endl << "Translation from default axis to C1: " << translationOfC1 << endl;
    cout << "Rotation from default axis to C1: " << rotationOfC1 << endl;
    Mat transformationToC1 = buildTransformationMatrix(rotationOfC1, translationOfC1);
    cout << "Transformation from default axis to C1: " << transformationToC1 << endl << endl;

    //Define the translation and rotation between main axis and the camera 2 axis
    Mat translationOfC2 = (Mat_<double>(3, 1) << (0-centreOfC2.x), (0-centreOfC2.y), (0-centreOfC2.z));
    float rotxC2 = 0, rotyC2 = 0, rotzC2 = -90;
    int focaleC2 = 2;
    Mat rotationOfC2 = rotation3D(rotxC2, rotyC2,rotzC2);
    cout << endl << "Translation from default axis to C2: " << translationOfC2 << endl;
    cout << "Rotation from default axis to C2: " << rotationOfC2 << endl;
    Mat transformationToC2 = buildTransformationMatrix(rotationOfC2, translationOfC2);
    cout << "Transformation from default axis to C2: " << transformationToC2 << endl << endl;

    Mat centreOfC2InMat = (Mat_<double>(3, 1) << centreOfC2.x, centreOfC2.y, centreOfC2.z);
    Mat centreOfC2InCamera1 = rotationOfC1 * centreOfC2InMat + translationOfC1;
    Mat translationBetweenC1AndC2 = -centreOfC2InCamera1;
    cout << endl << "****Translation from C2 to C1" << endl;
    cout << translationBetweenC1AndC2 << endl;
    Mat centreOfC1InMat = (Mat_<double>(3, 1) << centreOfC1.x, centreOfC1.y, centreOfC1.z);
    Mat centreOfC1InCamera2 = rotationOfC2 * centreOfC1InMat + translationOfC2;
    Mat translationBetweenC2AndC1 = -centreOfC1InCamera2;
    cout << "****Translation from C1 to C2" << endl;
    cout << translationBetweenC2AndC1 << endl;

    cout << "Tran1-1 * Trans2 = " << transformationToC1.inv() * transformationToC2 << endl;
    cout << "Tran2-1 * Trans1 = " << transformationToC2.inv() * transformationToC1 << endl;

Mat rotation3D(int alpha, int beta, int gamma)
{
    // Rotation matrices around the X, Y, and Z axis
    double alphaInRadian = alpha * M_PI / 180.0;
    double betaInRadian = beta * M_PI / 180.0;
    double gammaInRadian = gamma * M_PI / 180.0;
    Mat RX = (Mat_<double>(3, 3) <<
                  1,          0,           0,
                  0, cosf(alphaInRadian), sinf(alphaInRadian),
                  0, -sinf(alphaInRadian),  cosf(alphaInRadian));
    Mat RY = (Mat_<double>(3, 3) <<
                  cosf(betaInRadian), 0, sinf(betaInRadian),
                  0, 1,          0,
                  -sinf(betaInRadian), 0,  cosf(betaInRadian));
    Mat RZ = (Mat_<double>(3, 3) <<
                  cosf(gammaInRadian), sinf(gammaInRadian), 0,
                  -sinf(gammaInRadian),cosf(gammaInRadian), 0,
                  0,          0,           1);
    // Composed rotation matrix with (RX, RY, RZ)
    Mat R = RX * RY * RZ;
    return R;
}
Mat buildTransformationMatrix(Mat rotation, Mat translation)
{
    Mat transformation = (Mat_<double>(4, 4) <<
                          rotation.at<double>(0,0), rotation.at<double>(0,1), rotation.at<double>(0,2), translation.at<double>(0,0),
                          rotation.at<double>(1,0), rotation.at<double>(1,1), rotation.at<double>(1,2), translation.at<double>(1,0),
                          rotation.at<double>(2,0), rotation.at<double>(2,1), rotation.at<double>(2,2), translation.at<double>(2,0),
                          0, 0, 0, 1);

    return transformation;
}

这是输出：

//Origin of 3 axis     
O(0,0,0), OC1 (23, 0, 50), OC2 (0, 42, 20)

Translation from default axis to OC1: [-23;
 0;
 -50]
Rotation from default axis to OC1: [0.7071067690849304, -0.7071067690849304, 0;
 0.7071067690849304, 0.7071067690849304, 0;
 0, 0, 1]
Trans1 = Transformation from default axis to OC1: [0.7071067690849304, -0.7071067690849304, 0, -23;
 0.7071067690849304, 0.7071067690849304, 0, 0;
 0, 0, 1, -50;
 0, 0, 0, 1]

Translation from default axis to OC2: [0;
 -42;
 -20]
Rotation from default axis to OC2: [-4.371138828673793e-08, -1, 0;
 1, -4.371138828673793e-08, 0;
 0, 0, 1]
Trans2 = Transformation from default axis to OC2: [-4.371138828673793e-08, -1, 0, 0;
 1, -4.371138828673793e-08, 0, -42;
 0, 0, 1, -20;
 0, 0, 0, 1]

(Trans1)-1 * (Trans2) = [0.7071067623795453, -0.7071068241967844, 0, -13.43502907247513;
 0.7071068241967844, 0.7071067623795453, 0, -45.96194156373071;
 0, 0, 1, 30;
 0, 0, 0, 1]
(Trans2)-1 * (Trans1) = [0.7071067381763105, 0.7071067999935476, 0, 42.00000100536185;
 -0.7071067999935475, 0.7071067381763104, -0, 22.99999816412165;
 0, 0, 1, -30;
 0, 0, 0, 1]

//Calculation of translation between OC1 and OC2:
****Translation from C2 to C1
[52.69848430156708;
 -29.69848430156708;
 30]
****Translation from C1 to C2
[1.005361930594972e-06;
 19;
 -30]

正如您在上面所看到的，(Trans1)-1 * (Trans2)的第4列既不与C2->C1的翻译也不同，也不等于C2->C1的翻译

它让我觉得c1Mc2 = (oMc1)^-1 oMc2没有达到我想要的但我不明白为什么？有没有其他解决方案可以得到我想要的东西？

Answer 1

Oc1的旋转矩阵定义为局部XYZ轴的分量：

      | Xc1[0] Yc1[0] Zc1[0] |
R_1 = | Xc1[1] Yc1[1] Zc1[1] |
      | Xc1[2] Yc1[2] Zc1[2] |

，同样适用于R_2的{{1}}。

如果它们之间的相对旋转为Oc2，那么您可以定义

因此：

R_2 = R_1*R

这就是你所需要的一切。

两个3D点之间的3D旋转矩阵

1 个答案: