记录学习过程,向量到欧拉角
目的是将机器人工具指向目标平面法线
- 先通过点云拟合出平面,得到平面方程,相应得到平面法向量(A,B,C)
typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud<PointT> PointCloudT;
cout << "->正在估计平面..." << endl;
pcl::SampleConsensusModelPlane<PointT>::Ptr model_plane(new pcl::SampleConsensusModelPlane<PointT>(cloud_transformed)); //选择拟合点云与几何模型
pcl::RandomSampleConsensus<PointT> ransac(model_plane); //创建随机采样一致性对象
ransac.setDistanceThreshold(0.01); //设置距离阈值,与平面距离小于0.01的点作为内点
ransac.computeModel(); //执行模型估计
PointCloudT::Ptr cloud_plane(new PointCloudT);
vector<int> inliers; //存储内点索引的向量
ransac.getInliers(inliers); //提取内点对应的索引
pcl::copyPointCloud<PointT>(*cloud_transformed, inliers, *cloud_plane);
//输出模型参数Ax+By+Cz+D=0
Eigen::VectorXf coefficient;
ransac.getModelCoefficients(coefficient);
cout << "平面方程为:\n"
<< coefficient[0] << "x + "
<< coefficient[1] << "y + "
<< coefficient[2] << "z + "
<< coefficient[3] << " = 0"
<< endl;
- 固定法线方向,因为法线有可能会为反向
if (coefficient[2] > 0)
{
coefficient[0] = 0 - coefficient[0];
coefficient[1] = 0 - coefficient[1];
coefficient[2] = 0 - coefficient[2];
cout << "翻转:\n" << endl;
}
- 向量转换欧拉角,需要确定两个方向向量,如果只有一个方向,则存在不确定性,这里主要是让Z指向平面法向量,x就固定在45°位置
Eigen::Matrix3f rotM = Eigen::Matrix3f::Identity();
/*计算完都需要归一化处理
* 如果放进去的Z方向和X方向不垂直,那就先计算出Y方向,在重新算出X方向
* 例子
curZ(0, 1, 0);将z指向本体坐标系下的Y方向
curX(0, 0, 1); 将x指向本体坐标系下的Z方向
*/
Eigen::Vector3f curZ(coefficient[0], coefficient[1], coefficient[2]);//定义z的指向
Eigen::Vector3f curX(1, 1, 0);//定义x的指向
curX /= curX.norm();//归一化
Eigen::Vector3f curY = curZ.cross(curX);
curY /= curY.norm();
//重新算出X方向
curX = curY.cross(curZ);
curX /= curX.norm();
rotM(0, 0) = curX(0);
rotM(0, 1) = curY(0);
rotM(0, 2) = curZ(0);
rotM(1, 0) = curX(1);
rotM(1, 1) = curY(1);
rotM(1, 2) = curZ(1);
rotM(2, 0) = curX(2);
rotM(2, 1) = curY(2);
rotM(2, 2) = curZ(2);
std::cout << rotM << std::endl;
std::cout << rotM.eulerAngles(2, 1, 0) * 180 / 3.1415926 << std::endl;