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和三維場景做一些設置：
?
[cpp]?view plaincopy
void?testApp::setup(){??????????ofSetVerticalSync(true);??????ofSetWindowShape(640,480);??????ofBackground(0,0,0);????????????glEnable(GL_DEPTH_TEST);??????glDepthFunc(GL_LEQUAL);??????glShadeModel(GL_SMOOTH);????????????????m_camera.setDistance(3);??????m_camera.setNearClip(0.1f);????????????m_light.enable();????????????m_cloud_map.Resize(DEPTH_IMAGE_WIDTH,DEPTH_IMAGE_HEIGHT);??????m_normal_map.Resize(DEPTH_IMAGE_WIDTH,DEPTH_IMAGE_HEIGHT);??????????InitNui();??}??
OF是使用OpenGL進行繪圖的，所以可以直接使用OpenGL中的函數（以gl開頭），為了方便，OF還自己封裝了一些常用函數（以of開頭）。在上面代碼的最后有一個InitNui()函數，在那里面我們會對Kinect進行初始化：
?
?
[cpp]?view plaincopy
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;??????}????????????hr?=?m_nui->NuiInitialize(NUI_INITIALIZE_FLAG_USES_DEPTH);????????if?(FAILED(hr))??????{??????????cout<<"Initialize?Kinect?Failed!!"<<endl;??????????goto?Final;??????}????????????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中，專門寫一個函數來做這件事情：
?
?
[cpp]?view plaincopy
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?(SUCCEEDED(hr))??????{??????????hr?=?image_frame.pFrameTexture->LockRect(0,&locked_rect,NULL,0);??????????if?(SUCCEEDED(hr))??????????{??????????????????????????memcpy(m_depth_buffer,locked_rect.pBits,locked_rect.size);????????????????image_frame.pFrameTexture->UnlockRect(0);??????????}??????????????????m_nui->NuiImageStreamReleaseFrame(m_depth_stream,&image_frame);??????}????????????if?(SUCCEEDED(hr))return?true;????????return?false;??}??
通過上面幾步，我們已經可以拿到一幅深度圖了。在OF中，每一幀更新時，update()函數都會被調用，我們可以把所有需要適時更新的代碼都寫在里面：
?
?
[cpp]?view plaincopy
void?testApp::update(){????????????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的窗口進行中值濾波：
?
[cpp]?view plaincopy
void?testApp::update(){????????????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()函數，具體用法可看官方文檔。
下面是這個類中生成點云的代碼：
?
[cpp]?view plaincopy
void?PointCloudMap::Create(Mat&?depth_image,USHORT?max_depth,float?scale)??{??????USHORT*?depth_line?=?(USHORT*)depth_image.data;??????UINT?stride?=?depth_image.step1();????????????????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)??????????????{??????????????????????????????????vec?=?NuiTransformDepthImageToSkeleton(??????????????????????x,??????????????????????y,??????????????????????depth_line[x]??????????????????);????????????????????????????????????????????????????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方法：
?
[cpp]?view plaincopy
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)??????????????????{??????????????????????????????????????????mesh_break?=?true;??????????????????????glEnd();??????????????????}??????????????????continue;??????????????}????????????????if?(mesh_break)??????????????{??????????????????????????????????glBegin(GL_TRIANGLE_STRIP);??????????????????mesh_break?=?false;??????????????}????????????????????????????glColor3f(0.7,0.7,0.7);??????????????glVertex3f(space_point1.x,space_point1.y,space_point1.z);????????????????????????????????????????glColor3f(0.7,0.7,0.7);??????????????glVertex3f(space_point2.x,space_point2.y,space_point2.z);??????????}??????????if?(!mesh_break)???????????{??????????????????????????glEnd();??????????????mesh_break?=?true;??????????}??????????points_line?+=?DEPTH_IMAGE_WIDTH;??????????points_next_line?+=?DEPTH_IMAGE_WIDTH;??????}????????????m_camera.end();????????????????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領域點作為領點。（圖畫得丑，還請諒解）：
但是我們的點是有深度的，所以還需對以上領域點判斷一下深度，只有某一點的深度與中心點的深度接近時，才能真正當做領點。
現在還有最后一個問題，通過上面的方法算出來的法線方向是不定的（有可能是你想要的法向的反方向），因此我們還需要一個方法讓所有法線的朝向一致，我這里就簡單的選擇了朝向攝像機。
將上面的所有方法寫在了一個類中，這個類根據點云圖計算法線，并像點云圖一樣將所有法線保存為一副法線圖。下面是計算法線和調整朝向的代碼：
?
[cpp]?view plaincopy
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]);??????????????????????????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;??????????????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)??????{??????????????????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??????{????????????????????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){??????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函數，以使其設置頂點的法向：
?
[cpp]?view plaincopy
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)??????????????????{??????????????????????????????????????????mesh_break?=?true;??????????????????????glEnd();??????????????????}??????????????????continue;??????????????}????????????????if?(mesh_break)??????????????{??????????????????????????????????glBegin(GL_TRIANGLE_STRIP);??????????????????mesh_break?=?false;??????????????}????????????????????????????????????????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);????????????????????????????????????????glColor3f(0.8,0.8,0.8);??????????????glVertex3f(space_point2.x,space_point2.y,space_point2.z);??????????}??????????if?(!mesh_break)???????????{??????????????????????????glEnd();??????????????mesh_break?=?true;??????????}??????????points_line?+=?DEPTH_IMAGE_WIDTH;??????????points_next_line?+=?DEPTH_IMAGE_WIDTH;??????????normals_line?+=?DEPTH_IMAGE_WIDTH;??????}????????????m_camera.end();????????????????ofSetColor(255);??????ofDrawBitmapString(ofToString(ofGetFrameRate()),10,20);??}??
最后編譯運行，我們的目標就達到了！！！！
?
?
作為一個自娛自樂的小程序，感覺還不錯吧！！！注意看左上角的幀率，60fps妥妥的。
?
小結：
做這個完全是為了學習和興趣，不要說我是重復造輪子啊。寫這個程序復習了很多線性代數的知識，溫故而知新，感覺還是很有收獲的。最后的效果還可以改進，最大的改進點就是三角化的方法，以后發現快速且效果好的三角化方法再和大家分享。
最后給出代碼的下載地址?點擊打開鏈接
代碼在Windows7 ultimate，opencv?2.4.3，OpenFrameworks 0073，Kinect SDK 1.7 下編譯通過。
編譯有問題的可以看看下面的評論。
轉載于:https://www.cnblogs.com/lyx2018/p/7130401.html
總結
以上是生活随笔為你收集整理的Kinect实现简单的三维重建的全部內容，希望文章能夠幫你解決所遇到的問題。
如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。
