关于坐标系转换部分的代码
pixel2cam函数
/**
* 函数名: pixel2cam
* 传入: Armor &armor (装甲板结构体)
* 传出: Vector3d camera_position (相机坐标)
* 功能: 为传入装甲板赋予相机坐标,返回相机坐标
*/
Eigen::Vector3d AngleSolve::pixel2cam(Armor &armor) {
// 传入装甲板的坐标和类型
armor.camera_position = pnpSolve(armor.armor_pt4,armor.type,armor.R);
return armor.camera_position;
}
将像素坐标系转换为相机坐标系，pnpSolve 函数返回 tv平移向量
cam2imu函数
/**
* 函数名: cam2imu
* 传入: Vector3d cam_pos (相机坐标)
* 传出: Vector3d imu_pos (世界坐标)
* 功能: 通过传入相机坐标,转换为世界坐标
*/
Eigen::Vector3d AngleSolve::cam2imu(Eigen::Vector3d cam_pos) {
Eigen::Vector3d pos_tmp;
Eigen::Vector3d imu_pos;
// 相机坐标系下加偏置
cam_pos += center_offset_position; //原点偏移
// 左右手系，单靠旋转矩阵转换不了，与其矩阵运算不如直接赋值
pos_tmp = {cam_pos[0],cam_pos[2],-cam_pos[1]}; //世界坐标系
imu_pos = RotationMatrix_imu * pos_tmp; //旋转矩阵
return imu_pos;
}
将相机坐标系转换成世界坐标系，我们的相机坐标系是左手坐标系，世界坐标系是右手坐标系
旋转矩阵是不能实现转换的，而且直接更改轴的方向就可以转换
重点在后面的旋转矩阵，这里的旋转是实现相机上转换到C板上的坐标系
为什么会这么复杂呢？这里需要注意，C板在上电的那一刻，坐标系就确定好了，不会因为车的旋转而变化，如下图：
这个坐标系是上电时刻就定死的，不会移动，会移动的是C板和摄像头，也就是说，相机坐标系是会移动的，我们最后的旋转矩阵就是为了将相机坐标系移动回C板上电时刻的坐标系，如下图：
红色的就是C板的世界坐标系，黑色的就是会动的云台和相应的相机坐标系，总结下来就是，我们的车上电的时候（C板上电）会固定死坐标系，同时给我们发送偏移的角度，我们通过偏移的角度将当前计算出来的坐标转换回去，这样我们的坐标就是绝对的坐标系了
这里的旋转矩阵依靠电控发来的四元数，为什么使用四元数，是为了防止万向锁的问题
万向锁问题不详细展开了，请自行查阅
/**
* 函数名: quaternionToRotationMatrix
* 传入: 无
* 传出: Matrix3d R_x
* 功能: 通过电控发来的四元数,计算出旋转矩阵
*/
Eigen::Matrix3d AngleSolve::quaternionToRotationMatrix(float quaternion[4]) {
Eigen::Matrix3d R_x;
// 四元数
float w=quaternion[0],x=quaternion[1],y=quaternion[2],z=quaternion[3];
R_x << 1-2*y*y-2*z*z, 2*x*y-2*z*w, 2*x*z+2*y*w,
2*x*y+2*z*w, 1-2*x*x-2*z*z, 2*y*z-2*x*w,
2*x*z-2*y*w, 2*y*z+2*w*x, 1-2*x*x-2*y*y;
// float roll = atan2(2*y*z + 2*w*x,1 - 2*x*x - 2*y*y)/CV_PI * 180.0f;
// float pitch = asin(2*w*y - 2*x*z)/CV_PI*180.0f;
// float yaw = atan2(2*x*y + 2*w*z, 1 - 2*y*y - 2*z*z)/CV_PI*180.0f;
// std::cout<<"----------[quaternion_euler]-----------"<<std::endl;
// std::cout<<"[roll:] |"<<roll<<std::endl;
// std::cout<<"[pitch:] |"<<pitch<<std::endl;
// std::cout<<"[yaw:] |"<<yaw<<std::endl;
// std::cout<<"----------[get_from_euler]-----------"<<std::endl;
// std::cout<<"[get_roll:] |"<<ab_roll<<std::endl;
// std::cout<<"[get_pitch:] |"<<ab_pitch<<std::endl;
// std::cout<<"[get_yaw:] |"<<ab_yaw<<std::endl;
return R_x;
}
我们这里的四元数转换旋转矩阵，使用的是Hamilton四元数，四元数到旋转矩阵的推导有兴趣的可以去推导
我们这里直接将公式拿过来使用
下面的部分就是反向推导了，也就是从世界坐标系到相机坐标，相机坐标到像素坐标
imu2cam函数
/**
* 函数名: imu2cam
* 传入: Vector3d imu_pos (世界坐标)
* 传出: Vector3d cam_pos (相机坐标)
* 功能: 通过传入世界坐标,转换为相机坐标
*/
Eigen::Vector3d AngleSolve::imu2cam(Eigen::Vector3d imu_pos) {
Eigen::Vector3d tmp_pos;
// 使用逆矩阵将坐标系反推回去
tmp_pos = RotationMatrix_imu.inverse() * imu_pos; //逆矩阵（3x3）逆矩阵的定义是，逆矩阵乘以原矩阵等于单位矩阵
Eigen::Vector3d cam_pos;
cam_pos = {tmp_pos[0],-tmp_pos[2],tmp_pos[1]}; //将坐标系换回相机坐标系
cam_pos -= center_offset_position; //将原点偏差补偿回去
return cam_pos;
}
我们将旋转矩阵求逆就可以将坐标系还原回去，如果你不知道为什么求逆，可以补一下线性代数的知识
其实也就是将原式子左右同乘逆矩阵而已
这样我们就可以还原相机坐标系
cam2pixel函数
/**
* 函数名: cam2pixel
* 传入: Vector3d cam_pos (相机坐标)
* 传出: Point2f pixel_pos (像素坐标)
* 功能: 通过传入相机坐标,转换为像素坐标
* https://www.cnblogs.com/wangguchangqing/p/8126333.html (推导)
*/
cv::Point2f AngleSolve::cam2pixel(Eigen::Vector3d cam_pos) {
Eigen::Vector3d tmp_pixel;
// 使用内参矩阵将相机坐标转换为像素坐标
tmp_pixel = F_EGN * cam_pos;
// 将z的距离归一化
cv::Point2f pixel_pos = cv::Point2f((float)tmp_pixel[0]/tmp_pixel[2],(float)tmp_pixel[1]/tmp_pixel[2]);
return pixel_pos;
}
这里其实在坐标系简介里面介绍过了，可以回去看看这里为什么要将Z的距离归一化，或者看推导的网站进行学习