#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;
}
相机内参外参代码
最新推荐文章于 2023-04-06 11:02:31 发布