void updateHandPosition(PointCloud handPcl) {
int i, j;
int pointCount = barycenter(handPcl, &barycX, &barycY, &barycZ) ;
if (pointCount < MIN_HAND_POINT_NUMBER) {
return;
}
isOpenHand = openHand(handPcl);
double posCovarMat[3][3];
double posCovarMatEigVec[3][3];
double posCovarMatEigVal[3];
for (i=0; i<3; i++) {
for (j=0; j<3; j++) {
posCovarMat[i][j] = 0;
}
}
for(j=0; j<FREENECT_FRAME_H; j++) {
for(i=0; i<FREENECT_FRAME_W; i++) {
if (handPcl[j][i].valid) {
posCovarMat[0][0] += (handPcl[j][i].x - barycX)*(handPcl[j][i].x - barycX);
posCovarMat[0][1] += (handPcl[j][i].x - barycX)*(handPcl[j][i].y - barycY);
posCovarMat[0][2] += (handPcl[j][i].x - barycX)*(handPcl[j][i].z - barycZ);
posCovarMat[1][0] += (handPcl[j][i].y - barycY)*(handPcl[j][i].x - barycX);
posCovarMat[1][1] += (handPcl[j][i].y - barycY)*(handPcl[j][i].y - barycY);
posCovarMat[1][2] += (handPcl[j][i].y - barycY)*(handPcl[j][i].z - barycZ);
posCovarMat[2][0] += (handPcl[j][i].z - barycZ)*(handPcl[j][i].x - barycX);
posCovarMat[2][1] += (handPcl[j][i].z - barycZ)*(handPcl[j][i].y - barycY);
posCovarMat[2][2] += (handPcl[j][i].z - barycZ)*(handPcl[j][i].z - barycZ);
}
}
}
for (i=0; i<3; i++) {
for (j=0; j<3; j++) {
posCovarMat[i][j] = posCovarMat[i][j] / pointCount;
}
}
eigen_decomposition(posCovarMat, posCovarMatEigVec, posCovarMatEigVal);
// printMatrix("Covar Mat", posCovarMat, 3);
// printMatrix("Pos Eigen Vec", posCovarMatEigVec, 3);
majorAxeX = posCovarMatEigVec[1][2] > 0 ? posCovarMatEigVec[0][2] : - posCovarMatEigVec[0][2];
majorAxeY = posCovarMatEigVec[1][2] > 0 ? posCovarMatEigVec[1][2] : - posCovarMatEigVec[1][2];
majorAxeZ = posCovarMatEigVec[1][2] > 0 ? posCovarMatEigVec[2][2] : - posCovarMatEigVec[2][2];
normVecX = posCovarMatEigVec[2][0] > 0 ? posCovarMatEigVec[0][0] : - posCovarMatEigVec[0][0];
normVecY = posCovarMatEigVec[2][0] > 0 ? posCovarMatEigVec[1][0] : - posCovarMatEigVec[1][0];
normVecZ = posCovarMatEigVec[2][0] > 0 ? posCovarMatEigVec[2][0] : - posCovarMatEigVec[2][0];
//arduinoCmd();
//printf("Barycenter: X: %.4f Y: %.4f Z: %.4f\tDirection: X: %.4f Y: %.4f Z: %.4f\n", barycX, barycY, barycZ, normVecX, normVecY, normVecZ);
moveMouse();
}
void timerCB(const ros::TimerEvent&)
{
ROS_DEBUG_THROTTLE(1, "State: %i", (int)state_);
switch (state_)
{
case IDLE:
// Do nothing.
break;
case GO:
// Always transition to OPEN_HAND, send an open command if
// necessary.
if (!handOpened())
{
openHand();
}
state_ = OPEN_HAND;
break;
case OPEN_HAND:
if (handOpened())
{
// Transition to toward pose.
pubGoal(fixed_toward_pose_);
state_ = GO_TOWARD;
}
break;
case GO_TOWARD:
if (at(fixed_toward_pose_))
{
// Transition to target pose.
pubGoal(fixed_target_pose_);
state_ = LOWER;
}
break;
case LOWER:
if (at(fixed_target_pose_))
{
// Transition to hand closing.
closeHand();
state_ = CLOSE_HAND;
}
break;
case CLOSE_HAND:
if (!handOpened())
{
// Transition to lifted pose.
pubGoal(fixed_lifted_pose_);
state_ = LIFT;
}
break;
case LIFT:
if (at(fixed_lifted_pose_))
{
// Transition back to idle.
state_ = IDLE;
}
break;
default:
state_ = IDLE;
break;
};
}
