Kinect运用OpenNI产生点云

#include "DepthmapToPointCloud.h" #include <stdio.h> #include <gl/glut.h> #include <math.h> #define TODEGREE 57.2957795 // radian->degree #define PI 3.1415926 /** * global variable */ xn::Context context; xn::DepthGenerator dethGenerator; DepthmapPointCloud cloud(context, dethGenerator); /** * OpenGL drawing */ int h_rotate_angle=0, v_rotate_angle=0; // h: horizontal v: vertical GLfloat cameraX=0.0f, cameraY=0.0f, cameraZ=0.0f; // 相机坐标位置 GLfloat h_rotate_radian,h_sin_value, h_cos_value; // 旋转角度、对应角度sin值与cos值 GLfloat v_rotate_radian,v_sin_value, v_cos_value; const GLfloat origin_to_center = 2.5f; // 坐标原点到gluLookAt函数中视线点center的距离 GLfloat zoom_step = origin_to_center; // 缩放变量 void OpenGL_init(void) { glClearColor(0.0f, 0.0f, 0.0f, 1.0f); // background color: white glShadeModel(GL_FLAT); glClear(GL_COLOR_BUFFER_BIT); } ////////////////////////////////////// void OpenGL_rending(void) { // GLshort r=0, g=0, b=0; glColor3f(1.0f, 1.0f, 1.0f); glPointSize(0.1f); glBegin(GL_POINTS); for (unsigned int i=0; i<cloud.getPointCloudNum(); ++i) { // if (cloud.getPointCloudData()[i].Z >=0.0f && cloud.getPointCloudData()[i].Z < 0.4f) { r=1.0f; g=1.0f; b=1.0f; } // 白色 // else if (cloud.getPointCloudData()[i].Z >= 0.4f && cloud.getPointCloudData()[i].Z < 0.8f) { r=0.0f; g=1.0f; b=1.0f; } // 青色 // else if (cloud.getPointCloudData()[i].Z >= 0.8f && cloud.getPointCloudData()[i].Z < 1.2f) { r=1.0f; g=0.0f; b=1.0f; } // 洋红 // else if (cloud.getPointCloudData()[i].Z >= 1.2f && cloud.getPointCloudData()[i].Z < 1.6f) { r=0.0f; g=0.75f; b=1.0f; } // 深天蓝 // else if (cloud.getPointCloudData()[i].Z >= 1.6f && cloud.getPointCloudData()[i].Z < 2.0f) { r=1.0f; g=0.27f; b=0.0f; } // 橙红 // else { r=0.0f; g=0.0f; b=0.0f; } // 黑色 // 深度图形式表示 // if (cloud.getPointCloudData()[i].Z >= 0.4) // r = g = b = (int)((cloud.getPointCloudData()[i].Z-0.4)*100)>>1; // else // r = g = b = 255; // glColor3b(r, g, b); glVertex3f(cloud.getPointCloudData()[i].X, cloud.getPointCloudData()[i].Y, cloud.getPointCloudData()[i].Z); } glEnd(); cloud.updataPointCloud(); // update next frame cloudpoint data glFlush(); glutSwapBuffers(); } ///////////////////////////////////// void OpenGL_Idel() { OpenGL_rending(); // 刷新显示 glClear(GL_COLOR_BUFFER_BIT); } ///////////////////////////////////// void OpenGL_changeSize(int w, int h) { glViewport(0, 0, GLsizei(w), GLsizei(h)); glMatrixMode(GL_PROJECTION); glLoadIdentity(); // if (w <= h) // 正交投影 // glOrtho(-2.0, 2.0, -2.0*(GLfloat)h/(GLfloat)w, 2.0*(GLfloat)h/(GLfloat)w, -10.0, 10.0); // else // glOrtho(-2.0*(GLfloat)w/(GLfloat)h, 2.0*(GLfloat)w/(GLfloat)h, -2.0, 2.0, -10.0, 10.0); gluPerspective(60.0, (GLfloat)w/(GLfloat)h, 0.1f, 50.0f); glMatrixMode(GL_MODELVIEW); glLoadIdentity(); gluLookAt(cameraX, cameraY, cameraZ, 0.0f, 0.0f, 0.1f, 0.0f, 1.0f, 0.0f); } //////////////////////////////////// void OpenGL_mouseMove(int button, int state, int x, int y) { static int old_x, old_y, old_zoom; switch(button) { case GLUT_LEFT_BUTTON: if (state == GLUT_DOWN){ old_x = x; old_y = y; } if (state == GLUT_UP){ if (x-old_x > 0) // 水平面上移动 h_rotate_angle += 10; if (x-old_x < 0) h_rotate_angle -= 10; if (y-old_y > 0) // 垂直面上移动 v_rotate_angle += 10; if (y-old_y < 0) v_rotate_angle -= 10; if (h_rotate_angle>=360 || h_rotate_angle <= -360) h_rotate_angle = 0; if (v_rotate_angle>=360 || v_rotate_angle <= -360) v_rotate_angle = 0; h_rotate_radian = h_rotate_angle*PI/360; // 角度换算到弧度 v_rotate_radian = v_rotate_angle*PI/360; h_sin_value = sin(h_rotate_radian); v_sin_value = sin(v_rotate_radian); // sin(θ)*origin_to_center h_cos_value = cos(h_rotate_radian); v_cos_value = cos(v_rotate_radian); // cos(θ)*origin_to_center cameraX = h_sin_value * v_cos_value * zoom_step; cameraY = v_sin_value * zoom_step; cameraZ = origin_to_center - h_cos_value * v_cos_value * zoom_step; } break; case GLUT_MIDDLE_BUTTON: if (state == GLUT_DOWN) {cameraX=cameraY=cameraZ=0.0f;h_rotate_angle=v_rotate_angle=0;zoom_step=origin_to_center;h_sin_value=v_sin_value=.0;h_cos_value=v_cos_value=1.0;} // 照相机恢复默认位置 break; case GLUT_RIGHT_BUTTON: if (state == GLUT_DOWN){ old_zoom = y; } if (state == GLUT_UP){ if (y < old_zoom){ // 拉近视角 if (zoom_step > 0.0) zoom_step -= 0.05; } else{ // 拉远视角 if (zoom_step <= origin_to_center) zoom_step += 0.05; } } cameraX = h_sin_value * v_cos_value * zoom_step; cameraY = v_sin_value * zoom_step; cameraZ = origin_to_center - h_cos_value * v_cos_value * zoom_step; break; default :break; } glLoadIdentity(); gluLookAt(cameraX, cameraY, cameraZ, 0.0f, 0.0f, origin_to_center, 0.0f, 1.0f, 0.0f); } int main(int argc, char **argv) { cloud.setOutputMode(cloud.getDefaultOutputMode()); cloud.init(); glutInit(&argc, argv); glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGBA); glutInitWindowSize(800, 600); glutInitWindowPosition(200, 100); glutCreateWindow("Kinect Point Cloud"); OpenGL_init(); glutDisplayFunc(OpenGL_rending); glutReshapeFunc(OpenGL_changeSize); glutMouseFunc(OpenGL_mouseMove); glutIdleFunc(OpenGL_Idel); glutMainLoop(); // while (1) // { // cloud.updataPointCloud(); // unsigned int index, shiftStep; // for (unsigned int j=0; j<cloud.getXYRes().nYRes; ++j) // { // shiftStep = j*cloud.getXYRes().nXRes; // for (unsigned int i=0; i<cloud.getXYRes().nXRes; ++i) // { // index =shiftStep + i; // printf("***********************************\n"); // printf("%-6.4f\t%-6.4f\t%-6.4f\n", cloud.getRawPointData()[index].X, // cloud.getRawPointData()[index].Y, cloud.getRawPointData()[index].Z); // printf("%-6.4f\t%-6.4f\t%-6.4f\n", cloud.getPointCloudData()[index].X, // cloud.getPointCloudData()[index].Y, cloud.getPointCloudData()[index].Z); // printf("***********************************\n"); // } // } // printf("***********************************\n"); // printf("%-6.4f\t%-6.4f\t%-6.4f\n", cloud.getPointCloudData()[0].X, // center:640*240+320-1 // cloud.getPointCloudData()[0].Y, cloud.getPointCloudData()[640*240+320-1].Z); // printf("***********************************\n"); // // print field of view // XnFieldOfView FOV; // for (unsigned int i=0; i<480;) // { // cloud.getFieldofView(FOV); // printf("[%5.3f] [%5.3f]\n", FOV.fHFOV*TODEGREE, FOV.fVFOV*TODEGREE); // i = i+5; // } // printf("***********************************\n"); // Sleep(1000); // } return 0; }