初步接触这方面,根据自己的理解先简单实现效果。熟练掌握和使用后再进行优化。
因为百度搜索时,大部分答主描述的比较复杂,这里记录一下如何直接使用
//其中pose1是由机械手坐标X、Y、Z和Rx、Ry、Rz组成。
int Pose2Isometry3f(const std::vector<float>& pose1, Eigen::Isometry3f& matrix)
{
float roll = pose1[3] * PI / 180;
float pitch = pose1[4] * PI / 180;
float yaw = pose1[5] * PI / 180;
Eigen::AngleAxisf rotX(roll, Eigen::Vector3f::UnitX()); // 轴角表达公式
Eigen::AngleAxisf rotY(pitch, Eigen::Vector3f::UnitY()); // 轴角表达公式
Eigen::AngleAxisf rotZ(yaw, Eigen::Vector3f::UnitZ()); // 轴角表达公式
Eigen::Translation3f trans(pose1[0], pose1[1], pose1[2]);//位移量,录入当前位置
matrix = trans * rotZ * rotY * rotX;
std::cout << "Hand Matrix = " << matrix.matrix() << std::endl;//机器人位姿转换为矩阵的形式
return 0;
}
int main()
{
Eigen::Isometry3f PoseMat;
std::vector<float> pose1 = {0.76227, 0.59453, 0.28947, -178.9, 0.87, 92.96};//X、Y、Z、Rx、Ry、Rz
Pose2Isometry3f(pose1, PoseMat);
}
结果如下,跟halcon中的算子pose_to_hom_mat3d的输出结果相同