Kinect PointCloud
(Kinect) Point Cloud
KinectとVTKでPoint Cloud Rendering
Kinectの距離情報と可視化ライブラリのVTKを使ってPoint Cloud Rendering(キャプチャはOpenCVかOpenNI)
- OpenCVまたはOpenNIでKinectが使える状態
- VTKの開発環境がインストール済み
ライブラリの参照エラーが出る場合はエラーメッセージで不足している関数名を参考にvtk*.libを片っ端から参照させると何とかなる。あとOpenGL関係はVC++のフォルダを検索すれば見付かる。
VTKの機能をそのまま使えるのでStereoscopicな出力も簡単。マウスでグリグリ動く。 Kinectの入力さえ取れればWrapperが共通するPythonでも(遅いけど)利用可。(freenectは断念)
SetStereoTypeTo* で指定する。標準ではフレームシーケンシャルな出力(要QuadBuffer)。 nVidia 3D VisionはOpenGLで使う場合はQuadBufferが必要なのでQuadro以上のチップを搭載したPCでのみ利用可能。アナグリフや赤青はどんなPCでも表示可能。
MacBook Air '11でだいたい3~5 fps出るのでそれなりに速い。
/*
* PointCloud.cpp
*
* Created on: 2011/06/21
* Author: Eiichiro Momma
* Libraries:
* libvtkFiltering, libvtkRendering, libvtkCommon,
* libopencv_core, libopencv_imgproc, libopencv_highgui
*/
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <vtkType.h>
#include <vtkPoints.h>
#include <vtkPointData.h>
#include <vtkPolyData.h>
#include <vtkPolyDataMapper.h>
#include <vtkUnsignedCharArray.h>
#include <vtkCellArray.h>
#include <vtkSmartPointer.h>
#include <vtkActor.h>
#include <vtkRenderer.h>
#include <vtkRenderWindow.h>
#include <vtkRenderWindowInteractor.h>
#include <vtkCommand.h>
#include <vtkCamera.h>
using namespace cv;
//更新のためグローバル
VideoCapture capture;
Mat depthMap;
Mat bgrImage;
Mat validMask;
std::vector<Mat> planes;
vtkSmartPointer<vtkPolyData> polyData;
void updatePolyData(void)
{
capture.grab();
capture.retrieve(depthMap, CV_CAP_OPENNI_DEPTH_MAP);
capture.retrieve(bgrImage, CV_CAP_OPENNI_BGR_IMAGE);
capture.retrieve(validMask, CV_CAP_OPENNI_VALID_DEPTH_MASK);
flip(depthMap, depthMap, 0);
flip(bgrImage, bgrImage, 0);
flip(validMask, validMask, 0);
//Point Cloudの実体
vtkSmartPointer<vtkPoints> points = vtkSmartPointer<vtkPoints>::New();
split(bgrImage, planes);
//色情報用
vtkSmartPointer<vtkUnsignedCharArray> colors = vtkSmartPointer<vtkUnsignedCharArray>::New();
colors->SetName("Colors");
colors->SetNumberOfComponents(3);
//CellArrayで構成する。Sphereで作るとメモリを食い潰す
vtkSmartPointer<vtkCellArray> cellArray = vtkSmartPointer<vtkCellArray>::New();
for (int y=0; y< bgrImage.rows; y++)
{
for (int x=0; x< bgrImage.cols; x++)
{
unsigned short dp = depthMap.at<unsigned short>(y,x);
uchar isValid = validMask.at<uchar>(y,x);
//ValidPixel以外はz=4096に色を黒にして置く。
//Pointの配置は適当。見た目重視で0.5倍してる。ちゃんとやりたい場合はパラメータを拾う
vtkIdType id = points->InsertNextPoint(x, y, isValid? dp*0.5 : 4096 );
cellArray->InsertNextCell(1);
cellArray->InsertCellPoint(id);
if (isValid)
{
colors->InsertNextTuple3(planes[2].at<uchar>(y,x),
planes[1].at<uchar>(y,x), planes[0].at<uchar>(y,x));
}
else
{
colors->InsertNextTuple3(0,0,0);
}
}
}
polyData->SetPoints(points);
polyData->SetVerts(cellArray);
polyData->GetPointData()->SetScalars(colors);
polyData->Modified();
polyData->Update();
}
class vtkTimerCallback:public vtkCommand
{
public:
static vtkTimerCallback *New()
{
vtkTimerCallback *cb = new vtkTimerCallback;
cb->TimerCount=0;
return cb;
}
virtual void Execute(vtkObject *caller, unsigned long eventId, void *vtkNotUsed(callData))
{
if (vtkCommand::TimerEvent == eventId)
{
++this->TimerCount;
//PolyDataを初期化して再描画
//ループ中の描画はここでやる
polyData->Initialize();
updatePolyData();
((vtkRenderWindowInteractor*)caller)->GetRenderWindow()->Render();
}
}
private:
int TimerCount;
};
int main(void)
{
capture = VideoCapture(CV_CAP_OPENNI);
if (!capture.isOpened())
{
return -1;
}
capture.set(CV_CAP_OPENNI_IMAGE_GENERATOR_OUTPUT_MODE, CV_CAP_OPENNI_VGA_30HZ);
polyData = vtkSmartPointer<vtkPolyData>::New();
updatePolyData();
//VTKのお約束
vtkSmartPointer<vtkPolyDataMapper> mapMesh = vtkSmartPointer<vtkPolyDataMapper>::New();
mapMesh->SetInput(polyData);
vtkSmartPointer<vtkActor> meshActor = vtkSmartPointer<vtkActor>::New();
meshActor->SetMapper(mapMesh);
vtkSmartPointer<vtkRenderer> ren = vtkSmartPointer<vtkRenderer>::New();
vtkSmartPointer<vtkRenderWindow> renWin = vtkSmartPointer<vtkRenderWindow>::New();
renWin->AddRenderer(ren);
vtkSmartPointer<vtkRenderWindowInteractor> renWinInteractor = vtkSmartPointer<vtkRenderWindowInteractor>::New();
renWinInteractor->SetRenderWindow(renWin);
ren->AddActor(meshActor);
renWin->SetFullScreen(1);
/*
//ステレオを使いたい場合はここを弄る
renWin->SetStereoCapableWindow(1);
renWin->StereoRenderOn();
renWin->SetStereoTypeToAnaglyph();
*/
//カメラの設定も適当。リアルに作る場合はベースライン，焦点距離等を元に計算する
vtkSmartPointer<vtkCamera> cam = vtkSmartPointer<vtkCamera>::New();
cam->SetViewUp(0,1,0);
cam->SetPosition(0,1,0);
cam->SetFocalPoint(0,0,10);
cam->ComputeViewPlaneNormal();
ren->SetActiveCamera(cam);
ren->ResetCamera();
renWin->Render();
renWinInteractor->Initialize();
vtkSmartPointer<vtkTimerCallback> cb = vtkSmartPointer<vtkTimerCallback>::New();
renWinInteractor->AddObserver(vtkCommand::TimerEvent, cb);
//waitKey()は使わない。VTKのタイマーで制御する
int timerid = renWinInteractor->CreateRepeatingTimer(33);
renWinInteractor->Start();
return 0;
} /*
* PointCloud.cpp
*
* Created on: 2011/06/21
* Author: Eiichiro Momma
* Libraries:
* libvtkFiltering, libvtkRendering, libvtkCommon,
* libopencv_core, libopencv_imgproc, libopencv_highgui
*/
//#include <opencv2/core/core.hpp>
//#include <opencv2/highgui/highgui.hpp>
//#include <opencv2/imgproc/imgproc.hpp>
#include <OpenNI.h>
#include <vtkType.h>
#include <vtkPoints.h>
#include <vtkPointData.h>
#include <vtkPolyData.h>
#include <vtkPolyDataMapper.h>
#include <vtkUnsignedCharArray.h>
#include <vtkCellArray.h>
#include <vtkSmartPointer.h>
#include <vtkActor.h>
#include <vtkRenderer.h>
#include <vtkRenderWindow.h>
#include <vtkRenderWindowInteractor.h>
#include <vtkCommand.h>
#include <vtkCallbackCommand.h>
#include <vtkCamera.h>
//using namespace cv;
using namespace std;
enum Modes{
ColorImage,
DepthImage,
DepthVert,
PointCloudRendering
};
Modes dspMode = ColorImage;
void KeypressCallbackFunction(vtkObject *caller, unsigned long eventid, void *vtkNotUsed(clientData), void *vtkNotUsed(callData))
{
vtkRenderWindowInteractor *iren = static_cast<vtkRenderWindowInteractor*>(caller);
char key = iren->GetKeyCode();
switch(key)
{
case 'c':
dspMode = ColorImage;
break;
case 'd':
dspMode = DepthImage;
break;
case 'v':
dspMode = DepthVert;
break;
case 'f':
dspMode = PointCloudRendering;
break;
default:
break;
}
}
//更新のためグローバル
//VideoCapture capture;
//Mat depthMap;
//Mat bgrImage;
//Mat validMask;
openni::VideoStream colorStream;
openni::VideoStream depthStream;
vector <openni::VideoStream*> streams;
vtkSmartPointer<vtkPolyData> polyData;
void updatePolyData(void)
{
//capture.grab();
//std::vector<Mat> planes;
/*
capture.retrieve(depthMap, CV_CAP_OPENNI_DEPTH_MAP);
capture.retrieve(bgrImage, CV_CAP_OPENNI_BGR_IMAGE);
capture.retrieve(validMask, CV_CAP_OPENNI_VALID_DEPTH_MASK);
*/
//int changedIndex;
//openni::OpenNI(oniWaitForAnyStream(&streams[0], streams.size(), &changedIndex);
openni::VideoFrameRef depthFrame;
openni::VideoFrameRef colorFrame;
depthStream.readFrame(&depthFrame);
while(!depthFrame.isValid()){
depthStream.readFrame(&depthFrame);
}
colorStream.readFrame(&colorFrame);
while(!colorFrame.isValid()){
colorStream.readFrame(&colorFrame);
}
unsigned short * depthI = (unsigned short*)depthFrame.getData();
cout << depthFrame.getDataSize()<< ",";
cout << depthFrame.getVideoMode().getPixelFormat() << ",";
cout << depthFrame.getWidth() << ",";
cout << depthFrame.getHeight() << endl;
unsigned char* colorI = (unsigned char*)colorFrame.getData();
cout << colorFrame.getDataSize()<< ",";
cout << colorFrame.getVideoMode().getPixelFormat() << ",";
cout << colorFrame.getWidth() << ",";
cout << colorFrame.getHeight() << endl;
/*
flip(depthMap, depthMap, 0);
flip(bgrImage, bgrImage, 0);
flip(validMask, validMask, 0);
*/
//Point Cloudの実体
vtkSmartPointer<vtkPoints> points = vtkSmartPointer<vtkPoints>::New();
//Mat planes[] = {Mat::zeros(bgrImage.size(), CV_8UC1),Mat::zeros(bgrImage.size(), CV_8UC1),Mat::zeros(bgrImage.size(), CV_8UC1)};
//vector<Mat> planes;
//split(bgrImage, planes);
//色情報用
vtkSmartPointer<vtkUnsignedCharArray> colors = vtkSmartPointer<vtkUnsignedCharArray>::New();
colors->SetName("Colors");
colors->SetNumberOfComponents(3);
//CellArrayで構成する。Sphereで作るとメモリを食い潰す
vtkSmartPointer<vtkCellArray> cellArray = vtkSmartPointer<vtkCellArray>::New();
int offset_x = -colorFrame.getWidth()/2;
int offset_y = -colorFrame.getHeight()/2;
float fx = 580;
float fy = 580;
bool isXtion=false;
for (int y=0; y< colorFrame.getHeight(); y++)
{
for (int x=0; x< colorFrame.getWidth(); x++)
{
unsigned short dp = ((unsigned short*)(depthI+ y*depthFrame.getWidth()))[x];
if (depthFrame.getWidth() != colorFrame.getWidth()){
isXtion = true;
}
//uchar isValid = validMask.at<uchar>(y,x);
//ValidPixel以外はz=4096に色を黒にして置く。
//Pointの配置は適当。見た目重視で0.5倍してる。ちゃんとやりたい場合はパラメータを拾う
vtkIdType id;
if (dspMode == ColorImage || dspMode == DepthImage)
{
id = points->InsertNextPoint(x, y, 1000 );
}
else
{
float xx = dp*(x+offset_x)*2./fx;
float yy = dp*(y+offset_y)*2./fy;
id = points->InsertNextPoint(xx, yy, dp!=0? dp : 4096 );
}
cellArray->InsertNextCell(1);
cellArray->InsertCellPoint(id);
if (dspMode == ColorImage)
{
colors->InsertNextTuple3(
((unsigned char*)(colorI+3*y*colorFrame.getWidth()))[3*x + 0],
((unsigned char*)(colorI+3*y*colorFrame.getWidth()))[3*x + 1],
((unsigned char*)(colorI+3*y*colorFrame.getWidth()))[3*x + 2]);
// colorI[x+y*colorFrame.getWidth()*3],0,0);
// colorI[x+1 +y*colorFrame.getVideoMode().getResolutionX()*3],
// colorI[x+ +y*colorFrame.getVideoMode().getResolutionX()*3]);
}
else if (dspMode == DepthImage)
{
colors->InsertNextTuple3(255-255*dp/4096, 255-255*dp/4096, 255-255*dp/4096);
}
else if (dp!=0)
{
if (dspMode == PointCloudRendering)
{
colors->InsertNextTuple3(
((unsigned char*)(colorI+3*y*colorFrame.getWidth()))[3*x + 0],
((unsigned char*)(colorI+3*y*colorFrame.getWidth()))[3*x + 1],
((unsigned char*)(colorI+3*y*colorFrame.getWidth()))[3*x + 2]);
}
else
{
colors->InsertNextTuple3(255,255,255);
}
}
else
{
colors->InsertNextTuple3(0,0,0);
}
}
}
polyData->SetPoints(points);
polyData->SetVerts(cellArray);
polyData->GetPointData()->SetScalars(colors);
polyData->Modified();
polyData->Update();
}
class vtkTimerCallback:public vtkCommand
{
public:
static vtkTimerCallback *New()
{
vtkTimerCallback *cb = new vtkTimerCallback;
cb->TimerCount=0;
return cb;
}
virtual void Execute(vtkObject *caller, unsigned long eventId, void *vtkNotUsed(callData))
{
if (vtkCommand::TimerEvent == eventId)
{
++this->TimerCount;
//PolyDataを初期化して再描画
//ループ中の描画はここでやる
polyData->Initialize();
updatePolyData();
((vtkRenderWindowInteractor*)caller)->GetRenderWindow()->StereoUpdate();
((vtkRenderWindowInteractor*)caller)->GetRenderWindow()->Render();
}
}
private:
int TimerCount;
};
int main(void)
{
openni::Device device;
openni::Status ret = openni::OpenNI::initialize();
printf("After initialization:¥n%s¥n", openni::OpenNI::getExtendedError());
ret = device.open(openni::ANY_DEVICE);
if (ret!=openni::STATUS_OK){
cout<< " cant Open OpenNI" <<endl;
openni::OpenNI::shutdown();
return 1;
}
colorStream.create(device, openni::SensorType::SENSOR_COLOR);
colorStream.start();
colorStream.setMirroringEnabled(false);
depthStream.create(device, openni::SensorType::SENSOR_DEPTH);
depthStream.start();
depthStream.setMirroringEnabled(false);
cout << "color:" << colorStream.getVideoMode().getResolutionX() << endl;
cout << "color:" << colorStream.getVideoMode().getPixelFormat() << endl;
cout << "depth:" << depthStream.getVideoMode().getResolutionX() << endl;
streams.push_back(&colorStream);
streams.push_back(&depthStream);
/*
capture = VideoCapture(CV_CAP_OPENNI);
if (!capture.isOpened())
{
cout << "can't open kinect!" << endl;
return -1;
}
capture.set(CV_CAP_OPENNI_IMAGE_GENERATOR_OUTPUT_MODE, CV_CAP_OPENNI_VGA_30HZ);
*/
polyData = vtkSmartPointer<vtkPolyData>::New();
updatePolyData();
//VTKのお約束
vtkSmartPointer<vtkPolyDataMapper> mapMesh = vtkSmartPointer<vtkPolyDataMapper>::New();
mapMesh->SetInput(polyData);
vtkSmartPointer<vtkActor> meshActor = vtkSmartPointer<vtkActor>::New();
meshActor->SetMapper(mapMesh);
vtkSmartPointer<vtkRenderer> ren = vtkSmartPointer<vtkRenderer>::New();
vtkSmartPointer<vtkRenderWindow> renWin = vtkSmartPointer<vtkRenderWindow>::New();
renWin->AddRenderer(ren);
vtkSmartPointer<vtkRenderWindowInteractor> renWinInteractor = vtkSmartPointer<vtkRenderWindowInteractor>::New();
renWinInteractor->SetRenderWindow(renWin);
ren->AddActor(meshActor);
renWin->SetFullScreen(1);
renWin->SetCurrentCursor(VTK_CURSOR_CROSSHAIR);
//ステレオを使いたい場合はここを弄る
renWin->SetStereoCapableWindow(1);
renWin->StereoRenderOn();
// renWin->SetStereoTypeToAnaglyph();
renWin->SetStereoTypeToCrystalEyes();
//カメラの設定も適当。リアルに作る場合はベースライン，焦点距離等を元に計算する
vtkSmartPointer<vtkCamera> cam = vtkSmartPointer<vtkCamera>::New();
cam->SetViewUp(0,1,0);
cam->SetPosition(0,0,-2000);
cam->SetFocalPoint(0,0,-1500);
cam->ComputeViewPlaneNormal();
ren->SetActiveCamera(cam);
ren->ResetCamera();
ren->ResetCameraClippingRange(-320,320,-240,240,0,4096);
renWin->Render();
renWinInteractor->Initialize();
vtkSmartPointer<vtkTimerCallback> cb = vtkSmartPointer<vtkTimerCallback>::New();
renWinInteractor->AddObserver(vtkCommand::TimerEvent, cb);
vtkSmartPointer<vtkCallbackCommand> keypressCallback = vtkSmartPointer<vtkCallbackCommand>::New();
keypressCallback->SetCallback( KeypressCallbackFunction );
renWinInteractor->AddObserver( vtkCommand::KeyPressEvent, keypressCallback );
//waitKey()は使わない。VTKのタイマーで制御する
int timerid = renWinInteractor->CreateRepeatingTimer(8);
renWinInteractor->Start();
openni::OpenNI::shutdown();
return 0;
}