halcon matlab 中pose matrix之间的转换_halcon pose

 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

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;
}

对比：

 

 

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 案例代码：

* 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 ()