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

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

#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);