轉自：http://m.blog.csdn.net/blog/aichipmunk_11109/8721290
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和三維場景做一些設置：
void testApp::setup(){
//Do some environment settings.
ofSetVerticalSync(true);
ofSetWindowShape(640,480);
ofBackground(0,0,0);
//Turn on depth test for OpenGL.
glEnable(GL_DEPTH_TEST);
glDepthFunc(GL_LEQUAL);
glShadeModel(GL_SMOOTH);
//Put a camera in the scene.
m_camera.setDistance(3);
m_camera.setNearClip(0.1f);
//Turn on the light.
m_light.enable();
//Allocate memory to store point cloud and normals.
m_cloud_map.Resize(DEPTH_IMAGE_WIDTH,DEPTH_IMAGE_HEIGHT);
m_normal_map.Resize(DEPTH_IMAGE_WIDTH,DEPTH_IMAGE_HEIGHT);
//Initialize Kinect.
InitNui();
}
OF是使用OpenGL進行繪圖的，所以可以直接使用OpenGL中的函數（以gl開頭），爲了方便，OF還自己封裝了一些常用函數（以of開頭）。在上面代碼的最後有一個InitNui()函數，在那裏面我們會對Kinect進行初始化：
void testApp::InitNui()
{
m_init_succeeded = false;
m_nui = NULL;
int count = 0;
HRESULT hr;
hr = NuiGetSensorCount(&count);
if (count <= 0)
{
cout<<"No kinect sensor was found!!"<<endl;
goto Final;
}
hr = NuiCreateSensorByIndex(0,&m_nui);
if (FAILED(hr))
{
cout<<"Create Kinect Device Failed!!"<<endl;
goto Final;
}
//We only just need depth data.
hr = m_nui->NuiInitialize(NUI_INITIALIZE_FLAG_USES_DEPTH);
if (FAILED(hr))
{
cout<<"Initialize Kinect Failed!!"<<endl;
goto Final;
}
//Resolution of 320x240 is good enough to reconstruct a 3D model.
hr = m_nui->NuiImageStreamOpen(NUI_IMAGE_TYPE_DEPTH,NUI_IMAGE_RESOLUTION_320x240,0,2,NULL,&m_depth_stream);
if (FAILED(hr))
{
cout<<"Open Streams Failed!!"<<endl;
goto Final;
}
m_init_succeeded = true;
Final:
if (FAILED(hr))
{
if (m_nui != NULL)
{
m_nui->NuiShutdown();
m_nui->Release();
m_nui = NULL;
}
}
}
接下來我們需要將每一幀的深度信息保存到我們自己的buffer中，專門寫一個函數來做這件事情：
bool testApp::UpdateDepthFrame()
{
if (!m_init_succeeded)return false;
HRESULT hr;
NUI_IMAGE_FRAME image_frame = {0};
NUI_LOCKED_RECT locked_rect = {0};
hr = m_nui->NuiImageStreamGetNextFrame(m_depth_stream,0,&image_frame);
//If there's no new frame, we will return immediately.
if (SUCCEEDED(hr))
{
hr = image_frame.pFrameTexture->LockRect(0,&locked_rect,NULL,0);
if (SUCCEEDED(hr))
{
//Copy depth data to our own buffer.
memcpy(m_depth_buffer,locked_rect.pBits,locked_rect.size);
image_frame.pFrameTexture->UnlockRect(0);
}
//Release frame.
m_nui->NuiImageStreamReleaseFrame(m_depth_stream,&image_frame);
}
if (SUCCEEDED(hr))return true;
return false;
}
通過上面幾步，我們已經可以拿到一幅深度圖了。在OF中，每一幀更新時，update()函數都會被調用，我們可以把所有需要適時更新的代碼都寫在裏面：
void testApp::update(){
//Get a new depth frame from Kinect.
m_new_depth = UpdateDepthFrame();
if (m_new_depth)
{
Mat depth_frame = Mat(DEPTH_IMAGE_HEIGHT,DEPTH_IMAGE_WIDTH,CV_16UC1,m_depth_buffer);
<span style="white-space:pre"> </span>imshow("Depth Frame", depth_frame);
}
}現在編譯並運行程序，我們可以看到通過OpenCV畫出來的深度圖：
但你會發現，這樣的深度圖具有很多小孔和噪點，邊緣也不平滑。因此要對圖像進行濾波，爲了使邊緣不被模糊掉，這裏最好使用中值濾波。修改一下上面的update()函數，使用5x5的窗口進行中值濾波：
void testApp::update(){
//Get a new depth frame from Kinect.
m_new_depth = UpdateDepthFrame();
if (m_new_depth)
{
Mat smoothed_depth = Mat(DEPTH_IMAGE_HEIGHT,DEPTH_IMAGE_WIDTH,CV_16UC1,m_depth_buffer);
medianBlur(smoothed_depth,smoothed_depth,5);
imshow("Depth Frame", smoothed_depth);
}
}
再次運行程序，得到的深度圖就變成下面這樣了，感覺好了很多！！
2. 通過深度圖得到點雲：
爲了得到點雲，我專門寫了一個類來完成這一操作。這個類不僅會根據深度圖計算點雲，還會將得到的點雲以矩陣的形式存放起來，矩陣中每一個元素代表一個點，同時對應深度圖中具有相同行列座標的像素。而計算點雲的方法，Kinect SDK自身有提供，即NuiTransformDepthImageToSkeleton()函數，具體用法可看官方文檔。
下面是這個類中生成點雲的代碼：
void PointCloudMap::Create(Mat& depth_image,USHORT max_depth,float scale)
{
USHORT* depth_line = (USHORT*)depth_image.data;
UINT stride = depth_image.step1();
//m_points is the place where we store the whole point cloud.
ofVec3f* points_line = m_points;
Vector4 vec;
for (DWORD y = 0; y < m_height; y++)
{
for (DWORD x = 0; x < m_width; x++)
{
ofVec3f point(0);
USHORT real_depth = (depth_line[x] >> 3);
if (real_depth >= 800 && real_depth < max_depth)
{
//For each pixel in the depth image, we calculate its space coordinates.
vec = NuiTransformDepthImageToSkeleton(
x,
y,
depth_line[x]
);
//Save the point with a scale.
point.x = vec.x*scale;
point.y = vec.y*scale;
point.z = -vec.z*scale;
}
points_line[x] = point;
}
depth_line += stride;
points_line += m_width;
}
}
拿到點雲後，我們可以考慮對點雲進行三角化了。一提到三角化，很多人腦海中的第一印象是複雜、計算量大等等，我個人也是這樣。但是，Kinect返回的點雲是結構化的，並不是無序點雲，也就是說每一個點在空間中與其他點的相互關係我們是知道的，因此可以用一些簡單的方法來實現三角化，雖然這樣的三角化結果不是最優的，但是簡單快速，60fps毫無壓力。
首先，我們的點雲是存放在一個矩陣中的，而且矩陣的大小與深度圖完全一樣（行x列），因此我們將點雲視爲一幅圖，每一個像素存放的是點的空間座標。我們可以像遍歷一般圖像的像素一樣遍歷點雲圖，從而得到空間中某一點的所有相鄰點。然後，我們使用OpenGL的連線功能，每畫一個點就與它之前的兩個點連成一個三角面。
如下圖，點旁邊的序號是畫點的順序：
這樣我們就可以一行一行的將點雲三角化，但注意當一行結束時，要讓OpenGL停止連線，否則這一行最後的點會和下一行第一個點連在一起。
以上過程我直接寫在了主程序的draw方法中，OF在每一幀調用完update方法後，就會調用draw方法：
void testApp::draw(){
if (!m_init_succeeded)return;
m_camera.begin();
ofVec3f* points_line = m_cloud_map.m_points;
ofVec3f* points_next_line = m_cloud_map.m_points + DEPTH_IMAGE_WIDTH;
bool mesh_break = true;
for (int y = 0; y < m_cloud_map.m_height - 1; y++)
{
for (int x = 0; x < m_cloud_map.m_width; x++)
{
ofVec3f& space_point1 = points_line[x];
ofVec3f& space_point2 = points_next_line[x];
if (abs(space_point1.z) <= FLT_EPSILON*POINT_CLOUD_SCALE ||
abs(space_point2.z) <= FLT_EPSILON*POINT_CLOUD_SCALE)
{
if (!mesh_break)
{
//If there's no point here, the mesh should break.
mesh_break = true;
glEnd();
}
continue;
}
if (mesh_break)
{
//Start connecting points to form mesh.
glBegin(GL_TRIANGLE_STRIP);
mesh_break = false;
}
//Draw the point and set its normal.
glColor3f(0.7,0.7,0.7);
glVertex3f(space_point1.x,space_point1.y,space_point1.z);
//Draw the point below the prior one to form a triangle.
glColor3f(0.7,0.7,0.7);
glVertex3f(space_point2.x,space_point2.y,space_point2.z);
}
if (!mesh_break)
{
//At the end of the line, we break the mesh.
glEnd();
mesh_break = true;
}
points_line += DEPTH_IMAGE_WIDTH;
points_next_line += DEPTH_IMAGE_WIDTH;
}
m_camera.end();
//Draw frame rate for fun!
ofSetColor(255);
ofDrawBitmapString(ofToString(ofGetFrameRate()),10,20);
}再次編譯並運行程序，在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領域點作爲領點。（圖畫得醜，還請諒解）：
但是我們的點是有深度的，所以還需對以上領域點判斷一下深度，只有某一點的深度與中心點的深度接近時，才能真正當做領點。
現在還有最後一個問題，通過上面的方法算出來的法線方向是不定的（有可能是你想要的法向的反方向），因此我們還需要一個方法讓所有法線的朝向一致，我這裏就簡單的選擇了朝向攝像機。
將上面的所有方法寫在了一個類中，這個類根據點雲圖計算法線，並像點雲圖一樣將所有法線保存爲一副法線圖。下面是計算法線和調整朝向的代碼：
void NormalsMap::Create(PointCloudMap& point_cloud, float max_distance)//創建一副法線圖
{
if (point_cloud.m_height != m_height ||
point_cloud.m_width != m_width)
throw exception("NormalsMap has different size width the PointCloudMap");
ofVec3f* points_line0 = point_cloud.m_points;
ofVec3f* points_line1 = points_line0 + m_width;
ofVec3f* points_line2 = points_line1 + m_width;
ofVec3f* norms_line = m_normals + m_width;
vector<ofVec3f> neighbors;
int y_line0 = 0;
int y_line1 = y_line0 + m_width;
int y_line2 = y_line1 + m_width;
for (int y = 1; y < m_height - 1; y++)
{
for (int x = 1; x < m_width - 1; x++)
{
neighbors.clear();
norms_line[x] = ofVec3f(0);
if (points_line1[x].z == 0)continue;
neighbors.push_back(points_line1[x]);
//Add all neighbor points to the vector.
if (IsNeighbor(points_line0[x-1],points_line1[x],max_distance))
{
neighbors.push_back(points_line0[x-1]);
}
if (IsNeighbor(points_line0[x],points_line1[x],max_distance))
{
neighbors.push_back(points_line0[x]);
}
if (IsNeighbor(points_line0[x+1],points_line1[x],max_distance))
{
neighbors.push_back(points_line0[x+1]);
}
if (IsNeighbor(points_line1[x-1],points_line1[x],max_distance))
{
neighbors.push_back(points_line1[x-1]);
}
if (IsNeighbor(points_line1[x+1],points_line1[x],max_distance))
{
neighbors.push_back(points_line1[x+1]);
}
if (IsNeighbor(points_line2[x-1],points_line1[x],max_distance))
{
neighbors.push_back(points_line2[x-1]);
}
if (IsNeighbor(points_line2[x],points_line1[x],max_distance))
{
neighbors.push_back(points_line2[x]);
}
if (IsNeighbor(points_line2[x+1],points_line1[x],max_distance))
{
neighbors.push_back(points_line2[x+1]);
}
if (neighbors.size() < 3)continue;//Too small to identify a plane.
norms_line[x] = EstimateNormal(neighbors);
}
points_line0 += m_width;
points_line1 += m_width;
points_line2 += m_width;
norms_line += m_width;
y_line0 += m_width;
y_line1 += m_width;
y_line2 += m_width;
}
}
inline bool NormalsMap::IsNeighbor(ofVec3f& dst, ofVec3f& ori, float max_distance)//判斷是否是領點
{
if (abs(dst.z - ori.z) < max_distance)
return true;
return false;
}
ofVec3f NormalsMap::EstimateNormal(vector<ofVec3f>& points)//使用最小二乘法計算法線
{
ofVec3f normal(0);
float x = 0, y = 0, z = 0;
float x2 = 0, y2 = 0, z2 = 0;
float xy = 0, xz = 0, yz = 0;
for (int i = 0; i < points.size(); i++)
{
float cx = points[i].x;
float cy = points[i].y;
float cz = points[i].z;
x += cx; y += cy; z += cz;
x2 += cx*cx; y2 += cy*cy; z2 += cz*cz;
xy += cx*cy; xz += cx*cz; yz += cy*cz;
}
float D = x2*y2*z2 + 2*xy*xz*yz - x2*yz*yz - y2*xz*xz - z2*xy*xy;
if (abs(D) >= FLT_EPSILON)
{
//Use least squares technique to get the best normal.
float Da = x*(yz*yz - y2*z2) - y*(yz*xz - z2*xy) + z*(y2*xz - xy*yz);
float Db = x2*(z*yz - y*z2) - xy*(z*xz - x*z2) + xz*(y*xz - x*yz);
float Dc = x2*(y*yz - z*y2) - xy*(x*yz - z*xy) + xz*(x*y2 - y*xy);
normal.x = Da/D;
normal.y = Db/D;
normal.z = Dc/D;
normal.normalize();
}
else
{
/*D == 0, it means some axes(x,y or z) are on the normal plane.
We need another way to calculate normal vector.*/
ofVec3f row0(x2,xy,xz);
ofVec3f row1(xy,y2,yz);
ofVec3f row2(xz,yz,z2);
ofVec3f vec1 = row0.getCrossed(row1);
ofVec3f vec2 = row0.getCrossed(row2);
ofVec3f vec3 = row1.getCrossed(row2);
float len1 = vec1.lengthSquared();
float len2 = vec2.lengthSquared();
float len3 = vec3.lengthSquared();
if (len1 >= len2 && len1 >= len3)
normal = vec1 / sqrt(len1);
else if (len2 >= len1 && len2 >= len3)
normal = vec2 / sqrt(len2);
else
normal = vec3 / sqrt(len3);
}
return normal;
}
void NormalsMap::FlipNormalsToVector(ofVec3f main_vector)//調整法線朝向，是其全部指向main_vector方向
{
ofVec3f* normal = m_normals;
for (int i = 0; i < m_width*m_height; i++)
{
if ((*normal).dot(main_vector) < 0)
(*normal) *= -1;
normal++;
}
}4. 全部放在一起：
將以上全部放在一起，並修改一下我們的draw函數，以使其設置頂點的法向：
void testApp::draw(){
if (!m_init_succeeded)return;
m_camera.begin();
ofVec3f* points_line = m_cloud_map.m_points;
ofVec3f* points_next_line = m_cloud_map.m_points + DEPTH_IMAGE_WIDTH;
ofVec3f* normals_line = m_normal_map.m_normals;
bool mesh_break = true;
for (int y = 0; y < m_cloud_map.m_height - 1; y++)
{
for (int x = 0; x < m_cloud_map.m_width; x++)
{
ofVec3f& space_point1 = points_line[x];
ofVec3f& space_point2 = points_next_line[x];
if (abs(space_point1.z) <= FLT_EPSILON*POINT_CLOUD_SCALE ||
abs(space_point2.z) <= FLT_EPSILON*POINT_CLOUD_SCALE)
{
if (!mesh_break)
{
//If there's no point here, the mesh should break.
mesh_break = true;
glEnd();
}
continue;
}
if (mesh_break)
{
//Start connecting points to form mesh.
glBegin(GL_TRIANGLE_STRIP);
mesh_break = false;
}
//Draw the point and set its normal.
glColor3f(0.8,0.8,0.8);
glNormal3f(normals_line[x].x,normals_line[x].y,normals_line[x].z);
glVertex3f(space_point1.x,space_point1.y,space_point1.z);
//Draw the point below the prior one to form a triangle.
glColor3f(0.8,0.8,0.8);
glVertex3f(space_point2.x,space_point2.y,space_point2.z);
}
if (!mesh_break)
{
//We break the mesh at the end of the line,.
glEnd();
mesh_break = true;
}
points_line += DEPTH_IMAGE_WIDTH;
points_next_line += DEPTH_IMAGE_WIDTH;
normals_line += DEPTH_IMAGE_WIDTH;
}
m_camera.end();
//Draw frame rate for fun!
ofSetColor(255);
ofDrawBitmapString(ofToString(ofGetFrameRate()),10,20);
}最後編譯運行，我們的目標就達到了！！！！
作爲一個自娛自樂的小程序，感覺還不錯吧！！！注意看左上角的幀率，60fps妥妥的。
小結：
做這個完全是爲了學習和興趣，不要說我是重複造輪子啊。寫這個程序複習了很多線性代數的知識，溫故而知新，感覺還是很有收穫的。最後的效果還可以改進，最大的改進點就是三角化的方法，以後發現快速且效果好的三角化方法再和大家分享。
最後給出代碼的下載地址 點擊打開鏈接
代碼在Windows7 ultimate，OpenCV 2.4.3，OpenFrameworks 0073，Kinect SDK 1.7 下編譯通過。
編譯有問題的可以看看下面的評論。