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