cvl-robot's diary

研究ノート メモメモ https://github.com/dotchang/

LeapMotion v2をopenFrameworksで動かす(その1)

LeapMotionがv2になって,指の関節まで推定できるようになり、とても良くなっているそうです。いつものようにwindows7のopenFrameworksで動かしてみましょう。

1.LeapMotionのセットアップ

まずは、普通にLeapMotionのドライバーと開発者向けSDKをインストールします。先に普通のドライバー入れた後、開発者向けSDKを入れるようにします。理由は良くわかりませんが、順序が逆だとopenFrameworksのexampleから正しく認識されないことがありました。

2.ofxLeapMotionのインストール

ofTheoさんのaddon ofxLeapMotionを使います。ただし、これはライブラリはv2に対応していますが、v1モデルを使用していてv2のデータ形式は扱いませんので、あとで改造します。正式対応するバージョンはof 0.8.1以降と書いてありますが、0.8.0でも動きました。

https://github.com/ofTheo/ofxLeapMotion

右側のDownload Zipボタンでソースコードをダウンロードして解凍し、フォルダ名の-masterを削除します。その後、openframeworksのaddonsフォルダの中に放り込みます。

exampleのフォルダの中のleapMotionExample.slnを開いてビルドします。

f:id:cvl-robot:20140716215518p:plain

 図.ofxLeapMotionの動作サンプル

 3.v2のデータ形式c++ SDKの確認

資料は文献[3]にあります。 Armや指の各関節Bornを扱えるようになっています。それぞれHandクラスにぶら下がっているようなので、データ取扱いの基準はhandsクラスになるようです。

Hands->Finger->Born

            ->Arm

 3.1 Arm

とりあえず、Armを扱えるようにしてみましょう。肘と手首の位置と、腕の幅と、行列から腕の向きも取れるようです。

class ofxLeapMotionSimpleHandに追加。

typedef struct{
  ofPoint elbow;
  ofPoint wrist;
  ofPoint dir;
  float width;
  ofMatrix4x4 transform;
}simpleArm;

simpleArm arm;

vector <ofxLeapMotionSimpleHand> ofxLeapMotion::getSimpleHands()の中に追加。

// Arm
Arm arm = leapHands[i].arm();
if(arm.isValid()){
  curHand.elbow = getMappedofPoint(arm.elbowPosition());
  curHand.wrist = getMappedofPoint(arm.wristPosition());
  curHand.dir = getofPoint(arm.direction());
  curHand.width = arm.width();

  Matrix basis = arm.basis();
  Vector xBasis = basis.xBasis;
  Vector yBasis = basis.yBasis;
  Vector zBasis = basis.zBasis;

  Vector armCenter = arm.elbowPosition() + (arm.wristPosition() - arm.elbowPosition()) * .5;
  ofPoint ac = getMappedofPoint(armCenter);
  Matrix transform = Matrix(xBasis, yBasis, zBasis, Vector(ac.x, ac.y, ac.z));
  transform.toArray4x4<float>(curHand.transform.getPtr());
}

 void ofxLeapMotionSimpleHand::debugDraw()に追加。

// Draw Arm
ofLine(arm.wrist, arm.elbow); // for Easy
ofPushMatrix();
ofMultMatrix(arm.transform);

//scale it to make it not a box
ofScale(arm.width, 1, (arm.elbow-arm.wrist).length());
#if (OF_VERSION_MAJOR == 0) && (OF_VERSION_MINOR < 8)
ofBox(0, 0, 0, 1);
#else
ofDrawBox(0, 0, 0, 1);
#endif
ofPopMatrix();

 線と箱で描画がずれている・・・

 描画がずれる原因は、ofxLeapMotionで保持している位置の情報は、getMappedofPoint()関数で画面に合わせてスケーリングされているため、で、ロボットを動かすためには実座標系をそのまま保持していてほしいので、多少の描画の狂いを仕方ない事として、getMappedofPoint関数をすべてgetofPoint()関数に置き換えます。

f:id:cvl-robot:20140717171450p:plain

表示位置がちょっと遠くなりましたが、正しくArmが描けていますね。挙動が謎だった赤い球も正しく表示されるようになりました。

 3.2 fingerとbone

同じ調子でfingerとboneも各関節ごとに扱えるようにしましょう。

ofxLeapMotion.h

class ofxLeapMotionSimpleHand_v2{

public:

typedef struct{
  ofMatrix4x4 basis;
  ofPoint center;
  ofPoint direction;
  bool isValid;
  float length;
  ofPoint nextJoint; // position
  ofPoint prevJoint;
  int32_t type;
  float width;
}simpleBone;

typedef struct{
  simpleBone bones[4];
  ofPoint direction;
  bool isExtended;
  bool isFinger;
  bool isTool;
  bool isValid;
  //ofPoint jointPosition;
  float length;
  ofPoint stabilizedTipPosition;
  float timeVisible;
  ofPoint tipPosition;
  ofPoint tipVelocity;
  float touchDistance;
  int32_t type;
  float width;

  int32_t id;
  ofPoint base;
}simpleFinger;

vector <simpleFinger> fingers;

ofMatrix4x4 basis;
float confidence;
ofPoint direction;
float grabStrength;
int32_t id;
bool isLeft;
bool isRight;
ofPoint palmNormal;
ofPoint palmPosition;
ofPoint palmVelocity;
float palmWidth;
float pinchStrength;

float rotationAngle;
ofPoint rotationAxis;
ofMatrix4x4 rotationMatrix;
float rotationProbability;

float scaleFactor;
float scaleProbability;

ofPoint sphereCenter;
float sphereRadius;

ofPoint stabilizedPalmPosition;
float timeVisible;

ofPoint translation;
float translationProbability;
ofPoint wristPosition;

typedef struct{
}simpleTool;

typedef struct{
  ofPoint elbow;
  ofPoint wrist;
  ofPoint dir;
  float width;
  ofMatrix4x4 transform;
}simpleArm;

simpleArm arm;

void debugDraw();
};

ofxLeapMotion.cpp

vector <ofxLeapMotionSimpleHand_v2> ofxLeapMotion::getSimpleHands_v2()

{
  vector <ofxLeapMotionSimpleHand_v2> simpleHands;
  vector <Hand> leapHands = getLeapHands();

  for(int i = 0; i < leapHands.size(); i++){
    ofxLeapMotionSimpleHand_v2 curHand;

  // Hand
  curHand.palmPosition = getofPoint(leapHands[i].palmPosition());
  curHand.palmNormal = getofPoint(leapHands[i].palmNormal());
  curHand.palmVelocity = getofPoint(leapHands[i].palmVelocity()); // more hand data - hand velocity
  curHand.palmWidth = leapHands[i].palmWidth();
  curHand.sphereRadius = leapHands[i].sphereRadius(); // more hand data - hand openness
  curHand.sphereCenter = getofPoint(leapHands[i].sphereCenter()); // more hand data - sphere center

{
  Matrix basis = leapHands[i].basis();
  Vector xBasis = basis.xBasis;
  Vector yBasis = basis.yBasis;
  Vector zBasis = basis.zBasis;
  Matrix transform = Matrix(xBasis, yBasis, zBasis, leapHands[i].palmPosition());
  transform.toArray4x4<float>(curHand.basis.getPtr());
}
  curHand.confidence = leapHands[i].confidence();
  curHand.direction = getofPoint(leapHands[i].direction());
  curHand.grabStrength = leapHands[i].grabStrength();
  curHand.id = leapHands[i].id();
  curHand.isLeft = leapHands[i].isLeft();
  curHand.isRight = leapHands[i].isRight();
  curHand.pinchStrength = leapHands[i].pinchStrength();
  curHand.stabilizedPalmPosition = getofPoint(leapHands[i].stabilizedPalmPosition());
  curHand.timeVisible = leapHands[i].timeVisible();
  curHand.wristPosition = getofPoint(leapHands[i].wristPosition());

  // Finger
  for(int j = 0; j < leapHands[i].fingers().count(); j++){
    const Finger & finger = hands[i].fingers()[j];

    Leap::Vector basePosition = -finger.direction() * finger.length(); // calculate finger base position
    basePosition += finger.tipPosition(); // calculate finger base position

    ofxLeapMotionSimpleHand_v2::simpleFinger f;
    f.tipPosition = getofPoint(finger.tipPosition());
    f.tipVelocity = getofPoint(finger.tipVelocity());
    f.base = getofPoint(basePosition);
    f.id = finger.id();

    f.isExtended = finger.isExtended();
    f.isFinger = finger.isFinger();
    f.isTool = finger.isTool();
    f.isValid = finger.isValid();

    f.length = finger.length();
    f.stabilizedTipPosition = getofPoint(finger.stabilizedTipPosition());
    f.timeVisible = finger.timeVisible();
    f.touchDistance = finger.touchDistance();
    f.type = finger.type();
    f.width = finger.width();

    // Bone
   Bone bone;
   Bone::Type boneType;
    for(int b = 0; b < 4; b++)
    {
       boneType = static_cast<Bone::Type>(b);
       bone = finger.bone(boneType);
       //std::cout << "Bone: " << bone << std::endl;

       f.bones[b].center = getofPoint(bone.center());
       f.bones[b].direction = getofPoint(bone.direction());
       f.bones[b].isValid = bone.isValid();
       f.bones[b].length = bone.length();
       f.bones[b].nextJoint = getofPoint(bone.nextJoint());
       f.bones[b].prevJoint = getofPoint(bone.prevJoint());
       f.bones[b].type = bone.type();
       f.bones[b].width = bone.width();
       {
          Matrix basis = bone.basis();
          Vector xBasis = basis.xBasis;
          Vector yBasis = basis.yBasis;
          Vector zBasis = basis.zBasis;
          Matrix transform = Matrix(xBasis, yBasis, zBasis, bone.center());
          transform.toArray4x4<float>(f.bones[b].basis.getPtr());
        }
     }

     curHand.fingers.push_back(f);
  }

  // Arm
  Arm arm = leapHands[i].arm();
  if(arm.isValid()){
     curHand.arm.elbow = getofPoint(arm.elbowPosition());
     curHand.arm.wrist = getofPoint(arm.wristPosition());
     curHand.arm.dir = getofPoint(arm.direction());
     curHand.arm.width = arm.width();

     Matrix basis = arm.basis();
     Vector xBasis = basis.xBasis;
     Vector yBasis = basis.yBasis;
     Vector zBasis = basis.zBasis;

     Vector armCenter = arm.elbowPosition() + (arm.wristPosition() - arm.elbowPosition()) * .5;
     Matrix transform = Matrix(xBasis, yBasis, zBasis, armCenter);
     transform.toArray4x4<float>(curHand.arm.transform.getPtr());
    }

    simpleHands.push_back(curHand);
  }

return simpleHands;
}

 ofxLeapMotion.cp

// ofxLeapMotionSimpleHand
//--------------------------------------------------------------
void ofxLeapMotionSimpleHand_v2::debugDraw()

{
  ofPushStyle();

  ofSetColor(190);
  ofSetLineWidth(2);

  ofEnableLighting();
  ofPushMatrix();
  ofMultMatrix(basis);

  //scale it to make it not a box
  ofScale(palmWidth, 1.0f, 60.0f);
#if (OF_VERSION_MAJOR == 0) && (OF_VERSION_MINOR < 8)
  ofBox(0, 0, 0, 60);
#else
  ofDrawBox(0, 0, 0, 1);
#endif
  ofPopMatrix();

  // sphere - hand openness debug draw
  ofSetColor(200, 0, 0, 80);
  //ofDrawSphere(sphereCenter, sphereRadius);

  for(int i = 0; i < fingers.size(); i++){
    //ofDrawArrow(handPos, fingers[i].pos, 10);

    // fingers base debug draw
    ofSetColor(190);
    //ofLine(palmPosition, fingers[i].base);
    //ofDrawBox(fingers[i].base, 10);
    //ofLine(fingers[i].base, fingers[i].tipPosition);

    ofSetColor(0, 200, 0);
    ofDrawSphere(fingers[i].tipPosition, fingers[i].width/2);

    for(int b = 0; b < 4; b++)
    {
      ofLine(fingers[i].bones[b].prevJoint, fingers[i].bones[b].nextJoint);
      ofPushMatrix();
      ofMultMatrix(fingers[i].bones[b].basis);
      //scale it to make it not a box
      ofScale(fingers[i].bones[b].width, 1.0f, fingers[i].bones[b].length);
#if (OF_VERSION_MAJOR == 0) && (OF_VERSION_MINOR < 8)
      ofBox(0, 0, 0, 1);
#else
      ofDrawBox(0, 0, 0, 1);
#endif
      ofPopMatrix();
    }

  }

  ofSetColor(220, 220, 0);
  for(int i = 0; i < fingers.size(); i++){
    //ofDrawArrow(fingers[i].tipPosition + fingers[i].tipVelocity/20, fingers[i].tipPosition + fingers[i].tipVelocity/10, 10);
  }

  // Draw Arm
  if(arm.isValid){
    ofLine(arm.wrist, arm.elbow); // for Easy
    ofPushMatrix();
    ofMultMatrix(arm.transform);

    //scale it to make it not a box
    ofScale(arm.width, 1, (arm.elbow-arm.wrist).length());
#if (OF_VERSION_MAJOR == 0) && (OF_VERSION_MINOR < 8)
    ofBox(0, 0, 0, 1);
#else
    ofDrawBox(0, 0, 0, 1);
#endif
    ofPopMatrix();
  }

  ofDisableLighting();

  ofPopStyle();
}

f:id:cvl-robot:20140718124629g:plain

 

ちょっと長くなったので後で、gitに上げます。

 https://github.com/dotchang/ofxLeapMotion_v2Test

 

GIFアニメの制作に使っているのは、次のソフトウェアです。

BBRecoder: http://www.bbflashback.jp/download

Giam: http://www.forest.impress.co.jp/library/software/giam/

 

[1]Leap Motion Controllerを使って始めましょう: https://www.leapmotion.com/setup

[2]開発者: https://developer.leapmotion.com/

[3]C++ SDK Documentation: https://developer.leapmotion.com/documentation/skeletal/cpp/index.html