点云端面转换为2D平面图

• 有时候我们需要获取点云的某一面或者截面用平面图的方式进行展示，这个时候就需要舍弃点云一个维度的数据将其映射到二维上。
• • 以下部分观点仅代表个人看法，可能存在误差之处还望指正

1. 利用PCA对点云进行摆正

• 在对点云进行映射前比较主要的点是如何获取我们所需要的端面，对于一个规则质量均匀的点云而言如果需要获取它某一端面的信息可以通过PCA将点云转换至原点坐标系下，直接舍弃一个维度的数据将其投影为图像。

//利用主成分分析将点云回正到原点坐标系
void pcaRotaion(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
{
    Eigen::Vector4f pcaCentroid;
    pcl::compute3DCentroid(*cloud ,pcaCentroid);
    Eigen::Matrix3f covariance;
    pcl::computeCovarianceMatrix(*cloud,pcaCentroid,covariance);
    Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eigen_solver(covariance, Eigen::ComputeEigenvectors);
    Eigen::Matrix3f eigenVectorsPCA = eigen_solver.eigenvectors();
    Eigen::Vector3f eigenValuesPCA = eigen_solver.eigenvalues();
    eigenVectorsPCA.col(2) = eigenVectorsPCA.col(0).cross(eigenVectorsPCA.col(1)); //校正主方向间垂直
    eigenVectorsPCA.col(0) = eigenVectorsPCA.col(1).cross(eigenVectorsPCA.col(2));
    eigenVectorsPCA.col(1) = eigenVectorsPCA.col(2).cross(eigenVectorsPCA.col(0));
    Eigen::Matrix4f tm = Eigen::Matrix4f::Identity();
    //可计算出逆变换矩阵，方便将转换坐标系后的点云转回原点云
    Eigen::Matrix4f tm_inv = Eigen::Matrix4f::Identity();
    tm.block<3, 3>(0, 0) = eigenVectorsPCA.transpose();   //R
    tm.block<3, 1>(0, 3) = -1.0f * (eigenVectorsPCA.transpose()) *(pcaCentroid.head<3>());//  -R*t
    tm_inv = tm.inverse();
    pcl::transformPointCloud(*cloud, *cloud, tm);

}

2. 分割点云端面

• 对于质量不均匀形状不太规则的点云而言可以先利用分割的方法（例如RANSAC平面拟合/区域生长分割/欧式聚类分割等）得到某一点云平面

2.1 RANSAC平面拟合

• 对于点云中平面点数最多的点云平面，利用RANSAC拟合一次基本就可以提取到该平面。

void :ransacplane(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
{
    pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
//    pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
    pcl::SACSegmentation<pcl::PointXYZ> seg;
    seg.setOptimizeCoefficients(true);
    seg.setModelType(pcl::SACMODEL_PLANE);
    seg.setMethodType(pcl::SAC_RANSAC);
    //    seg.setMaxIterations(9999);
    seg.setDistanceThreshold(1.0);
    seg.setInputCloud(cloud);
    seg.segment(*inliers_,*coefficients);
//    std::cout<<"平面参数个数: "<values.size()<
    std::cout<<"<--------------------拟合平面方程----------------->"<<std::endl;
    std::cout<<"normal.x: "<<coefficients->values[0]<<std::endl;
    std::cout<<"normal.y: "<<coefficients->values[1]<<std::endl;
    std::cout<<"normal.z: "<<coefficients->values[2]<<std::endl;
    std::cout<<"d: "<<coefficients->values[3]<<std::endl;
    pcl::copyPointCloud<pcl::PointXYZ>(*cloud,*inliers_,*cloud_);
}

2.2 extractIndices索引提取

对于并非点数最多的点云可以利用extractIndices索引提取获取去除了平面点数最多的点云平面的剩余点云继续进行平面拟合获取下一平面

pcl::PointCloud<pcl::PointXYZ>::Ptr extractIndices(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
{
    pcl::ExtractIndices<pcl::PointXYZ> extract;
//    pcl::IndicesPtr index_ptr(new vector(clusters[0].indices));
    extract.setInputCloud(cloud);
    extract.setIndices(inliers_);
    extract.setNegative(true);
    extract.filter(*cloud_);
    return cloud_;
}

2.3 区域生长分割

void regionGrowing(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
{
    pcl::RegionGrowing<pcl::PointXYZ,pcl::Normal> reg;
    reg.setMinClusterSize(100);
    reg.setMaxClusterSize(100000);
    reg.setSearchMethod(tree_);
    //邻域点数 周边多少个点来决定这是一个平面 决定容错率 设置大 某一个点稍微有点歪也可以接受 设置小则检测的平面都会很小
    reg.setNumberOfNeighbours(50);
    reg.setInputCloud(cloud);
    reg.setInputNormals(cloudNormals_);
    reg.setSmoothnessThreshold(3.0/180.0*M_PI);//平滑阈值,法向量的角度差
    reg.setCurvatureThreshold(0.5);//曲率阈值,代表平坦的程度
    std::vector <pcl::PointIndices> clusters;
    reg.extract(clusters);
    colorCloud_=reg.getColoredCloud();
//    std::cout<<"聚类数目"<
    int j = 0;
    for (std::vector<pcl::PointIndices>::const_iterator it = clusters.begin(); it != clusters.end(); ++it)
    {
        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster(new pcl::PointCloud<pcl::PointXYZ>);

        for (std::vector<int>::const_iterator pit = it->indices.begin(); pit != it->indices.end(); pit++)
            cloud_cluster->points.push_back(cloud->points[*pit]);
        cloud_cluster->width = cloud_cluster->points.size();
        cloud_cluster->height = 1;
        cloud_cluster->is_dense = true;

//        std::cout << "PointCloud representing the Cluster: " << cloud_cluster->points.size() << " data points."
//            << std::endl;
        std::stringstream ss;
        ss << "cloud_cluster_" << j << ".pcd";
        pcl::io::savePCDFileASCII(ss.str(), *cloud_cluster);
//        cout << ss.str() << "Saved" << endl;
        //这里判断选择端面
        if (***)
        {
        }
    }
j++;
}

3. 拟合平面方程

• 为了提高准确性，我们可以再拟合出该平面的平面方程（可以使用RANSAC拟合/SVD分解/最小二乘等）

3.1 RANSAC拟合平面方程

• 利用上方的RANSAC进行拟合的话可以直接得到平面参数normal和d

3.2 利用SVD计算点云平面方程

//通过SVD分解计算平面方程
void compute(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
{
    std::cout<<"区域生长分割平面点数： "<<cloud->size()<<std::endl;
    Eigen::MatrixXd ccloud(cloud->size(),3);
        for (int idx = 0; idx < cloud->size(); ++idx) {
//            ccloud.row(idx) = Eigen::Vector3d{cloud->points[idx].x,
//                        cloud->points[idx].y,
//                        cloud->points[idx].z};
            ccloud(idx,0)=cloud->points[idx].x;
            ccloud(idx,1)=cloud->points[idx].y;
            ccloud(idx,2)=cloud->points[idx].z;
        }
    // 1、计算质心(求每一列的平均值)
    Eigen::RowVector3d centroid = ccloud.colwise().mean();
    // 2、去质心
    Eigen::MatrixXd demean = ccloud;
    demean.rowwise() -= centroid;
    // 3、SVD分解求解协方差矩阵的特征值特征向量
    Eigen::JacobiSVD<Eigen::MatrixXd> svd(demean, Eigen::ComputeThinU | Eigen::ComputeThinV);
    Eigen::Matrix3d V = svd.matrixV();
    Eigen::MatrixXd U = svd.matrixU();
//    Eigen::Matrix3d S = U.inverse() * demean * V.transpose().inverse();
    // 5、平面的法向量a,b,c
    Eigen::RowVector3d normal;
    normal << V(0,2), V(1,2), V(2,2);
    // 6、原点到平面的距离d
    double d = -normal * centroid.transpose();
    //设置的结构体用来接收平面方程系数
    planarEquations_.A = V(0,2);
    planarEquations_.B = V(1,2);
    planarEquations_.C = V(2,2);
    planarEquations_.D = d;
}

4. 投影/模型滤波

• 利用得到的平面方程对源点云进行投影滤波或者模型滤波以得到更为精确的点云平面

4.1 投影滤波

void projectInliers(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
{
    //填充ModelCoefficients的值,使用ax+by+cz+d=0平面模型，其中 a=b=d=0,c=1 也就是X0Y平面
    //定义模型系数对象，并填充对应的数据
    //创建ProjectInliers对象，使用ModelCoefficients作为投影对象的模型参数
    pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients());
    coefficients->values.resize(4);
    coefficients->values[0] = planarEquations_.A;
    coefficients->values[1] = planarEquations_.B;
    coefficients->values[2] = planarEquations_.C;
    coefficients->values[3] = planarEquations_.D;
    //创建投影滤波对象
    pcl::ProjectInliers<pcl::PointXYZ> proj;
    //设置对象对应的投影模型
    proj.setModelType(pcl::SACMODEL_PLANE);
    proj.setInputCloud(cloud);
    //设置模型对应的系数
    proj.setModelCoefficients(coefficients);
    //保存投影结果
    proj.filter(*cloud_);
    pcl::io::savePCDFile("投影点云.pcd",*cloud_);
}

4.2 模型滤波

void modelOutlierRemoval(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
{
    //利用计算的得到的平面方程对点云进行模型滤波
    //x^2 + y^2 + z^2 = 1
    //position.x: 0, position.y: 0, position.z:0, radius: 1
    pcl::ModelCoefficients plane_coeff;
    plane_coeff.values.resize(4);
    plane_coeff.values[0] =planarEquations_.A;
    plane_coeff.values[1] =planarEquations_.B;
    plane_coeff.values[2] =planarEquations_.C;
    plane_coeff.values[3] =planarEquations_.D;
    pcl::ModelOutlierRemoval<pcl::PointXYZ> plane_filter;
    plane_filter.setModelCoefficients(plane_coeff);
    plane_filter.setThreshold(1.0);
    //设置滤波模型类型
    plane_filter.setModelType(pcl::SACMODEL_PLANE);
    plane_filter.setInputCloud(cloud);
    plane_filter.filter(*cloud_);
//    std::cout<<"模型滤波点云点数: "<size()<
    pcaRotaion(cloud_);
    pcl::io::savePCDFile("modelCloud1.pcd",*cloud_);
    std::cout<<"模型滤波后点数: "<<cloud_->size()<<std::endl;
}

5. 对点云进行格网化生成图像

void MainWindow::showResult2(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
{
    pcl::getMinMax3D(*cloud,min_p,max_p);
    double xmin = min_p.y;
    double xmax = max_p.y;
    double ymin = min_p.z;
    double ymax = max_p.z;

    double len=0.1;
          //对于vector 可以利用*min_element和*max_element来找最大最小值
//        xmin = *min_element(XArr.begin(), XArr.end());
//        xmax = *max_element(XArr.begin(), XArr.end());
//        ymin = *min_element(YArr.begin(), YArr.end());
//        ymax = *max_element(YArr.begin(), YArr.end());

    double Xmin = xmin - 200 * len;
    double Xmax = xmax + 200 * len;
    double Ymin = ymin - 200 * len;
    double Ymax = ymax + 200 * len;

    int rows = ceil((Ymax - Ymin) / len);
    int columns = ceil((Xmax - Xmin) / len);
    std::cout<<"行数： "<<rows<<std::endl;
    std::cout<<"列数： "<<columns<<std::endl;
    std::vector<pcl::PointXYZ> **GridPoints=new std::vector<pcl::PointXYZ>*[rows];
    for (int i=0;i<rows;i++)
    {
        GridPoints[i]=new std::vector<pcl::PointXYZ>[columns];
    }
    for (int i=0;i<cloud->size();i++)
    {
        int rowid=ceil((cloud->points[i].z-Ymin)/len);
        int colid=ceil((cloud->points[i].y-Xmin)/len);
        GridPoints[rowid][colid].push_back(cloud->points[i]);
    }

    cv::Mat originImg(rows,columns,CV_8UC3,cv::Scalar(0));
    for (int i=0;i<rows;i++)
    {
            if(GridPoints[i][j].size()>0)
            {
                originImg.ptr<uchar>(i)[j*3]=255;
                originImg.ptr<uchar>(i)[j*3+1]=255;
                originImg.ptr<uchar>(i)[j*3+2]=255;
            }
    }
    cv::imwrite("originImg.jpg",originImg);
    QString temp="originImg.jpg";
    pixImage.load(temp);
    if(pixImage.size().width()>ui->label->size().width()||
        pixImage.size().height()>ui->label->size().height())
    {
        pixImage=pixImage.scaled(ui->label->size(),Qt::KeepAspectRatio,Qt::SmoothTransformation);
    }
    if(pixImage.width()==0&&pixImage.height()==0)
    {
        std::cout<<"load img error"<<std::endl;
    }
    else
    {
        ui->label->setPixmap(pixImage);
        ui->label->setAlignment(Qt::AlignCenter);
        ui->label->setSizePolicy(QSizePolicy::Ignored,QSizePolicy::Ignored);
    }
}