关于OpenNI2和OpenCV2的那些事——获取三维点云数据并用OpenGL表示

上一节呢，我们利用openni2获得了彩色图像和深度图像，这一节我们用openni2的转换函数将深度数据转换为三维点云，然后用彩色数据作为纹理将点云用opengl画出来。

首先介绍CoordinateConverter::convertDepthToWorld(const VideoStream& depthStream, int depthX, int depthY, DepthPixel depthZ, float* pWorldX, float* pWorldY, float* pWorldZ)函数：depthStream表示深度数据流，depthX，depthY表示深度图像某一行，某一列，depthZ表示该行该列的值（即深度值）。pWorldX, pWorldY, pWorldZ是转换好的世界坐标系下的三维坐标。

建立三维数组xyzdata[480][640][3]存储点云数据：

pdepth = (DepthPixel*)frameDepth.getData();
for (int i = 0; i < frameDepth.getHeight(); i++)
{
	for (int j = 0; j < frameDepth.getWidth(); j++)
	{
		depthv = pdepth[i*frameDepth.getWidth() + j];
		CoordinateConverter::convertDepthToWorld(streamDepth, i, j, depthv, &x, &y, &z);
		xyzdata[i][j][0] = x ;
		xyzdata[i][j][1] = y ;
		xyzdata[i][j][2] = z ;
	}
}

接下来利用彩色数据获得纹理，存储在三维数组texture[480][640][3]中（注意opengl中着色是RGB，根据opencv中图像的不同作相应转换）：

for (int i = 0; i < cImageBGR.rows; i++)
{
	Vec3b *p = cImageBGR.ptr(i);
	for (int j = 0; j < cImageBGR.cols; j++)
	{
		texture[i][j][0] = p[j][2];  //red
		texture[i][j][1] = p[j][1];  //green
		texture[i][j][2] = p[j][0];  //blue
	}
}

最后在opengl中画出来即可（注意绕Z轴逆时针旋转90°）：

void display(void)
{
	// clear screen and depth buffer  
	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
	// Reset the coordinate system before modifying   
	glLoadIdentity();
	// set the camera position  
	gluLookAt(0.0, 0.0, -0.1, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0);
 	glRotatef(-90.0, 0.0, 0.0, 1.0); //绕Z轴逆时针旋转90°

	float x,y,z;
	// 绘制图像点云
	glBegin(GL_POINTS);
	for (int i=0;i<480;i++){
		for (int j=0;j<640;j++){
			// color interpolation
			glColor3f(texture[i][j][0]/255, texture[i][j][1]/255, texture[i][j][2]/255);
			x= xyzdata[i][j][0];
			y= xyzdata[i][j][1];
			z= xyzdata[i][j][2];
			glVertex3f(x,y,z);
		}
	}
	glEnd();
	glutSwapBuffers();
}

效果如下：

为了显示的更加清楚，我们加入了鼠标和键盘控制旋转和缩放：

void mouse(int button, int state, int x, int y)
{
	if (state == GLUT_DOWN)
	{
		mousedown = GL_TRUE;
	}
	mousex = x, mousey = y;
}

void motion(int x, int y)
{
	if (mousedown == GL_TRUE)
	{       /// 所除以的数字是调整旋转速度的，随便设置，达到自己想要速度即可
		xrotate -= (x - mousex) / 10.0f;
		yrotate -= (y - mousey) / 10.0f;
	}
	mousex = x, mousey = y;
	glutPostRedisplay();
}

void keyboard(unsigned char c, int x, int y)
{
	switch (c)
	{
	case 'w':
		eye[2] += 20.0f;
		break;
	case 's':
		eye[2] -= 20.0f;
		break;
	case 'a':
		eye[0] += 20.0f;
		break;
	case 'd':
		eye[0] -= 20.0f;
		break;
	case 'r':
		eye[0] = 0.0f;
		eye[2] = 0.0f;
		xrotate = 0;
		yrotate = 0;
		break;
	case 27:
		exit(0);
	default:
		break;
	}
	glutPostRedisplay();
}

改良后的显示函数：

void display(void)
{
	// clear screen and depth buffer  
	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
	// Reset the coordinate system before modifying   
	glLoadIdentity();
	// set the camera position  
//	gluLookAt(0.0, 0.0, -0.1, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0);
	gluLookAt(eye[0], eye[1], eye[2],
		center[0], center[1], center[2],
		0, 1, 0);
	glPushMatrix();
  	glRotatef(-90.0, 0.0, 0.0, 1.0);
	glTranslatef(-350.0, -300.0, 5000.0);
	glRotatef(xrotate, 1.0, 0.0, 0.0);
	glRotatef(yrotate, 0.0, 1.0, 0.0);
	glTranslatef(350.0, 300.0, -5000.0);

	float x,y,z;
	// 绘制图像点云
	glBegin(GL_POINTS);
	for (int i=0;i<480;i++){
		for (int j=0;j<640;j++){
			// color interpolation
			glColor3f(texture[i][j][0]/255, texture[i][j][1]/255, texture[i][j][2]/255);
			x= xyzdata[i][j][0];
			y= xyzdata[i][j][1];
			z= xyzdata[i][j][2];
			glVertex3f(x,y,z);
		}
	}
	glEnd();

	glPopMatrix();
	glutSwapBuffers();
}

控制鼠标和键盘效果如下：

缩放

旋转1

旋转2

旋转3

详细代码与工程点此下载

这一节就到这，自认为效果还不错，欢迎大家讨论。接下来开始研究实时三维重建的相关论文，立体匹配，滤波等等，可能要一段时间之后才能写出相关博客。