pcl小知识（八）——xyzrgb与xyz转换、kd树、ply文件、点索引

转载自：https://segmentfault.com/a/1190000007125502

 

一、将xyzrgb格式转换为xyz格式的点云

#include <pcl/io/pcd_io.h>
#include <ctime>
#include <Eigen/Core>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>

using namespace std;
typedef pcl::PointXYZ point;
typedef pcl::PointXYZRGBA pointcolor;

int main(int argc,char **argv)
{
        pcl::PointCloud<pointcolor>::Ptr input (new pcl::PointCloud<pointcolor>);
        pcl::io::loadPCDFile(argv[1],*input);
        

        pcl::PointCloud<point>::Ptr output (new pcl::PointCloud<point>);
        int M = input->points.size();
        cout<<"input size is:"<<M<<endl;

        for (int i = 0;i <M;i++)
        {
                point p;
                p.x = input->points[i].x;
                p.y = input->points[i].y;
                p.z = input->points[i].z; 
                output->points.push_back(p);
        }
        output->width = 1;
        output->height = M;
        
        cout<< "size is"<<output->size()<<endl;
        pcl::io::savePCDFile("output.pcd",*output);

}

 

 

 

二、flann快速kdtree 查询k近邻

   //平均密度计算
        pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;  //创建一个快速k近邻查询,查询的时候若该点在点云中，则第一个近邻点是其本身
        kdtree.setInputCloud(cloud);
        int k =2;
        float everagedistance =0;
        for (int i =0; i < cloud->size()/2;i++)
        {
                vector<int> nnh ;
                vector<float> squaredistance;
                //  pcl::PointXYZ p;
                //   p = cloud->points[i];
                kdtree.nearestKSearch(cloud->points[i],k,nnh,squaredistance);
                everagedistance += sqrt(squaredistance[1]);
                //   cout<<everagedistance<<endl;
        }

        everagedistance = everagedistance/(cloud->size()/2);
        cout<<"everage distance is : "<<everagedistance<<endl;
        

#include <pcl/kdtree/kdtree_flann.h>
        pcl::KdTreeFLANN<pcl::PointXYZ> kdtree; //创建KDtree
        kdtree.setInputCloud (in_cloud);
        pcl::PointXYZ searchPoint; //创建目标点，（搜索该点的近邻）
        searchPoint.x = 1;
        searchPoint.y = 2;
        searchPoint.z = 3;

        //查询近邻点的个数
        int k = 10; //近邻点的个数
        std::vector<int> pointIdxNKNSearch(k); //存储近邻点集的索引
        std::vector<float>pointNKNSquareDistance(k); //近邻点集的距离
        if (kdtree.nearestKSearch(searchPoint,k,pointIdxNKNSearch,pointNKNSquareDistance)>0)
        {
                for (size_t i = 0; i < pointIdxNKNSearch.size (); ++i)
                        std::cout << "    "  <<   in_cloud->points[ pointIdxNKNSearch[i] ].x 
                                  << " " << in_cloud->points[ pointIdxNKNSearch[i] ].y 
                                  << " " <<in_cloud->points[ pointIdxNKNSearch[i] ].z 
                                  << " (squared distance: " <<pointNKNSquareDistance[i] << ")" << std::endl;
        }

        //半径为r的近邻点
        float radius = 40.0f;  //其实是求的40*40距离范围内的点
        std::vector<int> pointIdxRadiusSearch;  //存储的对应的平方距离
        std::vector<float> a;
        if ( kdtree.radiusSearch (searchPoint, radius, pointIdxRadiusSearch, a) > 0 )
        {
          for (size_t i = 0; i < pointIdxRadiusSearch.size (); ++i)
                  std::cout << "    "  <<   in_cloud->points[ pointIdxRadiusSearch[i] ].x 
                            << " " <<in_cloud->points[ pointIdxRadiusSearch[i] ].y 
                            << " " << in_cloud->points[ pointIdxRadiusSearch[i] ].z 
                            << " (squared distance: " <<a[i] << ")" << std::endl;
        }

 

三、关于ply文件

后缀命名为.ply格式文件，常用的点云数据文件。ply文件不仅可以存储点数据，而且可以存储网格数据. 用emacs打开一个ply文件，观察表头，如果表头element face的值为0,ze则表示该文件为点云文件，如果element face的值为某一正整数N，则表示该文件为网格文件，且包含N个网格.
所以利用pcl读取 ply 文件，不能一味用pcl::PointCloud<PointT>::Ptr cloud (new pcl::PointCloud<PintT>)来读取。
在读取ply文件时候，首先要分清该文件是点云还是网格类文件。如果是点云文件，则按照一般的点云类去读取即可，官网例子,就是这样。
如果ply文件是网格类，则需要

    pcl::PolygonMesh mesh;
    pcl::io::loadPLYFile(argv[1],mesh);
    pcl::io::savePLYFile("result.ply", mesh);

读取。（官网例子之所以能成功，是因为它对模型进行了细分处理，使得网格变成了点）

四、计算点的索引

例如sift算法中，pcl无法直接提供索引（主要原因是sift点是通过计算出来的，在某些不同参数下，sift点可能并非源数据中的点，而是某些点的近似），若要获取索引，则可利用以下函数：

void getIndices (pointcloud::Ptr cloudin, pointcloud keypoints, pcl::PointIndices::Ptr indices)
{
        pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
        kdtree.setInputCloud(cloudin);
        std::vector<float>pointNKNSquareDistance; //近邻点集的距离
        std::vector<int> pointIdxNKNSearch;

        for (size_t i =0; i < keypoints.size();i++)
        {
                kdtree.nearestKSearch(keypoints.points[i],1,pointIdxNKNSearch,pointNKNSquareDistance);
                // cout<<"the distance is:"<<pointNKNSquareDistance[0]<<endl;
                // cout<<"the indieces is:"<<pointIdxNKNSearch[0]<<endl;
                
                indices->indices.push_back(pointIdxNKNSearch[0]);
                
        }

}

其思想就是：将原始数据插入到flann的kdtree中，寻找keypoints的最近邻，如果距离等于0,则说明是同一点，提取索引即可.