Kinect PointCloud - eiichiromomma/CVMLAB GitHub Wiki

(Kinect) Point Cloud

KinectとVTKでPoint Cloud Rendering

Kinectの距離情報と可視化ライブラリのVTKを使ってPoint Cloud Rendering(キャプチャはOpenCVかOpenNI)

前提条件

  • OpenCVまたはOpenNIでKinectが使える状態
  • VTKの開発環境がインストール済み

Windowsでの注意点

ライブラリの参照エラーが出る場合はエラーメッセージで不足している関数名を参考に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出るのでそれなりに速い。

ソース(OpenCVとVTK)

    /*
     * 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;
    }

OpenNIとVTK

    /*
      * 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;
    }
⚠️ **GitHub.com Fallback** ⚠️