Kinect实现简单的三维重建

Kinect想必大家已经很熟悉了，最近基于Kinect的创意应用更是呈井喷状态啊！看到很多国外大牛用Kinect做三维重建，其中最著名的要数来自微软研究院的Kinect Fusion了，可以看看下面这个视频http://v.ku6.com/show/7q2Sa__pa4-rWcAVtB3Xuw...html，或者http://v.youku.com/v_show/id_XNDcxOTg3MzUy.html。

可惜Kinect Fusion是不开源的，不过PCL实现了一个差不多的开源版本，http://www.pointclouds.org/。有兴趣同时电脑配置高的朋友可以研究一下。

最近比较闲，有一点手痒，想自己做一个三维重建，不过肯定不会像Kinect Fusion那么强大，只是自己练练手、玩玩而已。代码在最后有下载。

1. 获取Kinect深度图：

首先我使用微软官方的Kinect SDK来控制Kinect，三维绘图我选用了OpenFrameworks。OpenFrameworks（以后简称OF）是一个开源的公共基础库，将很多常用的库统一到了一起，比如OpenGL，OpenCV，Boost等等，而且有大量的第三方扩展库，使用非常方便。具体可见http://www.openframeworks.cc/。

在一切开始之前，我们需要对OpenGL和三维场景做一些设置：

1. void testApp::setup(){
2. //Do some environment settings.
3. ofSetVerticalSync(true);
4. ofSetWindowShape(640,480);
5. ofBackground(0,0,0);
6. //Turn on depth test for OpenGL.
7. glEnable(GL_DEPTH_TEST);
8. glDepthFunc(GL_LEQUAL);
9. glShadeModel(GL_SMOOTH);
10. //Put a camera in the scene.
11. m_camera.setDistance(3);
12. m_camera.setNearClip(0.1f);
13. //Turn on the light.
14. m_light.enable();
15. //Allocate memory to store point cloud and normals.
16. m_cloud_map.Resize(DEPTH_IMAGE_WIDTH,DEPTH_IMAGE_HEIGHT);
17. m_normal_map.Resize(DEPTH_IMAGE_WIDTH,DEPTH_IMAGE_HEIGHT);
18. //Initialize Kinect.
19. InitNui();
20. }

OF是使用OpenGL进行绘图的，所以可以直接使用OpenGL中的函数（以gl开头），为了方便，OF还自己封装了一些常用函数（以of开头）。在上面代码的最后有一个InitNui()函数，在那里面我们会对Kinect进行初始化：

1. void testApp::InitNui()
2. {
3. m_init_succeeded = false;
4. m_nui = NULL;
5. int count = 0;
6. HRESULT hr;
7. hr = NuiGetSensorCount(&count);
8. if (count <= 0)
9. {
10. cout<<"No kinect sensor was found!!"<<endl;
11. goto Final;
12. }
13. hr = NuiCreateSensorByIndex(0,&m_nui);
14. if (FAILED(hr))
15. {
16. cout<<"Create Kinect Device Failed!!"<<endl;
17. goto Final;
18. }
19. //We only just need depth data.
20. hr = m_nui->NuiInitialize(NUI_INITIALIZE_FLAG_USES_DEPTH);
21. if (FAILED(hr))
22. {
23. cout<<"Initialize Kinect Failed!!"<<endl;
24. goto Final;
25. }
26. //Resolution of 320x240 is good enough to reconstruct a 3D model.
27. hr = m_nui->NuiImageStreamOpen(NUI_IMAGE_TYPE_DEPTH,NUI_IMAGE_RESOLUTION_320x240,0,2,NULL,&m_depth_stream);
28. if (FAILED(hr))
29. {
30. cout<<"Open Streams Failed!!"<<endl;
31. goto Final;
32. }
33. m_init_succeeded = true;
34. Final:
35. if (FAILED(hr))
36. {
37. if (m_nui != NULL)
38. {
39. m_nui->NuiShutdown();
40. m_nui->Release();
41. m_nui = NULL;
42. }
43. }
44. }

接下来我们需要将每一帧的深度信息保存到我们自己的buffer中，专门写一个函数来做这件事情：

1. bool testApp::UpdateDepthFrame()
2. {
3. if (!m_init_succeeded)return false;
4. HRESULT hr;
5. NUI_IMAGE_FRAME image_frame = {0};
6. NUI_LOCKED_RECT locked_rect = {0};
7. hr = m_nui->NuiImageStreamGetNextFrame(m_depth_stream,0,&image_frame);
8. //If there's no new frame, we will return immediately.
9. if (SUCCEEDED(hr))
10. {
11. hr = image_frame.pFrameTexture->LockRect(0,&locked_rect,NULL,0);
12. if (SUCCEEDED(hr))
13. {
14. //Copy depth data to our own buffer.
15. memcpy(m_depth_buffer,locked_rect.pBits,locked_rect.size);
16. image_frame.pFrameTexture->UnlockRect(0);
17. }
18. //Release frame.
19. m_nui->NuiImageStreamReleaseFrame(m_depth_stream,&image_frame);
20. }
21. if (SUCCEEDED(hr))return true;
22. return false;
23. }

通过上面几步，我们已经可以拿到一幅深度图了。在OF中，每一帧更新时，update()函数都会被调用，我们可以把所有需要适时更新的代码都写在里面：

1. void testApp::update(){
2. //Get a new depth frame from Kinect.
3. m_new_depth = UpdateDepthFrame();
4. if (m_new_depth)
5. {
6. Mat depth_frame = Mat(DEPTH_IMAGE_HEIGHT,DEPTH_IMAGE_WIDTH,CV_16UC1,m_depth_buffer);
7. <span style="white-space:pre">       </span>imshow("Depth Frame", depth_frame);
8. }
9. }

现在编译并运行程序，我们可以看到通过OpenCV画出来的深度图：

但你会发现，这样的深度图具有很多小孔和噪点，边缘也不平滑。因此要对图像进行滤波，为了使边缘不被模糊掉，这里最好使用中值滤波。修改一下上面的update()函数，使用5x5的窗口进行中值滤波：

1. void testApp::update(){
2. //Get a new depth frame from Kinect.
3. m_new_depth = UpdateDepthFrame();
4. if (m_new_depth)
5. {
6. Mat smoothed_depth = Mat(DEPTH_IMAGE_HEIGHT,DEPTH_IMAGE_WIDTH,CV_16UC1,m_depth_buffer);
7. medianBlur(smoothed_depth,smoothed_depth,5);
8. imshow("Depth Frame", smoothed_depth);
9. }
10. }

再次运行程序，得到的深度图就变成下面这样了，感觉好了很多！！

2. 通过深度图得到点云：

为了得到点云，我专门写了一个类来完成这一操作。这个类不仅会根据深度图计算点云，还会将得到的点云以矩阵的形式存放起来，矩阵中每一个元素代表一个点，同时对应深度图中具有相同行列坐标的像素。而计算点云的方法，Kinect SDK自身有提供，即NuiTransformDepthImageToSkeleton()函数，具体用法可看官方文档。
下面是这个类中生成点云的代码：

1. void PointCloudMap::Create(Mat& depth_image,USHORT max_depth,float scale)
2. {
3. USHORT* depth_line = (USHORT*)depth_image.data;
4. UINT stride = depth_image.step1();
5. //m_points is the place where we store the whole point cloud.
6. ofVec3f* points_line = m_points;
7. Vector4 vec;
8. for (DWORD y = 0; y < m_height; y++)
9. {
10. for (DWORD x = 0; x < m_width; x++)
11. {
12. ofVec3f point(0);
13. USHORT real_depth = (depth_line[x] >> 3);
14. if (real_depth >= 800 && real_depth < max_depth)
15. {
16. //For each pixel in the depth image, we calculate its space coordinates.
17. vec = NuiTransformDepthImageToSkeleton(
18. x,
19. y,
20. depth_line[x]
21. );
22. //Save the point with a scale.
23. point.x = vec.x*scale;
24. point.y = vec.y*scale;
25. point.z = -vec.z*scale;
26. }
27. points_line[x] = point;
28. }
29. depth_line += stride;
30. points_line += m_width;
31. }
32. }

拿到点云后，我们可以考虑对点云进行三角化了。一提到三角化，很多人脑海中的第一印象是复杂、计算量大等等，我个人也是这样。但是，Kinect返回的点云是结构化的，并不是无序点云，也就是说每一个点在空间中与其他点的相互关系我们是知道的，因此可以用一些简单的方法来实现三角化，虽然这样的三角化结果不是最优的，但是简单快速，60fps毫无压力。
首先，我们的点云是存放在一个矩阵中的，而且矩阵的大小与深度图完全一样（行x列），因此我们将点云视为一幅图，每一个像素存放的是点的空间坐标。我们可以像遍历一般图像的像素一样遍历点云图，从而得到空间中某一点的所有相邻点。然后，我们使用OpenGL的连线功能，每画一个点就与它之前的两个点连成一个三角面。
如下图，点旁边的序号是画点的顺序：

这样我们就可以一行一行的将点云三角化，但注意当一行结束时，要让OpenGL停止连线，否则这一行最后的点会和下一行第一个点连在一起。
以上过程我直接写在了主程序的draw方法中，OF在每一帧调用完update方法后，就会调用draw方法：

1. void testApp::draw(){
2. if (!m_init_succeeded)return;
3. m_camera.begin();
4. ofVec3f* points_line = m_cloud_map.m_points;
5. ofVec3f* points_next_line = m_cloud_map.m_points + DEPTH_IMAGE_WIDTH;
6. bool mesh_break = true;
7. for (int y = 0; y < m_cloud_map.m_height - 1; y++)
8. {
9. for (int x = 0; x < m_cloud_map.m_width; x++)
10. {
11. ofVec3f& space_point1 = points_line[x];
12. ofVec3f& space_point2 = points_next_line[x];
13. if (abs(space_point1.z) <= FLT_EPSILON*POINT_CLOUD_SCALE ||
14. abs(space_point2.z) <= FLT_EPSILON*POINT_CLOUD_SCALE)
15. {
16. if (!mesh_break)
17. {
18. //If there's no point here, the mesh should break.
19. mesh_break = true;
20. glEnd();
21. }
22. continue;
23. }
24. if (mesh_break)
25. {
26. //Start connecting points to form mesh.
27. glBegin(GL_TRIANGLE_STRIP);
28. mesh_break = false;
29. }
30. //Draw the point and set its normal.
31. glColor3f(0.7,0.7,0.7);
32. glVertex3f(space_point1.x,space_point1.y,space_point1.z);
33. //Draw the point below the prior one to form a triangle.
34. glColor3f(0.7,0.7,0.7);
35. glVertex3f(space_point2.x,space_point2.y,space_point2.z);
36. }
37. if (!mesh_break)
38. {
39. //At the end of the line, we break the mesh.
40. glEnd();
41. mesh_break = true;
42. }
43. points_line += DEPTH_IMAGE_WIDTH;
44. points_next_line += DEPTH_IMAGE_WIDTH;
45. }
46. m_camera.end();
47. //Draw frame rate for fun!
48. ofSetColor(255);
49. ofDrawBitmapString(ofToString(ofGetFrameRate()),10,20);
50. }

再次编译并运行程序，在OF的窗口中，我们会看到如下结果：

怎么看起来是一张平面图，一点3D感觉都没有，呵呵~~因为我们还没有给顶点设置法向。OpenGL会根据顶点法线来计算该点的光照，如果没有法线，光照是失效的，也就是我们看到的白茫茫一片。

3. 计算顶点法向
法线的计算可以非常简单，比如对每一个点，取和它相邻的两个点组成三角形，计算这个三角形的法向，即作为该点的法向。但这种方法太不精确了，而且其中一个点的坐标稍有变化，就会影响最终法线的方向，光照效果会很不稳定。
我打算考虑一个点周围所有的点，并使用最小二乘法来拟合一个最佳平面，这个平面的法向即为该点的法向。

我们希望该点包括周围的领点到这个平面的距离之平方和最小，即使下式最小：

其中a，b，c是决定这个平面的参数，也就是这个平面的法矢量（a，b，c）。x，y，z是点的坐标。为了求出适合的abc值，分别对这三个变量求偏导：

要求最小值，就要使下面三式成立：

这样我们就得到一个关于a，b，c的三元一次线性方程组，表示为矩阵形式即如下：

根据Cramer法则，这个方程组的解可以表示为：

其中：

，即系数矩阵的行列式

计算这些行列式的值后，就可解出a，b，c。

但是这里要注意，使用Cramer法则时，D不能为零，也就是说我们所期望的平面不能过原点。而过原点这种事情是很可能发生的，这时我们怎么办呢？

当平面过原点时，上面的三元一次方程组可简化为一个齐次方程组：

若上面系数矩阵的每一行所构成的向量共面但不共线，则a，b，c是有唯一解的，而其他情况下，只有零阶或无穷多个解。后者在实际应用中一般是不会出现的。因此我们只考虑前一种情况。这种情况的解，就是三个行向量所在面的法线。因此我们将这三个行向量两两作叉积（外积），得到三个垂直于该面的法线，取模最大的一个作为我们的解。

现在考虑什么点可以作为所求点的领点，由于点云是一幅图，我们可以借鉴二维图像滤波器的思想，将所求点周围的8领域点作为领点。（图画得丑，还请谅解）：

但是我们的点是有深度的，所以还需对以上领域点判断一下深度，只有某一点的深度与中心点的深度接近时，才能真正当做领点。

现在还有最后一个问题，通过上面的方法算出来的法线方向是不定的（有可能是你想要的法向的反方向），因此我们还需要一个方法让所有法线的朝向一致，我这里就简单的选择了朝向摄像机。

将上面的所有方法写在了一个类中，这个类根据点云图计算法线，并像点云图一样将所有法线保存为一副法线图。下面是计算法线和调整朝向的代码：

1. void NormalsMap::Create(PointCloudMap& point_cloud, float max_distance)//创建一副法线图
2. {
3. if (point_cloud.m_height != m_height ||
4. point_cloud.m_width != m_width)
5. throw exception("NormalsMap has different size width the PointCloudMap");
6. ofVec3f* points_line0 = point_cloud.m_points;
7. ofVec3f* points_line1 = points_line0 + m_width;
8. ofVec3f* points_line2 = points_line1 + m_width;
9. ofVec3f* norms_line = m_normals + m_width;
10. vector<ofVec3f> neighbors;
11. int y_line0 = 0;
12. int y_line1 = y_line0 + m_width;
13. int y_line2 = y_line1 + m_width;
14. for (int y = 1; y < m_height - 1; y++)
15. {
16. for (int x = 1; x < m_width - 1; x++)
17. {
18. neighbors.clear();
19. norms_line[x] = ofVec3f(0);
20. if (points_line1[x].z == 0)continue;
21. neighbors.push_back(points_line1[x]);
22. //Add all neighbor points to the vector.
23. if (IsNeighbor(points_line0[x-1],points_line1[x],max_distance))
24. {
25. neighbors.push_back(points_line0[x-1]);
26. }
27. if (IsNeighbor(points_line0[x],points_line1[x],max_distance))
28. {
29. neighbors.push_back(points_line0[x]);
30. }
31. if (IsNeighbor(points_line0[x+1],points_line1[x],max_distance))
32. {
33. neighbors.push_back(points_line0[x+1]);
34. }
35. if (IsNeighbor(points_line1[x-1],points_line1[x],max_distance))
36. {
37. neighbors.push_back(points_line1[x-1]);
38. }
39. if (IsNeighbor(points_line1[x+1],points_line1[x],max_distance))
40. {
41. neighbors.push_back(points_line1[x+1]);
42. }
43. if (IsNeighbor(points_line2[x-1],points_line1[x],max_distance))
44. {
45. neighbors.push_back(points_line2[x-1]);
46. }
47. if (IsNeighbor(points_line2[x],points_line1[x],max_distance))
48. {
49. neighbors.push_back(points_line2[x]);
50. }
51. if (IsNeighbor(points_line2[x+1],points_line1[x],max_distance))
52. {
53. neighbors.push_back(points_line2[x+1]);
54. }
55. if (neighbors.size() < 3)continue;//Too small to identify a plane.
56. norms_line[x] = EstimateNormal(neighbors);
57. }
58. points_line0 += m_width;
59. points_line1 += m_width;
60. points_line2 += m_width;
61. norms_line += m_width;
62. y_line0 += m_width;
63. y_line1 += m_width;
64. y_line2 += m_width;
65. }
66. }
67. inline bool NormalsMap::IsNeighbor(ofVec3f& dst, ofVec3f& ori, float max_distance)//判断是否是领点
68. {
69. if (abs(dst.z - ori.z) < max_distance)
70. return true;
71. return false;
72. }
73. ofVec3f NormalsMap::EstimateNormal(vector<ofVec3f>& points)//使用最小二乘法计算法线
74. {
75. ofVec3f normal(0);
76. float x = 0, y = 0, z = 0;
77. float x2 = 0, y2 = 0, z2 = 0;
78. float xy = 0, xz = 0, yz = 0;
79. for (int i = 0; i < points.size(); i++)
80. {
81. float cx = points[i].x;
82. float cy = points[i].y;
83. float cz = points[i].z;
84. x += cx; y += cy; z += cz;
85. x2 += cx*cx; y2 += cy*cy; z2 += cz*cz;
86. xy += cx*cy; xz += cx*cz; yz += cy*cz;
87. }
88. float D = x2*y2*z2 + 2*xy*xz*yz - x2*yz*yz - y2*xz*xz - z2*xy*xy;
89. if (abs(D) >= FLT_EPSILON)
90. {
91. //Use least squares technique to get the best normal.
92. float Da = x*(yz*yz - y2*z2) - y*(yz*xz - z2*xy) + z*(y2*xz - xy*yz);
93. float Db = x2*(z*yz - y*z2) - xy*(z*xz - x*z2) + xz*(y*xz - x*yz);
94. float Dc = x2*(y*yz - z*y2) - xy*(x*yz - z*xy) + xz*(x*y2 - y*xy);
95. normal.x = Da/D;
96. normal.y = Db/D;
97. normal.z = Dc/D;
98. normal.normalize();
99. }
100. else
101. {
102. /*D == 0, it means some axes(x,y or z) are on the normal plane.
103. We need another way to calculate normal vector.*/
104. ofVec3f row0(x2,xy,xz);
105. ofVec3f row1(xy,y2,yz);
106. ofVec3f row2(xz,yz,z2);
107. ofVec3f vec1 = row0.getCrossed(row1);
108. ofVec3f vec2 = row0.getCrossed(row2);
109. ofVec3f vec3 = row1.getCrossed(row2);
110. float len1 = vec1.lengthSquared();
111. float len2 = vec2.lengthSquared();
112. float len3 = vec3.lengthSquared();
113. if (len1 >= len2 && len1 >= len3)
114. normal = vec1 / sqrt(len1);
115. else if (len2 >= len1 && len2 >= len3)
116. normal = vec2 / sqrt(len2);
117. else
118. normal = vec3 / sqrt(len3);
119. }
120. return normal;
121. }
122. void NormalsMap::FlipNormalsToVector(ofVec3f main_vector)//调整法线朝向，是其全部指向main_vector方向
123. {
124. ofVec3f* normal = m_normals;
125. for (int i = 0; i < m_width*m_height; i++)
126. {
127. if ((*normal).dot(main_vector) < 0)
128. (*normal) *= -1;
129. normal++;
130. }
131. }

4. 全部放在一起：

将以上全部放在一起，并修改一下我们的draw函数，以使其设置顶点的法向：

1. void testApp::draw(){
2. if (!m_init_succeeded)return;
3. m_camera.begin();
4. ofVec3f* points_line = m_cloud_map.m_points;
5. ofVec3f* points_next_line = m_cloud_map.m_points + DEPTH_IMAGE_WIDTH;
6. ofVec3f* normals_line = m_normal_map.m_normals;
7. bool mesh_break = true;
8. for (int y = 0; y < m_cloud_map.m_height - 1; y++)
9. {
10. for (int x = 0; x < m_cloud_map.m_width; x++)
11. {
12. ofVec3f& space_point1 = points_line[x];
13. ofVec3f& space_point2 = points_next_line[x];
14. if (abs(space_point1.z) <= FLT_EPSILON*POINT_CLOUD_SCALE ||
15. abs(space_point2.z) <= FLT_EPSILON*POINT_CLOUD_SCALE)
16. {
17. if (!mesh_break)
18. {
19. //If there's no point here, the mesh should break.
20. mesh_break = true;
21. glEnd();
22. }
23. continue;
24. }
25. if (mesh_break)
26. {
27. //Start connecting points to form mesh.
28. glBegin(GL_TRIANGLE_STRIP);
29. mesh_break = false;
30. }
31. //Draw the point and set its normal.
32. glColor3f(0.8,0.8,0.8);
33. glNormal3f(normals_line[x].x,normals_line[x].y,normals_line[x].z);
34. glVertex3f(space_point1.x,space_point1.y,space_point1.z);
35. //Draw the point below the prior one to form a triangle.
36. glColor3f(0.8,0.8,0.8);
37. glVertex3f(space_point2.x,space_point2.y,space_point2.z);
38. }
39. if (!mesh_break)
40. {
41. //We break the mesh at the end of the line,.
42. glEnd();
43. mesh_break = true;
44. }
45. points_line += DEPTH_IMAGE_WIDTH;
46. points_next_line += DEPTH_IMAGE_WIDTH;
47. normals_line += DEPTH_IMAGE_WIDTH;
48. }
49. m_camera.end();
50. //Draw frame rate for fun!
51. ofSetColor(255);
52. ofDrawBitmapString(ofToString(ofGetFrameRate()),10,20);
53. }

最后编译运行，我们的目标就达到了！！！！

作为一个自娱自乐的小程序，感觉还不错吧！！！注意看左上角的帧率，60fps妥妥的。

小结：

做这个完全是为了学习和兴趣，不要说我是重复造轮子啊。写这个程序复习了很多线性代数的知识，温故而知新，感觉还是很有收获的。最后的效果还可以改进，最大的改进点就是三角化的方法，以后发现快速且效果好的三角化方法再和大家分享。

最后给出代码的下载地址 点击打开链接

代码在Windows7 ultimate，OpenCV 2.4.3，OpenFrameworks 0073，Kinect SDK 1.7 下编译通过。

编译有问题的可以看看下面的评论。