|
本帖最后由 和你一样小馒头 于 2014-7-3 16:09 编辑
本人最近在做有关三维重建的项目,现在利用kinect生成了三维点云数据,然后想要把点云坐标写入一个pcd文件,但是在把生成点云的工程代码与pcd_write.cpp结合的时候,遇到了阻碍,编译倒是成功了,可pcd文件建立不了,数据更是无法写入。代码如下:- #include <pcl/io/pcd_io.h>
- #include <pcl/point_types.h>
- #include <XnCppWrapper.h>
- #include <iostream>
- #include <iomanip>
- #include <vector>
- using namespace xn;
- using namespace std;
- //point cloud data struct
- struct SColorPoint3D
- {
- float X;
- float Y;
- float Z;
- float R;
- float G;
- float B;
- SColorPoint3D( XnPoint3D pos, XnRGB24Pixel color )
- {
- X = pos.X;
- Y = pos.Y;
- Z = pos.Z;
- R = (float)color.nRed / 255;
- G = (float)color.nGreen / 255;
- B = (float)color.nBlue / 255;
- }
- };
- void GeneratePointCloud( DepthGenerator& rDepthGen,
- const XnDepthPixel* pDepth,
- const XnRGB24Pixel* pImage,
- vector<SColorPoint3D>& vPointCloud)
- {
- // number of point is the number of 2D image pixel
- DepthMetaData mDepthMD;
- rDepthGen.GetMetaData( mDepthMD );
- unsigned int uPointNum = mDepthMD.FullXRes() * mDepthMD.FullYRes();
- // build the data structure for convert
- XnPoint3D* pDepthPointSet = new XnPoint3D[ uPointNum ];
- unsigned int i, j, idxShift, idx;
- for( j = 0; j < mDepthMD.FullYRes(); ++j )
- {
- idxShift = j * mDepthMD.FullXRes();
- for( i = 0; i < mDepthMD.FullXRes(); ++i )
- {
- idx = idxShift + i;
- pDepthPointSet[idx].X = i;
- pDepthPointSet[idx].Y = j;
- pDepthPointSet[idx].Z = pDepth[idx];
- }
- }
- // un-project points to real world
- XnPoint3D* p3DPointSet = new XnPoint3D[ uPointNum ];
- rDepthGen.ConvertProjectiveToRealWorld( uPointNum, pDepthPointSet, p3DPointSet );
- delete[] pDepthPointSet;
- // build point cloud
- for( i = 0; i < uPointNum; ++ i )
- {
- // skip the depth 0 points
- //if( p3DPointSet[i].Z == 0 )
- //continue;
- vPointCloud.push_back( SColorPoint3D( p3DPointSet[i], pImage[i] ) );
- }
- delete[] p3DPointSet;
- }
-
- int main(void)
- {
- XnStatus eResult = XN_STATUS_OK;
- int i = 0;
-
-
- // init
- Context mContext;
- eResult = mContext.Init();
- DepthGenerator mDepthGenerator;
- eResult = mDepthGenerator.Create(mContext);
- ImageGenerator mImageGenerator;
- eResult = mImageGenerator.Create(mContext);
- // set output mode
- XnMapOutputMode mapMode;
- mapMode.nXRes = XN_VGA_X_RES;
- mapMode.nYRes = XN_VGA_Y_RES;
- mapMode.nFPS = 30;
- eResult = mDepthGenerator.SetMapOutputMode(mapMode);
- eResult = mImageGenerator.SetMapOutputMode(mapMode);
- //mDepthGenerator.GetAlternativeViewPointCap().SetViewPoint( mImageGenerator );
- // start generating
- eResult = mContext.StartGeneratingAll();
- // read data
- vector<SColorPoint3D> vPointCloud;
- while ( !xnOSWasKeyboardHit() )
- {
- eResult = mContext.WaitNoneUpdateAll();
- // get the depth map
- const XnDepthPixel* pDepthMap = mDepthGenerator.GetDepthMap();
- // get the image map
- const XnRGB24Pixel* pImageMap = mImageGenerator.GetRGB24ImageMap();
- // generate point cloud
- vPointCloud.clear();
- GeneratePointCloud(mDepthGenerator, pDepthMap, pImageMap, vPointCloud);
- // print point cloud运行结果只有这部分输出
- cout.flags(ios::left); //Left-aligned
- cout << "Point number: " << vPointCloud.size() << endl;
- for(i=0;i<vPointCloud.size();i++)
- {
- cout << setw(10) << i;
- cout << "X:" << setw(10) << vPointCloud[i].X;
- cout << "Y:" << setw(10) << vPointCloud[i].Y;
- cout << "Z:" << setw(10) << vPointCloud[i].Z;
- cout << "R:" << setw(10) << vPointCloud[i].R;
- cout << "G:" << setw(10) << vPointCloud[i].G;
- cout << "B:" << setw(10) << vPointCloud[i].B <<endl;
- }
- }
- // 将数据写入data.pcd并保存
- pcl::PointCloud<pcl::PointXYZRGB> cloud;
- cloud.width=640;
- cloud.height=480;
- cloud.is_dense=false;
- cloud.points.resize(cloud.width*cloud.height);
- for(size_t i=0;i<cloud.points.size();++i)
- {
- cloud.points[i].x=vPointCloud[i].X;
- cloud.points[i].y=vPointCloud[i].Y;
- cloud.points[i].z=vPointCloud[i].Z;
- cloud.points[i].r=vPointCloud[i].R;
- cloud.points[i].g=vPointCloud[i].G;
- cloud.points[i].b=vPointCloud[i].B;
- }
- pcl::io::savePCDFileASCII("data.pcd",cloud);
- std::cerr<<"Saved "<<cloud.points.size()<<" data points to data.pcd."<<std::endl;
- for(size_t i=0;i<cloud.points.size();++i)
- std::cerr<<" "<<cloud.points[i].x<<" "<<cloud.points[i].y<<" "<<cloud.points[i].z<<std::endl;
- //stop
- mContext.StopGeneratingAll();
- mContext.Shutdown();
- return(0);
- }
复制代码 代码运行结果只是输出XYZRGB的值,感觉代码并没有运行最后关于pcl的部分,没有生成data.pcd文件,更没有写入数据。我的目的就是利用openNI+PCL+Kinect得到一个记录三维点云坐标的pcd文件,我也试图调试过,但是因为水平太过菜鸟,始终找不到原因,恳请各位指点迷津,谢谢
|
|