函数用于读取.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;
}