NtKinect: Kinect V2 C++ Programming with OpenCV on Windows10

NtKinect: Kinect V2 で BallFall ゲーム


2016.07.22: created by
Japanese English
目次へ

前提として理解しておくべき知識


BallFallゲーム

仮想的に上から降ってきたボールを、骨格で跳ね返して下に落ちないようにするゲームを作ってみましょう。 説明を簡単にするため点数の処理などはしていません。


プログラム作成の手順

  • NtKinect: Kinect V2 で骨格を認識する」 の Visual Studio 2015 のプロジェクト KinectV2_sieleton.zipを用いて作成します。
  • Ball.h をプロジェクトに追加します。
  • プロジェクトのソースを置くフォルダに Ball.h をコピーしてから、プロジェクトに加えます。

    Ball.h
    #pragma once
    #include <iostream>
    #include <sstream>
    #include <opencv2/opencv.hpp>
    using namespace std;
    
    class Ball
    {
    #define EPS 1.0e-6
     private:
      cv::Vec3f pos;
      cv::Vec3f vel;
      cv::Vec3f accel;
      double r;
     public:
     Ball() : pos(0, 0, 0), vel(0, 0, 0), accel(0, 0, 0), r(1.0) {}
      Ball(cv::Vec3f &pos, cv::Vec3f &v, cv::Vec3f &accel, double r) {
        this->pos = pos;
        this->vel = v;
        this->accel = accel;
        this->r = r;
      }
      ~Ball() {}
      void setPos(cv::Vec3f &pos) { this->pos = pos; }
      cv::Vec3f getPos() { return pos; }
      void setV(cv::Vec3f &v) { this->vel = v; }
      cv::Vec3f getV() { return vel; }
      void setAccel(cv::Vec3f &a) { accel = a; }
      cv::Vec3f getAccel() { return accel; }
      void setR(double r) { this->r = r; }
      double getR() { return r; }
      void step() {
        vel += accel;
        pos += vel;
      }
      bool bounce(pair<cv::Vec3f, cv::Vec3f> &seg) {
        cv::Vec3f nearPt = nearest(seg, pos);
        double len = cv::norm(pos, nearPt);
    
        cv::Vec3f oldPos = pos - vel;
        pair<cv::Vec3f, cv::Vec3f> trail(oldPos, pos);
        pair<cv::Vec3f, cv::Vec3f> pr = nearest(seg, trail);
        double len2 = cv::norm(pr.first, pr.second);
    
        if (len2 < EPS) {
          cv::Vec3f pt = oldPos - pr.second;
          cv::Vec3f par = seg.second - seg.first;
          cv::Vec3f normal = par.cross(cv::Vec3f(0, 0, 1));
          normal /= cv::norm(normal);
          double cs = pt.dot(normal);
          if (cs < 0){
    	normal *= -1.0; cs *= -1.0;
          }
          vel = 2 * cs * normal - pt;
          pos += vel;
          return true;
        }
        else if (len < r) {
          reflect(seg, nearPt);
          return true;
        }
        return false;
      }
      bool bounce(Ball &ball) {
        cv::Vec3f normal = pos - ball.pos;
        double len = cv::norm(normal);
        if (len >= r + ball.r) return false;
        if (len < 1.0) {
          pos[0] = -1;
          normal = pos - ball.pos;
          len = cv::norm(normal);
        }
        normal /= len;
        cv::Vec3f u = -vel;
        double cs = normal.dot(u);
        if (cs > 0) vel = 2 * cs * normal - u;
        pos += (r + ball.r - len) / 2 * normal;
    
        normal *= -1.0;
        u = -ball.vel;
        cs = normal.dot(u);
        if (cs > 0) ball.vel = 2 * cs * normal - u;
        ball.pos += (r + ball.r - len) / 2 * normal;
    
        return true;
      }
      pair<cv::Vec3f, cv::Vec3f> nearest(pair<cv::Vec3f, cv::Vec3f> &seg1, pair<cv::Vec3f, cv::Vec3f> &seg2) {
        pair<cv::Vec3f, cv::Vec3f> ans;
        cv::Vec3f ab = seg1.second - seg1.first, cd = seg2.second - seg2.first, ac = seg2.first - seg1.first;
        cv::Vec3f v = ab.cross(cd);
        if (v[2] > EPS) {
          cv::Vec3f u = ac.cross(cd), w = ac.cross(ab);
          double t = u[2] / v[2], s = w[2] / v[2];
          if (t >= 0 - EPS && t <= 1.0 + EPS && s >= 0 - EPS && s <= 1.0 + EPS) {
    	if (abs(t) < EPS) { ans.first = ans.second = seg1.first; }
    	else if (abs(t - 1) < EPS) { ans.first = ans.second = seg1.second; }
    	else if (abs(s) < EPS) { ans.first = ans.second = seg2.first; }
    	else if (abs(s - 1) < EPS) { ans.first = ans.second = seg2.second; }
    	else {
    	  ab[2] = 0.0;
    	  ans.first = ans.second = ab*t + seg1.first;
    	}
    	return ans;
          }
        }
        ans.first = seg1.first;
        ans.second = nearest(seg2, seg1.first);
        double minlen = cv::norm(ans.second, seg1.first);
        v = nearest(seg2, seg1.second);
        double len = cv::norm(v, seg1.second);
        if (len < minlen) { ans.first = seg1.first; ans.second = v; minlen = len; }
        v = nearest(seg1, seg2.first);
        len = cv::norm(v, seg2.first);
        if (len < minlen) { ans.first = v; ans.second = seg2.first; minlen = len; }
        v = nearest(seg1, seg2.second);
        len = cv::norm(v, seg2.second);
        if (len < minlen) { ans.first = v; ans.second = seg2.second; minlen = len; }
        return ans;
      }
    
      double minDistance(pair<cv::Vec3f, cv::Vec3f> &seg, cv::Vec3f &pt) {
        cv::Vec3f nearPt = nearest(seg, pt);
        return cv::norm(nearPt, pt);
      }
      cv::Vec3f nearest(pair<cv::Vec3f, cv::Vec3f> &seg, cv::Vec3f &pt) {
        cv::Vec3f p(seg.second - seg.first), q(pt - seg.first);
        double t = p.dot(q) / p.dot(p);
        if (t <= 0.0 + EPS) return seg.first;
        if (t >= 1.0 - EPS) return seg.second;
        p = p*t + seg.first;
        return p;
      }
      void reflect(pair<cv::Vec3f, cv::Vec3f> &seg, cv::Vec3f &nearPt) {
        cv::Vec3f normal = pos - nearPt;
        double len = cv::norm(normal);
        if (len < 1.0) { // if ball's center is on the segment, add 2 to ball.y
          pos[1] -= 2;
          normal = pos - nearPt;
          len = cv::norm(normal);
        }
        normal /= len;
        if (len<r) pos += (r - len) * normal;
        if (normal.dot(vel) > 0) { // same direction
          vel += 5 * normal;
        }
        else {
          cv::Vec3f u = -vel;
          vel = 2 * normal.dot(u) * normal - u; // reflect
        }
      }
      bool in(cv::Mat &image) {
        return pos[0] >= 0 && pos[0] < image.cols && pos[1] >= -30 && pos[1] < image.rows;
      }
      void draw(cv::Mat &image, int id = -1) {
        cv::Point pt((int)pos[0], (int)pos[1]);
        cv::circle(image, pt, r, cv::Scalar(255, 0, 255), -1);
        stringstream ss; ss << id;
        if (id >= 0) cv::putText(image, ss.str(), pt, cv::FONT_HERSHEY_PLAIN, 1.0, cv::Scalar(0, 0, 255));
      }
      string tostring() {
        stringstream ss;
        ss << "ball[" << pos << " " << vel << " " << accel << " " << r << "]";
        return ss.str();
      }
    };
    
    
  • BallFall.h をプロジェクトに追加します。
  • プロジェクトのソースを置くフォルダに BallFall.h をコピーしてから、プロジェクトに加えます。

    BallFall.h
    #pragma once
    #include <time.h>
    #include <opencv2/opencv.hpp>
    #include "Ball.h"
    using namespace std;
    
    class BallFall
    {
    #define N_BALL	8
    
     private:
      cv::Mat image;
      vector<Ball> balls;
      cv::Vec3f mouse;
     public:
      BallFall() {
        srand((unsigned int)time(NULL));
      }
      ~BallFall() {}
      void start(int w = 640, int h = 480) {
        image = cv::Mat(h, w, CV_8UC3);
        for (int i = 0; i < N_BALL; i++) {
          Ball ball;
          randomBall(ball);
          balls.push_back(ball);
        }
        cv::namedWindow("ball fall");
        mouse[0] = -1;
      }
      void randomBall(Ball &b) {
        cv::Vec3f pos, v, accel;
        pos[0] = rand() % image.cols;
        pos[1] = 0; pos[2] = 0;
        v[0] = rand() % 4 - 2; v[1] = rand() % 1; v[2] = 0;
        accel[0] = 0; accel[1] = 1.0; accel[2] = 0;
        b.setPos(pos); b.setV(v); b.setAccel(accel); b.setR(30);
      }
      void step(vector<pair<cv::Vec3f, cv::Vec3f>>& segments) {
        for (int i = 0; i < balls.size(); i++) balls[i].step();
        for (int i = 0; i < balls.size(); i++) {
          for (int j = i + 1; j < balls.size(); j++)
    	balls[i].bounce(balls[j]);
        }
        for (int i = 0; i < balls.size(); i++) {
          for (int j = 0; j < segments.size(); j++)
    	balls[i].bounce(segments[j]);
          if (!balls[i].in(image)) randomBall(balls[i]);
        }
      }
      void draw(vector<pair<cv::Vec3f, cv::Vec3f>>& segments) {
        cv::rectangle(image, cv::Rect(0, 0, image.cols, image.rows), cv::Scalar(50, 50, 50), -1);
        draw(segments, image);
        cv::imshow("ball fall", image);
      }
    
      void draw(vector<pair<cv::Vec3f, cv::Vec3f>>& segments, cv::Mat& image) {
        for (int i = 0; i < balls.size(); i++) {
          balls[i].draw(image);
        }
        for (int i = 0; i < segments.size(); i++) {
          cv::Point p((int)segments[i].first[0], (int)segments[i].first[1]);
          cv::Point q((int)segments[i].second[0], (int)segments[i].second[1]);
          cv::line(image, p, q, cv::Scalar(255, 255, 0), 8);
        }
      }
    };
    
    
    
  • main.cppの内容を以下のように変更します。
  • main.cpp
    #include <iostream>
    #include <sstream>
    
    #include "NtKinect.h"
    #include "BallFall.h"
    
    const int segment[] = {
      JointType_SpineBase, JointType_SpineMid,           // spine
      JointType_SpineMid, JointType_SpineShoulder,
      JointType_SpineShoulder, JointType_Neck,           // head
      JointType_Neck, JointType_Head,
    
      JointType_SpineShoulder, JointType_ShoulderLeft,   // left hand
      JointType_ShoulderLeft, JointType_ElbowLeft,
      JointType_ElbowLeft, JointType_WristLeft,
      JointType_WristLeft, JointType_HandLeft,
      JointType_HandLeft, JointType_HandTipLeft,
      JointType_HandLeft, JointType_ThumbLeft,
    
      JointType_SpineShoulder, JointType_ShoulderRight,  // right hand
      JointType_ShoulderRight, JointType_ElbowRight,
      JointType_ElbowRight, JointType_WristRight,
      JointType_WristRight, JointType_HandRight,
      JointType_HandRight, JointType_HandTipRight,
      JointType_HandRight, JointType_ThumbRight,
      
      JointType_SpineBase,  JointType_HipLeft,           // left leg
      JointType_HipLeft,  JointType_KneeLeft,
      JointType_KneeLeft,  JointType_AnkleLeft,
      JointType_AnkleLeft,  JointType_FootLeft,
    
      JointType_SpineBase,  JointType_HipRight,          // right leg
      JointType_HipRight,  JointType_KneeRight,
      JointType_KneeRight,  JointType_AnkleRight,
      JointType_AnkleRight,  JointType_FootRight,
    };
    const int segmentSize = sizeof(segment) / sizeof(int);
    
    using namespace std;
    
    void doJob() {
      NtKinect kinect;
      BallFall bf;
      bool flag = false;
      kinect.setRGB();
      bf.start(kinect.rgbImage.cols, kinect.rgbImage.rows);
      vector<pair<cv::Vec3f, cv::Vec3f>> skel;
      while (1) {
        kinect.setRGB();
        kinect.setSkeleton();
        skel.clear();
        for (int i = 0; i < kinect.skeleton.size(); i++) {
          for (int j = 0; j < segmentSize/2; j++) {
    	Joint joint1 = kinect.skeleton[i][segment[j * 2]];
    	Joint joint2 = kinect.skeleton[i][segment[j * 2 + 1]];
    	if (joint1.TrackingState != TrackingState_NotTracked
    	    && joint2.TrackingState != TrackingState_NotTracked) {
    	  ColorSpacePoint cp;
    	  kinect.coordinateMapper->MapCameraPointToColorSpace(joint1.Position, &cp);
    	  cv::Vec3f p1 = cv::Vec3f(cp.X, cp.Y, 0.0f);
    	  kinect.coordinateMapper->MapCameraPointToColorSpace(joint2.Position, &cp);
    	  cv::Vec3f p2 = cv::Vec3f(cp.X, cp.Y, 0.0f);
    	  skel.push_back(pair<cv::Vec3f, cv::Vec3f>(p1,p2));
    	}
          }
        }
        bf.step(skel);
        cv::Mat image;
        if (flag) image = kinect.rgbImage.clone();
        else {
          image = cv::Mat(kinect.rgbImage.rows,kinect.rgbImage.cols,CV_8UC3);
          cv::rectangle(image,cv::Rect(0,0,image.cols,image.rows),cv::Scalar(0,0,0), -1);
        }
        bf.draw(skel, image);
        cv::imshow("ball fall", image);
        auto key = cv::waitKey(1);
        if (key == 'q') break;
        if (key == 'v') flag = !flag;
      }
    }
    
    int main(int argc, char** argv) {
      try {
        doJob();
      } catch (exception &ex) {
        cout << ex.what() << endl;
        string s;
        cin >> s;
      }
      return 0;
    }
    
    
  • プログラムを実行すると骨格が線画表示されて、ボールが降ってきて衝突すると跳ね返ります。"q"キーで終了します。"v"キーでRGB画像の上に重ねて描くかどうかが変わります。

  •    

  • サンプルのプロジェクトはこちら KinectV2_ballFall.zip
  • 上記のzipファイルには必ずしも最新の NtKinect.h が含まれていない場合があるので、 こちらから最新版をダウンロードして 差し替えてお使い下さい。



    http://nw.tsuda.ac.jp/