如圖：由標定板位姿獲取測量板位姿
1.pose_to_hom_mat3d( : : Pose : HomMat3D)
把三維位姿轉化為齊次變換矩陣
2.hom_mat3d_translate_local( : : HomMat3D, Tx, Ty, Tz : HomMat3DTranslate)
相對於新坐標系的平移變換
/ 1 0 0 \ / Tx \
HomMat3DTranslate = HomMat3D * | 0 1 0 t | t = | Ty |
| 0 0 1 | \ Tz /
\ 0 0 0 1 / 返回值為HomMat3DTranslate矩陣。3.hom_mat3d_rotate_local( : :HomMat3D, Phi, Axis : HomMat3DRotate)
相對於新坐標系的旋轉變換，輸出為原 HomMat3D右乘旋轉矩陣后的齊次變換矩陣。
4.hom_mat3d_to_pose( : : HomMat3D : Pose)
把齊次矩陣轉化為3D位姿態。利用0類代碼，即先平移順序RPT,旋轉gba，以點的形式表達位姿。
原文來自：
大瘋熊和小兔子的幸福生活