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

https://blog.csdn.net/aptx704610875/article/details/49798543

上一节呢，我们利用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<Vec3b>(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

详细代码与工程点此下载

这一节就到这，自认为效果还不错，欢迎大家讨论。接下来开始研究实时三维重建的相关论文，立体匹配，滤波等等，可能要一段时间之后才能写出相关博客。
--------------------- 
作者：cc_sunny 
来源：CSDN 
原文：https://blog.csdn.net/aptx704610875/article/details/49798543 
版权声明：本文为博主原创文章，转载请附上博文链接！