#include <OpenNI.h>包含错误,是版本问题吗?我用的1.6.0,配置的pcl all-in-one.网上找的程序不能跑,是什么原因?还需要其他配置吗?
#include <iostream>
// OpenCV Header
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
// OpenNI Header
#include <OpenNI.h>
// namespace
using namespace std;
using namespace openni;
using namespace cv;
typedef struct{
double x;
double y;
double z;
int r;
int g;
int b;
int TorF;
}color3D;
int getPicAndPointcloud(char *a,char*b,std::vector<color3D>&data)
{
int h=320,v=240;
data.resize(v*h);
FILE *fp=fopen(b,"w");
// 1. Initial OpenNI
if( OpenNI::initialize() != STATUS_OK )
{
std::cout << "OpenNI Initial Error: "
<< OpenNI::getExtendedError() << endl;
return -1;
}
// 2. Open Device
Device mDevice;
if( mDevice.open( ANY_DEVICE ) != STATUS_OK )
{
std::cout << "Can't Open Device: "
<< OpenNI::getExtendedError() << endl;
return -1;
}
// 3. Create depth stream
VideoStream mDepthStream;
if( mDevice.hasSensor( SENSOR_DEPTH ) )
{
if( mDepthStream.create( mDevice, SENSOR_DEPTH ) == STATUS_OK )
{
// 3a. set video mode
VideoMode mMode;
mMode.setResolution( h, v );
//mMode.setResolution( h, v );
mMode.setFps( 30 );
mMode.setPixelFormat( PIXEL_FORMAT_DEPTH_1_MM );
if( mDepthStream.setVideoMode( mMode) != STATUS_OK )
{
std::cout << "Can't apply VideoMode: "
<< OpenNI::getExtendedError() << endl;
}
}
else
{
std::cout << "Can't create depth stream on device: "
<< OpenNI::getExtendedError() << endl;
return -1;
}
}
else
{
std::cout << "ERROR: This device does not have depth sensor" << endl;
return -1;
}
// 4. Create color stream
VideoStream mColorStream;
if( mDevice.hasSensor( SENSOR_COLOR ) )
{
if( mColorStream.create( mDevice, SENSOR_COLOR ) == STATUS_OK )
{
// 4a. set video mode
VideoMode mMode;
mMode.setResolution( 640, 480 );
mMode.setFps( 30 );
mMode.setPixelFormat( PIXEL_FORMAT_RGB888 );
if( mColorStream.setVideoMode( mMode) != STATUS_OK )
{
std::cout << "Can't apply VideoMode: "
<< OpenNI::getExtendedError() << endl;
}
// 4b. image registration
if( mDevice.isImageRegistrationModeSupported(
IMAGE_REGISTRATION_DEPTH_TO_COLOR ) )
{
mDevice.setImageRegistrationMode( IMAGE_REGISTRATION_DEPTH_TO_COLOR );
}
}
else
{
std::cout << "Can't create color stream on device: "
<< OpenNI::getExtendedError() << endl;
return -1;
}
}
// 5. create OpenCV Window
cv::namedWindow( "Depth Image", CV_WINDOW_AUTOSIZE );
cv::namedWindow( "Color Image", CV_WINDOW_AUTOSIZE );
// 6. start
VideoFrameRef mColorFrame;
VideoFrameRef mDepthFrame;
mDepthStream.start();
mColorStream.start();
int iMaxDepth = mDepthStream.getMaxPixelValue();
// 7. check is color stream is available
cv::Mat cImageBGR;
cv::Mat mat(Size(h,v),CV_8UC3);
cv::Mat matdis(Size(h,v),CV_8UC3);
Vec3b color_dist;
if( mColorStream.isValid() )
{
// 7a. get color frame
if( mColorStream.readFrame( &mColorFrame ) == STATUS_OK )
{
// 7b. convert data to OpenCV format
const cv::Mat mImageRGB(
mColorFrame.getHeight(), mColorFrame.getWidth(),
CV_8UC3, (void*)mColorFrame.getData() );
// 7c. convert form RGB to BGR
//mat= mImageRGB;
cv::cvtColor( mImageRGB, cImageBGR, CV_RGB2BGR );
// 7d. show image
IplImage imgIpl1 = (IplImage)(mImageRGB);
cvShowImage("Color Image",&imgIpl1);
//cv::imshow( "Color Image", mImageRGB );
}
}
// 8a. get depth frame
if( mDepthStream.readFrame( &mDepthFrame ) == STATUS_OK )
{
// 8b. convert data to OpenCV format
const cv::Mat mImageDepth(
mDepthFrame.getHeight(), mDepthFrame.getWidth(),
CV_16UC1, (void*)mDepthFrame.getData() );
// 8c. re-map depth data [0,Max] to [0,255]
cv::Mat mScaledDepth;
// 8d. show image
//cv::imshow( "Depth Image", mScaledDepth );
IplImage imgIpl2 = (IplImage)(mScaledDepth);
cvShowImage("Color Image",&imgIpl2);
}
//生成点云
VideoFrameRef vfDepthFrame;
const DepthPixel* pDepthArray = NULL;
if( mDepthStream.readFrame( &vfDepthFrame ) == STATUS_OK )
{
pDepthArray = (const DepthPixel*)vfDepthFrame.getData();
for( int y = 0; y < vfDepthFrame.getHeight() - 1; ++ y )
{
for( int x = 0; x < vfDepthFrame.getWidth() - 1; ++ x )
{
int icolor;int jcolor;
int idx = x + y * vfDepthFrame.getWidth();
const DepthPixel& rDepth = pDepthArray[idx];
color_dist.val[0]=rDepth%256;
color_dist.val[1]=rDepth/256;
color_dist.val[2]=0;/*rand()/256;*/
matdis.at<Vec3b>(y,x)=color_dist;
float fX, fY, fZ;
CoordinateConverter::convertDepthToColor(mDepthStream,mColorStream,
x, y, rDepth,&icolor,&jcolor );
CoordinateConverter::convertDepthToWorld( mDepthStream,
x, y, rDepth,
&fX, &fY, &fZ );
cv::Vec3b color;
if(icolor>0&&icolor<640&&jcolor>0&&jcolor<480)
{
color=cImageBGR.at<cv::Vec3b>(jcolor,icolor);
mat.at<Vec3b>(y,x)=color;
int r,g,b;
r=color.val[0];
g=color.val[1];
b=color.val[2];
fprintf(fp,"%lf %lf %lf %d %d %d\n",fX,fY,fZ,r,g,b);
//
data[x*v+y].x=fX;
data[x*v+y].y=fY;
data[x*v+y].z=fZ;
data[x*v+y].r=r;
data[x*v+y].g=g;
data[x*v+y].b=b;
data[x*v+y].TorF=1;
}
}
}
}
fclose(fp);
imwrite(a,mat);
imwrite("dist.png",matdis);
// 9. stop
mDepthStream.destroy();
mColorStream.destroy();
mDevice.close();
OpenNI::shutdown();
return 1;
}
int main()
{
char pcName1[500]="";//点云保存的文件名
char colorPic0[500]="";//保存彩色图片的名称
vector<color3D>data;
data.resize(307200);
getPicAndPointcloud(colorPic0,pcName1,data);//调用kinect获取点云和影像
}
|