利用标定出的相机内外参实现-像素坐标系->相机坐标系->Base坐标系的转换

这里我们的内参是由opencv中相机标定API标定出的，外参用的是AX=XB计算出的，详细过程，请百度。
废话不多说，直接上代码。保存内外参的xml文件很简单，不解释了。代码里有注释。

#include 
#include 

// Eigen 部分
#include 
// 稠密矩阵的代数运算（逆，特征值等）
#include 
//Eigen 几何模块
#include 

#include 

/*
 * 相机坐标系转base坐标系
 * */

using namespace std;
using namespace Eigen;
using namespace cv;

// 内参文件路径
cv::String inCailFilePath="./assets/3DCameraInCailResult.xml";
// 外参文件路径
cv::String exCailFilePath="./assets/3DCameraExCailResult.xml";


/**
 * 将行列坐标，转成相机坐标系中的x,y,z
 * @param piexld 输入 {列, 行, 深度}
 * @param cameraPos 输出 {x,y,z}
 */
void piexl2camera(double *piexld,double *cameraPos){
    Mat cameraMatrix=cv::Mat_(3, 3);;
    // 读取内参
    FileStorage paramFs("./assets/CameraInCailResult_VirtualCamera.yaml",FileStorage::READ);
    paramFs["cameraMatrix"]>>cameraMatrix;

    // 行
    double v = piexld[1];
    double u = piexld[0];
    double d = piexld[2];
    // 内参数据
    double camera_fx = cameraMatrix.at(0,0);
    double camera_fy = cameraMatrix.at(1,1);
    double camera_cx = cameraMatrix.at(0,2);
    double camera_cy = cameraMatrix.at(1,2);
    // 计算相机坐标系中的值
    double z = double(d)/1000; //单位是米
    double x = (u - camera_cx) * z / camera_fx;
    double y = (v - camera_cy) * z / camera_fy;

    cameraPos[0]=x;
    cameraPos[1]=y;
    cameraPos[2]=z;
}


/**
 * @param cameraPos 传入相机坐标系中的值
 * @param basePos 输出机器人Base坐标系中的值
 */
void camera2base(double *cameraPos, double *basePos) {
    // 外参：轴角对 平移距离
    double angle = 0;
    double axisX = 0;
    double axisY = 0;
    double axisZ = 0;
    double translationX = 0;
    double translationY = 0;
    double translationZ = 0;

    cv::FileStorage fs(exCailFilePath, cv::FileStorage::READ);
    // 这里的Angle等要与xml文件中的名字一样
    fs["Angle"] >> angle;
    fs["AxisX"] >> axisX;
    fs["AxisY"] >> axisY;
    fs["AxisZ"] >> axisZ;
    fs["TranslationX"] >> translationX;
    fs["TranslationY"] >> translationY;
    fs["TranslationZ"] >> translationZ;
    // 轴角对
    Vector3d axisMatrix(axisX, axisY, axisZ);
    AngleAxisd angleAxisd(angle, axisMatrix);
    // 获取旋转矩阵
    // 将轴角对变成旋转矩阵，Eigen库中的API
    Matrix3d ratationMatrix = angleAxisd.toRotationMatrix();
    cout << "旋转矩阵：" << angleAxisd.toRotationMatrix() << endl;
    // 获取平移矩阵
    Vector3d translationMatrix(translationX, translationY, translationZ);

    // 获取相机拍摄到的坐标
    Vector3d cameraPosition(cameraPos[0], cameraPos[1], cameraPos[2]);

    //进行转换                    3*3             3*1      +  平移矩阵
    //注意这里是左乘，还有单位的变换。
    Vector3d resultBase = ratationMatrix * cameraPosition + translationMatrix / 1000;
    // 获取输出结果
    cout << "Camera2Base:" << resultBase << endl;
    // 输出结果
    memcpy(basePos,resultBase.data(), sizeof(double)*3);
//    basePos[0]=resultBase[0];
//    basePos[1]=resultBase[1];
//    basePos[2]=resultBase[2];
}



int main(int argc, char **argv)
{

    double cameraPos[3]={0.01,0.01,0.9};
    double basePos[3]={0};
    camera2base(cameraPos, basePos);

    cout<