目录
一、Opencv函数使用
solvePnP原型为:
bool cv::solvePnP ( InputArray objectPoints,
InputArray imagePoints,
InputArray cameraMatrix,
InputArray distCoeffs,
OutputArray rvec,
OutputArray tvec,
bool useExtrinsicGuess = false,
int flags = SOLVEPNP_ITERATIVE
)
其中,rvec以及tvec为旋转向量与平移向量。
在solvePnP调用结束后,需要使用罗德里格斯变化将旋转向量转换成旋转矩阵。
Mat r,t;
solvePnP(pts_3d,pts_2d,K,Mat(),r,t,false,cv::SOLVEPNP_EPNP);
Mat R;
Rodrigues(r, R);
二、目标是求得相机在世界坐标系下的3D坐标。
1、法一:
利用求得的旋转矩阵和平移矩阵:
Pcam代表物体在相机坐标系下的坐标,Pworld代表物体在世界坐标系下的坐标,R和T代表了将点的从世界坐标系下映射到相机坐标系下,可以知道solvePnP求出的刚好是这样的映射关系。
使Pcam = 0,则意味着物体移到了相机坐标系的原点,求出来的Pworld代表了相机在世界坐标系中的位置,P的z轴坐标就是深度信息。
0=RPworld+T
P = -inverse(R)*T
代码为:
//Mat到Eigen格式转换
Eigen::Matrix3f R_n;
Eigen::Vector3f T_n;
cv2eigen(rotMat, R_n);
cv2eigen(tvec, T_n);
Eigen::Vector3f P_oc;
P_oc = -R_n.inverse()*T_n;
cout << "世界坐标" << P_oc <<