哈尔康姿态与坐标变换之间的关系

我们知道相机标定可以得到相机内参跟相机外参，而相机外参就是参考世界坐标系转换到相机坐标系的位姿，也就是在相机坐标系下世界坐标系的位姿

 可以把位姿转换为相机坐标系下的转换矩阵，通过pose_to_hom_mat3d

pose_to_hom_mat3d (PoseOfWCS, camHwcs)

//转换矩阵转换为位姿

hom_mat3d_to_pose (camHwcs, PoseOfWCS)

//vector_to_pose 可以根据提供的世界坐标值跟图像坐标值映射得到相机坐标系的位姿

vector_to_pose( : : WorldX, WorldY, WorldZ, ImageRow, ImageColumn, CameraParam, Method, QualityType : Pose, Quality)

//得到转换矩阵可以调用affine_trans_point_3d得到

affine_trans_point_3d( : : HomMat3D, Px, Py, Pz : Qx, Qy, Qz)

 //定义了世界坐标相对对象坐标下的坐标

ShiftXObj := 5.5 / 1000.0
ShiftYObj := 5.15 / 1000.0
ControlX := [18.73,-5.35,13.05,0.00] / 1000.0 + ShiftXObj
ControlY := [27.52,27.68,0.00,0.00] / 1000.0 + ShiftYObj
ControlZ := [0.00,0.00,0.00,0.00]

vector_to_pose (ControlX, ControlY, ControlZ, RowCenter, ColCenter, CamParam, 'iterative', 'error', PoseOfObject, Errors)//根据这个映射关系得到相机坐标系的对象位姿

pose_to_hom_mat3d (PoseOfWCS, camHwcs)//相机坐标系下的世界坐标转换得到相机坐标系的转换矩阵
pose_to_hom_mat3d (PoseOfObject, camHobj)//相机坐标下的位姿
* Determine the transformation matrix from object coordinates into world coordinates
hom_mat3d_invert (camHwcs, wcsHcam)
hom_mat3d_compose (wcsHcam, camHobj, wcsHobj)

* 定义了对象坐标系的3D坐标位姿
CornersXObj := [0.89,0.77,12.12,12.18] / 1000.0 + ShiftXObj
CornersYObj := [21.63,8.47,8.45,21.51] / 1000.0 + ShiftYObj
CornersZObj := [0,0,0,0]
* 把对象坐标位姿转换到世界坐标系的位姿
affine_trans_point_3d (wcsHobj, CornersXObj, CornersYObj, CornersZObj, CornersXWCS, CornersYWCS, CornersZWCS)
*  把3D坐标位姿转换到相机坐标系的位姿
affine_trans_point_3d (camHobj, CornersXObj, CornersYObj, CornersZObj, CornersXCam, CornersYCam, CornersZCam)

//把相机坐标系的位姿映射到图像的实际坐标像素
project_3d_point (CornersXCam, CornersYCam, CornersZCam, CamParam, CornersRow, CornersCol)