pose_to_hom_mat3d (TransPose, HomMat3D)
原理：
matlab 验证：
function [a,b]=getMatrix(pose)
syms x y z;
x=deg2rad(pose(4));
y=deg2rad(pose(5));
z=deg2rad(pose(6));
mat_x=[1 0 0 0;0 cos(x) -sin(x) 0;0 sin(x) cos(x) 0; 0 0 0 1];
mat_y=[cos(y) 0 sin(y) 0;0 1 0 0;-sin(y) 0 cos(y) 0;0 0 0 1];
mat_z=[cos(z) -sin(z) 0 0;sin(z) cos(z) 0 0;0 0 1 0;0 0 0 1];
% 最常用的集中方式
if pose(7)==0
a=mat_x*mat_y*mat_z;
elseif pose(7)==1
a=mat_z*mat_y*mat_x;
elseif pose(7)==2
a=mat_z*mat_x*mat_y;
end
M_t=[1 0 0 pose(1);
0 1 0 pose(2);
0 0 1 pose(3);
0 0 0 1];
b=M_t*a;
end
pose_compose
* pose1->m1 pose2->m2
* PoseCompose=m1*m2
pose_compose (pose1, TransPose, PoseCompose)matlab
clear;
pose1=[0.0 0.0 1.0 0.0 90.0 0.0 0];
poseTrans=[1.0 2.5 2.0 90.0 20.0 0.0 0];
[~,b]=getMatrix(pose1);
[~,b2]=getMatrix(poseTrans);
mat=b*b2rigid_trans_object_model_3d
halcon 案例代码：
* This example program shows how to use the operator
* rigid_trans_object_model_3d in HALCON. In this example
* a cylinder is created. In a second step, the cylinder is
* tranformed with a rigid transformation. The generated
* primitves of the 3D object models are visualized.
*
* Initialize program
dev_close_window ()
dev_open_window (0, 0, 640, 480, 'black', WindowHandle)
set_display_font (WindowHandle, 16, 'mono', 'true', 'false')
gen_cam_par_area_scan_division (0.016, 0, 5e-6, 5e-6, 320, 240, 640, 480, CamParam)
create_pose (-1, 1, 20, 110, 0, 270, 'Rp+T', 'gba', 'point', DisplayPose)
create_pose (1, 2.5, 2, 90, 20, 0, 'Rp+T', 'gba', 'point', TransPose)
*
* Create a cylinder without transformation
gen_cylinder_object_model_3d ([0,0,1,0,90,0,0], 0.5, 0, 2, ObjectModel3DCylinder)
get_object_model_3d_params (ObjectModel3DCylinder, 'primitive_pose', pose1)
disp_object_model_3d_safe (WindowHandle, ObjectModel3DCylinder, CamParam, DisplayPose, 'disp_pose', 'true')
disp_message (WindowHandle, 'Cylinder before transformation', 'window', 12, 12, 'black', 'true')
disp_continue_message (WindowHandle, 'black', 'true')
stop ()
*
* Apply a rigid transformation
* 姿态和矩阵之间的想换转换
pose_to_hom_mat3d (pose1, HomMat3D)
pose_to_hom_mat3d (TransPose, HomMat3D2)
* 计算两个pose 之间的乘法
* pose1->m1 pose2->m2
* PoseCompose=m1*m2
pose_compose (pose1, TransPose, PoseCompose)
pose_to_hom_mat3d (PoseCompose, HomMat3DPosecompose)
***
rigid_trans_object_model_3d (ObjectModel3DCylinder, TransPose, ObjectModel3DRigidTrans)
get_object_model_3d_params (ObjectModel3DRigidTrans, 'primitive_pose', pose2)
pose_to_hom_mat3d (pose2, HomMat3D24)
dev_clear_window ()
disp_object_model_3d_safe (WindowHandle, ObjectModel3DRigidTrans, CamParam, pose2, 'disp_pose', 'true')
disp_message (WindowHandle, 'Cylinder after transformation', 'window', 12, 12, 'black', 'true')
stop ()
