pose_to_hom_mat3d (TransPose, HomMat3D)
principle:
matlab verification:
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
pcl:
//
int pose_to_hom_mat3d(vector<double>&pose,Eigen::Matrix4d &matarix)
{
if (pose.size()!=7)
{
return -1;
}
// 转换为弧度
double roll_arc = pose[3] * M_PI/180.0;
double pitch_arc = pose[4] * M_PI / 180.0;
double yaw_arc = pose[5] * M_PI / 180.0;
// 初始化欧拉角
Eigen::Vector3d euler_angle(roll_arc, pitch_arc, yaw_arc);
// 使用eigen库,欧拉角转旋转矩阵
Eigen::AngleAxisd x_rollAngle (euler_angle[0], Eigen::Vector3d::UnitX()); // x
Eigen::AngleAxisd Y_yawAngle(euler_angle[1], Eigen::Vector3d::UnitY()); // y
Eigen::AngleAxisd z_pitchAngle(euler_angle[2], Eigen::Vector3d::UnitZ()); //z
Eigen::Matrix3d q = Eigen::Matrix3d::Zero();
if (pose[6]==0) // xyz
{
q = x_rollAngle*Y_yawAngle*z_pitchAngle;
}
else if (pose[6] == 1) // zxy
{
q = z_pitchAngle *x_rollAngle* Y_yawAngle ;
}
else if (pose[6] == 2) // zyx
{
q = z_pitchAngle * Y_yawAngle * x_rollAngle;
}
else if (pose[6] == 3) // xzy
{
q = x_rollAngle*z_pitchAngle * Y_yawAngle ;
}
Eigen::Matrix4d Tranlate_Matrix = Eigen::Matrix4d::Zero();
Eigen::Matrix4d R_Matrix = Eigen::Matrix4d::Zero();
Tranlate_Matrix(0, 0) = 1.0;
Tranlate_Matrix(1, 1) = 1.0;
Tranlate_Matrix(2, 2) = 1.0;
Tranlate_Matrix(0,3) = pose[0];
Tranlate_Matrix(1, 3) = pose[1];
Tranlate_Matrix(2, 3) = pose[2];
Tranlate_Matrix(3, 3) = 1.0;
for (size_t i = 0; i < 3; i++)
{
for (size_t j = 0; j < 3; j++)
{
R_Matrix(i, j) = q(i,j);
}
}
R_Matrix(3,3) = 1.0;
matarix = Tranlate_Matrix*R_Matrix;
cout << "\matarix =\n" << matarix << endl << endl;
return 0;
}
Compared:
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*b2
rigid_trans_object_model_3d
halcon case code:
* 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 ()