Kinect-OpenNI-OpenCV-OpenGLに基づく環境三次元再構成
68381 ワード
http://www.opencv.org.cn/index.php/%E5%9F%BA%E4%BA%8EKinect-OpenNI-OpenCV-OpenGL%E 7%E 7%E 7%E 7%8 E 5%A 2%83%E 4%B 8%B 7%BB%B 4%E 9%8 D%E 6%9 E 8%84
プロジェクトのソースコードの詳細:http://www.opencv.org.cn/forum/viewtopic.php?f=1&t=13042
先日、待望のKinectを入手しました。実験室のロボットプロジェクトの視覚ナビゲーションと環境理解に使います。
まずはクラック-->接続PC-->奥行きデータと画像データを取得します。三次元ポイントクラウドはこのようないくつかの基本的な仕事を示しています。
模倣し始めたのは飲水思源[1]である。 ブログの方法(VSL 2008を使ってwindowsプラットフォームでKinect[2]を試してみます。 ),CL-NUID-Platformを利用して解読して、その最新版は1.0.0.1210ですが、XPの上で会当機を使って、その後1.0.0.1121版のを交換して使えます。CL NUIは、非常に使いやすいインターフェースを提供しています。OpenCVでの呼び出しは簡単です。また、Kinectベースモータの制御インターフェースとLEDライトの色の選択インターフェースも提供しています。深さデータと画像データだけを得る必要があるなら、CL NUIは十分です。ただし、人間の姿の識別、骨格の抽出、奥行きデータと画像データの統合など、深い応用を行うには、OpenNIが必要です。
国内のCNKINECT[3] いいKinect開発フォーラムです。版元が豊富で、参考になる資料がたくさんあります。私はフォーラムを通じて紹介した方法[4] OpenNI+Kinectを配置することに成功しました。最初は最新版のOpenNI+Sensorキンect+NITEを使っていますが、XPの下では正常に運行できません。ネットのプラットフォームと関係があるかもしれません。また、CMake+VVS 2008で最新のOpenCVを搭載しました。SVNは、CMakeでWith TBを選択したことがありますが、怪しいのはTBだけVSL 2005に適用されているようです。VSL 2008でコンパイルした後、試用中のsamplesはいつもエラーが発生しました。msvcp 80.dllが見つからなくて、再びCMakeの配置でWith TBをキャンセルしたら、すべて正常になります。
[編集]
一、深さカメラの画角調整と深さ/カラー画像の統合
OpenCV_を研究することによりSVNとOpenNI関連コード(cap_OpenCVは現在、Kinectの深さ図、視差図、カラー/グレースケール図および関連する属性の読み取りのみをサポートしており、さらに設定はまだ実現されていないことがわかった。台湾のHerery’space[5]を参照してください。 のブログ記事「OpneNIを通してKinectの深さとカラー映像資料を統合」「6」を、深さ図とカラー図を統合して表示したいと思いますが、Kinectの深さカメラとカラーカメラは異なる位置にあり、レンズ自体のパラメータも全く同じではありません。このため、2つのカメラで取得した画面には多少の違いがあります(図1の左下隅図OpenGLの3次元点雲表示ウィンドウに示すように、天井の2つの蛍光灯の対応の深さ図とカラー図の領域が重なっていないので、ずれています)。
図1
Hereryの分析によると、深さカメラの画角を修正する必要があります。OpenNIの下では1行のコードだけで実現できます。
cap_をopenni.cpp 344行目のset DepthGeneratoPropertyは次の通り書き換えられます。
図2
[編集]
二、三次元点雲座標の計算
OpenCVでは実際に三次元点クラウド座標計算のインターフェースが提供されています。Kinectの深さデータによって三次元点クラウド座標に速く換算できます。cap_openni.cppのこの部分のコードは以下の通りです。
次に、マトリックスQの構造過程を以下のコードで見ます。
図3 Matlabで描かれた三次元点雲の深さによって明らかに階層化されている。
図4は、直接得られた奥行きデータMesh図であり、8ビットの視差データから計算された奥行きデータであり、右は対応する視差データである。
32ビット浮動小数点フォーマットを採用して視差データ(パラメータID:CV_CAP_OPENI_DISPARITY_地図32 F)は、視差誤差による深さ数値階層の現象を除去し、図5、6に示すように、視差誤差による深さ数値階層の現象を除去する。
図5の深さ数値の階層現象は基本的に除去される。
図6の2つの方法で得られた深さデータは一致している。
[編集]
三、OpenGLで三次元ポイントクラウドデータを表示する
この方面は主に学習ノート(15)に基づいています。 [7)の内容ですが、OpenCVウィンドウを別に設けてTrackbarを設定することでOpenGLの視点位置とカメラ位置を調整しました。ここでは主にOpenCVフォーラムの書き込み「HQ」OpenCVとOpenGLを参考にしてプログラムしました。表示点クラウドデータ-Stereo Visionソース共有」「8」でvillager 5から提供された方法でカメラの位置を調整して、マウスの左ボタンドラッグで上下左右の視角変換ができます。マウスの右ボタンでドラッグして、視認距離の変換ができます。Trackbarで調整する必要はありません。以下は関連コードです。
プロジェクトのソースコードの詳細:http://www.opencv.org.cn/forum/viewtopic.php?f=1&t=13042
先日、待望のKinectを入手しました。実験室のロボットプロジェクトの視覚ナビゲーションと環境理解に使います。
まずはクラック-->接続PC-->奥行きデータと画像データを取得します。三次元ポイントクラウドはこのようないくつかの基本的な仕事を示しています。
模倣し始めたのは飲水思源[1]である。 ブログの方法(VSL 2008を使ってwindowsプラットフォームでKinect[2]を試してみます。 ),CL-NUID-Platformを利用して解読して、その最新版は1.0.0.1210ですが、XPの上で会当機を使って、その後1.0.0.1121版のを交換して使えます。CL NUIは、非常に使いやすいインターフェースを提供しています。OpenCVでの呼び出しは簡単です。また、Kinectベースモータの制御インターフェースとLEDライトの色の選択インターフェースも提供しています。深さデータと画像データだけを得る必要があるなら、CL NUIは十分です。ただし、人間の姿の識別、骨格の抽出、奥行きデータと画像データの統合など、深い応用を行うには、OpenNIが必要です。
国内のCNKINECT[3] いいKinect開発フォーラムです。版元が豊富で、参考になる資料がたくさんあります。私はフォーラムを通じて紹介した方法[4] OpenNI+Kinectを配置することに成功しました。最初は最新版のOpenNI+Sensorキンect+NITEを使っていますが、XPの下では正常に運行できません。ネットのプラットフォームと関係があるかもしれません。また、CMake+VVS 2008で最新のOpenCVを搭載しました。SVNは、CMakeでWith TBを選択したことがありますが、怪しいのはTBだけVSL 2005に適用されているようです。VSL 2008でコンパイルした後、試用中のsamplesはいつもエラーが発生しました。msvcp 80.dllが見つからなくて、再びCMakeの配置でWith TBをキャンセルしたら、すべて正常になります。
[編集]
一、深さカメラの画角調整と深さ/カラー画像の統合
OpenCV_を研究することによりSVNとOpenNI関連コード(cap_OpenCVは現在、Kinectの深さ図、視差図、カラー/グレースケール図および関連する属性の読み取りのみをサポートしており、さらに設定はまだ実現されていないことがわかった。台湾のHerery’space[5]を参照してください。 のブログ記事「OpneNIを通してKinectの深さとカラー映像資料を統合」「6」を、深さ図とカラー図を統合して表示したいと思いますが、Kinectの深さカメラとカラーカメラは異なる位置にあり、レンズ自体のパラメータも全く同じではありません。このため、2つのカメラで取得した画面には多少の違いがあります(図1の左下隅図OpenGLの3次元点雲表示ウィンドウに示すように、天井の2つの蛍光灯の対応の深さ図とカラー図の領域が重なっていないので、ずれています)。
図1
Hereryの分析によると、深さカメラの画角を修正する必要があります。OpenNIの下では1行のコードだけで実現できます。
// 6. correct view port
mDepthGenerator.GetAlternativeViewPointCap().SetViewPoint( mImageGenerator );
しかし、OpenCVではこの設定は提供されていません。ソースコードmodules\highggui\src\cap_openni.cppのset DepthGeneratoPropertyは実質的な属性設定がありません。このためには、関数を書き換える必要があり、対応するヘッダファイルmodules\highgggui\include\opencv 2\highggghggggggh_c.hに新しいパラメータIDを追加します。具体的には以下の通りです。cap_をopenni.cpp 344行目のset DepthGeneratoPropertyは次の通り書き換えられます。
bool CvCapture_OpenNI::setDepthGeneratorProperty( int propIdx, double propValue )
{
bool res = false;
CV_Assert( depthGenerator.IsValid() );
switch( propIdx )
{
case CV_CAP_PROP_OPENNI_VIEW_POINT :
depthGenerator.GetAlternativeViewPointCap().SetViewPoint( imageGenerator );
res = true;
break;
default :
CV_Error( CV_StsBadArg, "Depth generator does not support such parameter for setting.
");
res = false;
}
return res;
}
ヒグヒグにいますc.hの348行目に、画角を変えるパラメータID番号を追加します。CV_CAP_PROP_OPENNI_VIEW_POINT = 24,
352行目に追加します。CV_CAP_OPENNI_DEPTH_GENERATOR_VIEW_POINT = CV_CAP_OPENNI_DEPTH_GENERATOR + CV_CAP_PROP_OPENNI_VIEW_POINT
したがって、OpenCVのVideoCapture属性はOpenNIについて以下のように示します。// OpenNI map generators
CV_CAP_OPENNI_DEPTH_GENERATOR = 0,
CV_CAP_OPENNI_IMAGE_GENERATOR = 1 << 31,
CV_CAP_OPENNI_GENERATORS_MASK = 1 << 31,
// Properties of cameras avalible through OpenNI interfaces
CV_CAP_PROP_OPENNI_OUTPUT_MODE = 20,
CV_CAP_PROP_OPENNI_FRAME_MAX_DEPTH = 21, // in mm
CV_CAP_PROP_OPENNI_BASELINE = 22, // in mm
CV_CAP_PROP_OPENNI_FOCAL_LENGTH = 23, // in pixels
'''CV_CAP_PROP_OPENNI_VIEW_POINT = 24,'''
CV_CAP_OPENNI_IMAGE_GENERATOR_OUTPUT_MODE = CV_CAP_OPENNI_IMAGE_GENERATOR + CV_CAP_PROP_OPENNI_OUTPUT_MODE,
CV_CAP_OPENNI_DEPTH_GENERATOR_BASELINE = CV_CAP_OPENNI_DEPTH_GENERATOR + CV_CAP_PROP_OPENNI_BASELINE,
CV_CAP_OPENNI_DEPTH_GENERATOR_FOCAL_LENGTH = CV_CAP_OPENNI_DEPTH_GENERATOR + CV_CAP_PROP_OPENNI_FOCAL_LENGTH,
'''CV_CAP_OPENNI_DEPTH_GENERATOR_VIEW_POINT = CV_CAP_OPENNI_DEPTH_GENERATOR + CV_CAP_PROP_OPENNI_VIEW_POINT'''
上記ソースコードを書き換えて保存し、CMakeとVSB 2008でOpenCVをコンパイルし直したら、OpenCVのインターフェースでKinect深さカメラの画角を制御できます。その深さデータと画像データをうまく重ね合わせられます。これは画角調整後の効果です。図2
[編集]
二、三次元点雲座標の計算
OpenCVでは実際に三次元点クラウド座標計算のインターフェースが提供されています。Kinectの深さデータによって三次元点クラウド座標に速く換算できます。cap_openni.cppのこの部分のコードは以下の通りです。
IplImage* CvCapture_OpenNI::retrievePointCloudMap()
{
int cols = depthMetaData.XRes(), rows = depthMetaData.YRes();
if( cols <= 0 || rows <= 0 )
return 0;
cv::Mat depth;
getDepthMapFromMetaData( depthMetaData, depth, noSampleValue, shadowValue );
const float badPoint = 0;
cv::Mat pointCloud_XYZ( rows, cols, CV_32FC3, cv::Scalar::all(badPoint) );
for( int y = 0; y < rows; y++ )
{
for( int x = 0; x < cols; x++ )
{
unsigned short d = depth.at(y, x);
// Check for invalid measurements
if( d == CvCapture_OpenNI::INVALID_PIXEL_VAL ) // not valid
continue;
XnPoint3D proj, real;
proj.X = x;
proj.Y = y;
proj.Z = d;
depthGenerator.ConvertProjectiveToRealWorld(1, &proj, &real);
pointCloud_XYZ.at(y,x) = cv::Point3f( real.X*0.001f, real.Y*0.001f, real.Z*0.001f); // from mm to meters
}
}
outputMaps[CV_CAP_OPENNI_POINT_CLOUD_MAP].mat = pointCloud_XYZ;
return outputMaps[CV_CAP_OPENNI_POINT_CLOUD_MAP].getIplImagePtr();
}
しかし、上のコアコードが見えます。depthGenerator.ConvertProjectiveToRealWorld(1, &proj, &real);
具体的にはどうやって実現しますか?OpenNIのソースコードをさらに探して分析できます。しかし、ここでは、元の双眼視の経験から、自分でコードを作成して、三次元点雲座標の計算を実現します。実際には、Kinectの深さカメラ撮影も、通常の両眼立体視と似ていますが、2つのカメラの間のベースラインと焦点距離、視差データを取得すれば、マトリクスQを構築することにより、OpenCVのreprojectimageTo 3 D関数を利用して、3次元座標を算出することもできます。次に、マトリックスQの構造過程を以下のコードで見ます。
capture.set(CV_CAP_OPENNI_DEPTH_GENERATOR_VIEW_POINT, 1.0); //
double b = capture.get( CV_CAP_OPENNI_DEPTH_GENERATOR_BASELINE ); // mm
double F = capture.get( CV_CAP_OPENNI_DEPTH_GENERATOR_FOCAL_LENGTH ); // pixels
double q[] =
{
1, 0, 0, -320.0,
0, 1, 0, -240.0,
0, 0, 0, F,
0, 0, 1./b, 0
};
Mat matQ(4, 4, CV_64F, q);
三次元ポイントクラウド座標の計算、深さ、視差、カラー画像の読み取りは、以下のようになる。//
if( !capture.grab() )
{
cout << "Can not grab images." << endl;
return -1;
}
//
capture.retrieve( depthMap, CV_CAP_OPENNI_DEPTH_MAP );
minMaxLoc( depthMap, &mindepth, &maxdepth );
//
capture.retrieve( disparityMap, CV_CAP_OPENNI_DISPARITY_MAP_32F );
colorizeDisparity( disparityMap, colorDisparityMap, maxDisparity );
colorDisparityMap.copyTo( validColorDisparityMap, disparityMap != 0 );
imshow( "colorized disparity map", validColorDisparityMap );
//
capture.retrieve( bgrImage, CV_CAP_OPENNI_BGR_IMAGE );
imshow( "Color image", bgrImage );
//
cv::reprojectImageTo3D(disparityMap, xyzMap, matQ, true);
ここで注目すべきことは、視差を計算する際に、視差マップがデフォルトのCV_を採用する場合である。8 UC 1フォーマット(パラメータIDはCV_です。CAP_OPENI_DISPARITY_MAP)視差が[0−255]の整数範囲に制限されているため、一定の誤差が生じ、得られた3次元座標データに階層が現れ、図3及び図4に示すように。図3 Matlabで描かれた三次元点雲の深さによって明らかに階層化されている。
図4は、直接得られた奥行きデータMesh図であり、8ビットの視差データから計算された奥行きデータであり、右は対応する視差データである。
32ビット浮動小数点フォーマットを採用して視差データ(パラメータID:CV_CAP_OPENI_DISPARITY_地図32 F)は、視差誤差による深さ数値階層の現象を除去し、図5、6に示すように、視差誤差による深さ数値階層の現象を除去する。
図5の深さ数値の階層現象は基本的に除去される。
図6の2つの方法で得られた深さデータは一致している。
[編集]
三、OpenGLで三次元ポイントクラウドデータを表示する
この方面は主に学習ノート(15)に基づいています。 [7)の内容ですが、OpenCVウィンドウを別に設けてTrackbarを設定することでOpenGLの視点位置とカメラ位置を調整しました。ここでは主にOpenCVフォーラムの書き込み「HQ」OpenCVとOpenGLを参考にしてプログラムしました。表示点クラウドデータ-Stereo Visionソース共有」「8」でvillager 5から提供された方法でカメラの位置を調整して、マウスの左ボタンドラッグで上下左右の視角変換ができます。マウスの右ボタンでドラッグして、視認距離の変換ができます。Trackbarで調整する必要はありません。以下は関連コードです。
#define SIGN(x) ( (x)<0 ? -1:((x)>0?1:0 ) )
//////////////////////////////////////////////////////////////////////////
//
//---OpenGL
float xyzdata[480][640][3];
float texture[480][640][3];
int glWinWidth = 640, glWinHeight = 480;
int width=640, height=480;
double eyex, eyey, eyez, atx, aty, atz; // eye* - ,at* -
bool leftClickHold = false, rightClickHold = false;
int mx,my; // OpenGL
int ry = 90, rx = 90; //
double mindepth, maxdepth; //
double radius = 6000.0; //
/************************************************************************/
/* OpenGL */
/************************************************************************/
//////////////////////////////////////////////////////////////////////////
//
void mouse(int button, int state, int x, int y)
{
if(button == GLUT_LEFT_BUTTON)
{
if(state == GLUT_DOWN)
{
leftClickHold=true;
}
else
{
leftClickHold=false;
}
}
if (button== GLUT_RIGHT_BUTTON)
{
if(state == GLUT_DOWN)
{
rightClickHold=true;
}
else
{
rightClickHold=false;
}
}
}
//////////////////////////////////////////////////////////////////////////
//
void motion(int x, int y)
{
int rstep = 5;
if(leftClickHold==true)
{
if( abs(x-mx) > abs(y-my) )
{
rx += SIGN(x-mx)*rstep;
}
else
{
ry -= SIGN(y-my)*rstep;
}
mx=x;
my=y;
glutPostRedisplay();
}
if(rightClickHold==true)
{
radius += SIGN(y-my)*100.0;
radius = std::max( radius, 100.0 );
mx=x;
my=y;
glutPostRedisplay();
}
}
//////////////////////////////////////////////////////////////////////////
//
void renderScene(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
atx = 0.0f;
aty = 0.0f;
atz = ( mindepth - maxdepth ) / 2.0f;
eyex = atx + radius * sin( CV_PI * ry / 180.0f ) * cos( CV_PI * rx/ 180.0f );
eyey = aty + radius * cos( CV_PI * ry/ 180.0f );
eyez = atz + radius * sin( CV_PI * ry / 180.0f ) * sin( CV_PI * rx/ 180.0f );
gluLookAt (eyex, eyey, eyez, atx, aty, atz, 0.0, 1.0, 0.0);
glRotatef(0,0,1,0);
glRotatef(-180,1,0,0);
//
// :http://www.codeproject.com/KB/openGL/OPENGLTG.aspx
// we are going to loop through all of our terrain's data points,
// but we only want to draw one triangle strip for each set along the x-axis.
for (int i = 0; i < height; i++)
{
glBegin(GL_TRIANGLE_STRIP);
for (int j = 0; j < width; j++)
{
// for each vertex, we calculate the vertex color,
// we set the texture coordinate, and we draw the vertex.
/* the vertexes are drawn in this order: 0 ---> 1 / / / 2 ---> 3 */
// draw vertex 0
glTexCoord2f(0.0f, 0.0f);
glColor3f(texture[i][j][0]/255.0f, texture[i][j][1]/255.0f, texture[i][j][2]/255.0f);
glVertex3f(xyzdata[i][j][0], xyzdata[i][j][1], xyzdata[i][j][2]);
// draw vertex 1
glTexCoord2f(1.0f, 0.0f);
glColor3f(texture[i+1][j][0]/255.0f, texture[i+1][j][1]/255.0f, texture[i+1][j][2]/255.0f);
glVertex3f(xyzdata[i+1][j][0], xyzdata[i+1][j][1], xyzdata[i+1][j][2]);
// draw vertex 2
glTexCoord2f(0.0f, 1.0f);
glColor3f(texture[i][j+1][0]/255.0f, texture[i][j+1][1]/255.0f, texture[i][j+1][2]/255.0f);
glVertex3f(xyzdata[i][j+1][0], xyzdata[i][j+1][1], xyzdata[i][j+1][2]);
// draw vertex 3
glTexCoord2f(1.0f, 1.0f);
glColor3f(texture[i+1][j+1][0]/255.0f, texture[i+1][j+1][1]/255.0f, texture[i+1][j+1][2]/255.0f);
glVertex3f(xyzdata[i+1][j+1][0], xyzdata[i+1][j+1][1], xyzdata[i+1][j+1][2]);
}
glEnd();
}
// enable blending
glEnable(GL_BLEND);
// enable read-only depth buffer
glDepthMask(GL_FALSE);
// set the blend function to what we use for transparency
glBlendFunc(GL_SRC_ALPHA, GL_ONE);
// set back to normal depth buffer mode (writable)
glDepthMask(GL_TRUE);
// disable blending
glDisable(GL_BLEND);
/* float x,y,z; // glPointSize(1.0); glBegin(GL_POINTS); for (int i=0;i<height;i++){ for (int j=0;j<width;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(); */
glFlush();
glutSwapBuffers();
}
//////////////////////////////////////////////////////////////////////////
//
void reshape (int w, int h)
{
glWinWidth = w;
glWinHeight = h;
glViewport (0, 0, (GLsizei)w, (GLsizei)h);
glMatrixMode (GL_PROJECTION);
glLoadIdentity ();
gluPerspective (45, (GLfloat)w / (GLfloat)h, 1.0, 15000.0);
glMatrixMode (GL_MODELVIEW);
}
//////////////////////////////////////////////////////////////////////////
//
void load3dDataToGL(IplImage* xyz)
{
CvScalar s;
//accessing the image pixels
for (int i=0;i<height;i++)
{
for (int j=0;j<width;j++)
{
s=cvGet2D(xyz,i,j); // s.val[0] = x, s.val[1] = y, s.val[2] = z
xyzdata[i][j][0] = s.val[0];
xyzdata[i][j][1] = s.val[1];
xyzdata[i][j][2] = s.val[2];
}
}
}
//////////////////////////////////////////////////////////////////////////
//
void loadTextureToGL(IplImage* img)
{
CvScalar ss;
//accessing the image pixels
for (int i=0;i<height;i++)
{
for (int j=0;j<width;j++)
{
ss=cvGet2D(img,i,j); // ss.val[0] = red, ss.val[1] = green, ss.val[2] = blue
texture[i][j][2] = ss.val[0];
texture[i][j][1] = ss.val[1];
texture[i][j][0] = ss.val[2];
}
}
}