Pcl点云中长方体的位姿估计

由于之前做了物体的分割，要用于机械臂的抓取，所以绕不开物体的位姿估计，所以本篇主要讲解的完成分割后如何进行长方体的位姿估计。

由于使用的分割算法是从低层次到高层次，所以在分割的结果蕴含了物体各个面的组成信息，并知道了各个面的大小，当然不知道的情况也可以进行处理，之后会进行相关阐述。

已知物体各个面的组成

由于已经知道了各个面的组成信息，首先选择最大面，计算得到对应的法向量，使其作为物体的z轴向量，进而选择次大面，计算得到对应的法向量，使其为物体的y轴向量，进而对其进行一系列的旋转平移操作，完成物体的尺寸和位姿变换关系的计算：
（1）首先计算得到物体z轴与相机坐标系下的z轴夹角为$\theta$，将其在中心点旋转$\theta$使物体中心坐标系下的z轴与相机坐标系的z轴平行;
（2）将物体中心平移与相机中心重合；
（3）计算得到物体y轴与相机坐标系下的y轴夹角为$\beta$，将其在中心点旋转$\beta$使物体中心坐标系下的y轴与相机坐标系的y轴重合。
由此可以得到与相机坐标系 $C_c$， 与物体坐标系 $C_o$ 之间的位姿变化关系中平移向量为$T$，旋转矩阵为：

$$R = \begin{bmatrix} \cos\theta & -\sin\theta & 0 \\ \sin\theta & \cos\theta & 0 \\ 0 & 0 & 1\end{bmatrix} \begin{bmatrix} \cos\beta & 0 & -\sin\beta \\ 0 & 1 & 0 \\ \sin\beta & \cos\beta & 0 \end{bmatrix}$$

结果可以得到:

不知道物体各个面的组成

当原始输入的数据不知道对应面的主成分，此时就需要对应的pca来进行最大面的拟合，然后再进行次小面的计算，然后按照上面的方法完成物体的位姿估计即可。

matlab源码要回实验室了再上传到博客，c++上只做了分割后得到结果的位姿估计，matlab上有将一个物体分成多个面然后再进行位姿估计，也用matlab随机生成数据，验证了该方法的正确性，好吧，这当然是正确的，我又too young too simple了。

void getSurfaceCub()
{
    std::vector<cubParam> cubsParam;
    CloudPtr cloudFilter(new CloudT);
    pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
    pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
    // Create the segmentation object
    pcl::SACSegmentation<PointT> seg;
    // Optional
    seg.setOptimizeCoefficients(true);
    //Mandatory
    seg.setModelType(pcl::SACMODEL_PLANE);
    seg.setMethodType(pcl::SAC_RANSAC);
    seg.setMaxIterations(1000);
    seg.setDistanceThreshold(0.01);
    pcl::ExtractIndices<PointT> extract;
    pcl::MomentOfInertiaEstimation <PointT> feature_extractor;
    for (int i = 0; i < surface_.size(); ++i)
    {
        seg.setInputCloud(surface_[i]);
        seg.segment(*inliers, *coefficients);

        extract.setInputCloud(surface_[i]);
        extract.setIndices(inliers);
        extract.setNegative(false);
        extract.filter(*cloudFilter);

        feature_extractor.setInputCloud(cloudFilter);
        feature_extractor.compute();
        //compute the width and height of the plane cloud
        std::vector <float> moment_of_inertia;
        std::vector <float> eccentricity;

		PointT min_point_OBB;
		PointT max_point_OBB;
		PointT position_OBB;
        Eigen::Matrix3f rotational_matrix_OBB;
        float major_value, middle_value, minor_value;
        Eigen::Vector3f major_vector, middle_vector, minor_vector;
        Eigen::Vector3f mass_center;

        feature_extractor.getMomentOfInertia (moment_of_inertia);
        feature_extractor.getEccentricity (eccentricity);
        feature_extractor.getOBB (min_point_OBB, max_point_OBB, position_OBB, rotational_matrix_OBB);
        feature_extractor.getEigenValues (major_value, middle_value, minor_value);
        feature_extractor.getEigenVectors (major_vector, middle_vector, minor_vector);
        feature_extractor.getMassCenter (mass_center);
        feature_extractor.getOBB(min_point_OBB, max_point_OBB, position_OBB, rotational_matrix_OBB);

        cubParam single_cub;

        Eigen::Vector3f position(position_OBB.x, position_OBB.y, position_OBB.z);
        Eigen::Quaternionf quat(rotational_matrix_OBB);
        float width = max_point_OBB.x - min_point_OBB.x;
        float height = max_point_OBB.y - min_point_OBB.y;
        float depth = max_point_OBB.z - min_point_OBB.z;

        single_cub.depth = depth;
        single_cub.width = width;
        single_cub.height = height;
        single_cub.rotation = quat;
        single_cub.position = position;
        cubsParam.push_back(single_cub);

    }
    return cubsParam;
}

由于涉及到类成员函数，然后我就只简单的贴一部分实现，有什么问题课题探讨一下。