读取点云文件(.xyz)

    函数用于读取.xyz的点云文件，点云的格式为:

17.371559 -6.531680 -8.080792 0.242422 0.419118 0.874970
15.640106 -16.101347 -9.550241 -0.543610 -0.382877 0.746922
17.750742 -6.395478 -8.307115 0.333093 0.494766 0.802655
15.432834 -15.947010 -9.587061 -0.548083 -0.385148 0.742473
23.626318 -7.729815 -13.608750 0.081697 0.502976 0.860431
15.300377 -15.610346 -9.547507 -0.569658 -0.341132 0.747743
23.975805 -7.512131 -13.775388 0.082388 0.564137 0.821561
24.251831 -7.345085 -13.949208 0.099309 0.574142 0.812711
14.999881 -15.463743 -9.748975 -0.629676 -0.333713 0.701530
14.804974 -15.162496 -9.758424 -0.616575 -0.334426 0.712737
27.607445 -6.731058 -16.160894 0.387612 0.713240 0.583991
14.560967 -14.955154 -9.909436 -0.638394 -0.335827 0.692584
27.938255 -6.707172 -16.443462 0.379390 0.740941 0.554139
14.290791 -14.852806 -10.137550 -0.692395 -0.381273 0.612552
14.386531 -15.114174 -10.178914 -0.719801 -0.337913 0.606384
14.001437 -14.247000 -10.103112 -0.735267 -0.343587 0.584235
13.762934 -13.909647 -10.200064 -0.752330 -0.295280 0.588906

    前面3个数字是坐标，后面3个数字是法向量，有多少行就代表有多少个点。

    代码：

struct Point3d
{
    Point3d(){x=y=z=0.0f;}
    float x;
    float y;
    float z;
    bool operator<(const Point3d& p2 ) const
    {
        if (x == p2.x) 
        {
            if (y == p2.y) 
            {
                return (z < p2.z);
            }
            else return (y < p2.y);
        }
        else return (x < p2.x);
    }
};

int ReadPointCloudFile(const std::string& strPath)
{
    FILE *ifp = fopen(strPath.c_str(),"r");
    if(ifp == NULL)
    {
        printf("Cannot open point cloud file.\n");
        return -1;
    }

    //获取行数，即点的数量，用于设置std::vector的容量
    const int BUFSIZE = 512;
    char buf[BUFSIZE];
    int rowNumber = 0;
    while(fgets(buf,BUFSIZE,ifp) != NULL)
    {
        ++rowNumber;
    }
    fclose(ifp);
    ifp = 0;

    //重新打开文件，读取所有点的信息
    ifp = fopen(strPath.c_str(),"r");
    Point3d pointCoor;
    Point3d pointNormal;

    std::vector<Point3d> vecPtCoor;        //点坐标
    std::vector<Point3d> vecPtNormal;      //法向量
    vecPtCoor.reserve(rowNumber);
    vecPtNormal.reserve(rowNumber);

    while(fgets(buf,BUFSIZE,ifp) != NULL)  // read info line by line
    {
        sscanf(buf, "%f %f %f %f %f %f",
               &(pointCoor.x),   &(pointCoor.y),   &(pointCoor.z),
               &(pointNormal.x), &(pointNormal.y), &(pointNormal.z));

        vecPtCoor.push_back(pointCoor);
        vecPtNormal.push_back(pointNormal);
    }

    fclose(ifp);
    ifp = 0;

    return 0;
}