相机内参外参代码

#include <iostream>
#include <cmath>

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

#include <opencv2/opencv.hpp>

/*
 * 相机坐标系转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_<double>(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<double>(0,0);
    double camera_fy = cameraMatrix.at<double>(1,1);
    double camera_cx = cameraMatrix.at<double>(0,2);
    double camera_cy = cameraMatrix.at<double>(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);
    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);
    // 获取旋转矩阵
    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<<basePos[0]<<endl;
    cout<<basePos[1]<<endl;
    cout<<basePos[2]<<endl;

    return 0;
}