void getJointPositions(Mat imgOrg, Arm *arm_left, Arm *arm_right, Chest *chest)
{
// reset found flag from all joints and both arms
arm_left->resetJ1Found();
arm_left->resetJ2Found();
arm_left->resetJ3Found();
arm_left->resetArmFound();
arm_right->resetJ1Found();
arm_right->resetJ2Found();
arm_right->resetJ3Found();
arm_right->resetArmFound();
// Create array of camera parameters
CameraParameters TheCameraParameters;
// set camera parameters
Mat dist(1,5,CV_32FC1);
dist.at<float>(0,0) = -0.0648763971625288;
dist.at<float>(0,1) = 0.0612520196884308;
dist.at<float>(0,2) = 0.0038281538281731;
dist.at<float>(0,3) = -0.00551104078371959;
dist.at<float>(0,4) = 0.000000;
Mat cameraP(3,3,CV_32FC1);
cameraP.at<float>(0,0) = 558.570339530768;
cameraP.at<float>(0,1) = 0.000000;
cameraP.at<float>(0,2) = 308.885375457296;
cameraP.at<float>(1,0) = 0.000000;
cameraP.at<float>(1,1) = 556.122943034837;
cameraP.at<float>(1,2) = 247.600724811385;
cameraP.at<float>(2,0) = 0.000000;
cameraP.at<float>(2,1) = 0.000000;
cameraP.at<float>(2,2) = 1.000000;
TheCameraParameters.setParams(cameraP, dist, CAMERA_RESOLUTION);
TheCameraParameters.resize(CAMERA_RESOLUTION);
// create vectors for joints
Vector3d j1Left, j2Left, j3Left;
Vector3d j1Right, j2Right, j3Right;
// create objects for marker
MarkerDetector MDetector;
vector<Marker> Markers;
// set marker settings
MDetector.setWarpSize(100);
MDetector.enableLockedCornersMethod(true);
MDetector.setMinMaxSize(0.01, 0.5);
// convert image to gray
Mat imgGray = imgOrg.clone();
cvtColor(imgGray, imgGray, CV_BGR2GRAY);
// detect all markers in the picture
MDetector.detect(imgGray, Markers, TheCameraParameters);
// go through all found markers
for(int i = 0; i < Markers.size(); i++)
{
Markers[i].calculateExtrinsics(0.09, TheCameraParameters);
Markers[i].draw(imgOrg, Scalar(0,0,255), 2);
// get abs position in picture for every joint
switch(Markers[i].id)
{
case SHOULDER_LEFT:
// switch x and y axis
j1Left(0) = Markers[i].Tvec.at<float>(1, 0);
j1Left(1) = Markers[i].Tvec.at<float>(0, 0);
j1Left(2) = Markers[i].Tvec.at<float>(2, 0);
arm_left->setJ1Found();
if(COUT_JOINT_ABS_POS == ON)
{
cout << "abs pos SHOULDER_LEFT: " << j1Left(0) << "\t" << j1Left(1) << "\t" << j1Left(2) << endl;
}
break;
case ELBOW_LEFT:
// switch x and y axis
j2Left(0) = Markers[i].Tvec.at<float>(1, 0);
j2Left(1) = Markers[i].Tvec.at<float>(0, 0);
j2Left(2) = Markers[i].Tvec.at<float>(2, 0);
arm_left->setJ2Found();
if(COUT_JOINT_ABS_POS == ON)
{
cout << "abs pos ELBOW_LEFT: " << j2Left(0) << "\t" << j2Left(1) << "\t" << j2Left(2) << endl;
}
break;
case WRIST_LEFT:
// switch x and y axis
j3Left(0) = Markers[i].Tvec.at<float>(1, 0);
j3Left(1) = Markers[i].Tvec.at<float>(0, 0);
j3Left(2) = Markers[i].Tvec.at<float>(2, 0);
arm_left->setJ3Found();
if(COUT_JOINT_ABS_POS == ON)
{
cout << "abs pos WRIST_LEFT: " << j3Left(0) << "\t" << j3Left(1) << "\t" << j3Left(2) << endl;
}
break;
case SHOULDER_RIGHT:
// switch x and y axis
j1Right(0) = Markers[i].Tvec.at<float>(1, 0);
j1Right(1) = Markers[i].Tvec.at<float>(0, 0);
j1Right(2) = Markers[i].Tvec.at<float>(2, 0);
arm_right->setJ1Found();
if(COUT_JOINT_ABS_POS == ON)
{
cout << "abs pos SHOULDER_RIGHT: " << j1Right(0) << "\t" << j1Right(1) << "\t" << j1Right(2) << endl;
}
break;
case ELBOW_RIGHT:
// switch x and y axis
j2Right(0) = Markers[i].Tvec.at<float>(1, 0);
j2Right(1) = Markers[i].Tvec.at<float>(0, 0);
j2Right(2) = Markers[i].Tvec.at<float>(2, 0);
arm_right->setJ2Found();
if(COUT_JOINT_ABS_POS == ON)
{
cout << "abs pos ELBOW_RIGHT: " << j2Right(0) << "\t" << j2Right(1) << "\t" << j2Right(2) << endl;
}
break;
case WRIST_RIGHT:
// switch x and y axis
j3Right(0) = Markers[i].Tvec.at<float>(1, 0);
j3Right(1) = Markers[i].Tvec.at<float>(0, 0);
j3Right(2) = Markers[i].Tvec.at<float>(2, 0);
arm_right->setJ3Found();
if(COUT_JOINT_ABS_POS == ON)
{
cout << "abs pos WRIST_RIGHT: " << j3Right(0) << "\t" << j3Right(1) << "\t" << j3Right(2) << endl;
}
break;
}
}
// Display camera imape with detected marker
if(SHOW_ARUCO_FOUND_IMG == ON)
cv::imshow("arruco", imgOrg);
// set initial position for upper boddy
if((chest->getInit() == false) && arm_left->getJ1Found() && arm_right->getJ1Found())
{
// set bool to true
chest->setInit();
// set initial position between shoulders -> both shoulders are needed
if(arm_left->getJ1Found() && arm_right->getJ1Found())
chest->setInitPos((-j1Left(2) -j1Right(2))/2);
if(COUT_CHEST_INIT_POS == ON)
cout << "chest init pos: " << chest->getInitPos() << endl;
}
// check if upper boddy moved
if(chest->getInit() && (arm_left->getJ1Found() || arm_right->getJ1Found()))
{
// reset bool
chest->resetTorsoMoved();
// calculate current chest position
double chestCurPos;
if(arm_left->getJ1Found() && arm_right->getJ1Found())
chestCurPos = (-j1Left(2) -j1Right(2))/2;
else if(arm_left->getJ1Found())
chestCurPos = -j1Left(2);
else if(arm_right->getJ1Found())
chestCurPos = -j1Right(2);
if(COUT_CHEST_CUR_POS == ON)
cout << "current pos: " << chestCurPos << endl;
// calculate distance
double dist = chest->getInitPos() - chestCurPos;
if(COUT_CHEST_DIST == ON)
cout << "chest dist: " << dist << endl;
// check if distance is greater than threshold
if(abs(dist) > CHEST_DIST_THRESH)
{
// set bool: thresh moved
chest->setTorsoMoved();
// calculate angle for torso
chest->setAngle(asin(dist/TORSO_LENGTH));
if(COUT_CHEST_ANGLE == ON)
cout << "angle" << chest->getAngle() << endl;
}
}
// calculate relative position for the left arm
if(arm_left->getJ1Found() && arm_left->getJ2Found() && arm_left->getJ3Found())
{
// calculate distance
Vector3d d1 = j2Left - j1Left;
Vector3d d2 = j3Left - j1Left;
// check min dist
for(int i = 0; i < 3; i++)
{
if(abs(d1(i)) < MIN_DIST)
d1(i) = 0.0;
if(abs(d2(i)) < MIN_DIST)
d2(i) = 0.0;
}
// cout << "d1 " << d1(0) << "\t" << d1(1) << "\t" << d1(2) << endl;
// cout << "d2 " << d2(0) << "\t" << d2(1) << "\t" << d2(2) << endl;
// set relative coordinates
j1Left(0) = 0;
j1Left(1) = 0;
j1Left(2) = 0;
j2Left(0) = -d1(2);
j2Left(1) = d1(1);
j2Left(2) = -d1(0);
j3Left(0) = -d2(2);
j3Left(1) = d2(1);
j3Left(2) = -d2(0);
// write into arm
arm_left->setJ1Coord(j1Left);
arm_left->setJ2Coord(j2Left);
arm_left->setJ3Coord(j3Left);
// set bool that all markers for the left arm have been found
arm_left->setArmFound();
if(COUT_JOINT_REL_POS == ON)
{
cout << "rel pos SHOULDER_LEFT: " << j1Left(0) << "\t\t" << j1Left(1) << "\t\t" << j1Left(2) << endl;
cout << "rel pos ELBOW_LEFT: " << j2Left(0) << "\t" << j2Left(1) << "\t" << j2Left(2) << endl;
cout << "rel pos WRIST_LEFT: " << j3Left(0) << "\t" << j3Left(1) << "\t" << j3Left(2) << endl;
cout << endl << endl;
}
}
// calculate relative position for the right arm
if(arm_right->getJ1Found() && arm_right->getJ2Found() && arm_right->getJ3Found())
{
// calculate distance
Vector3d d1 = j2Right - j1Right;
Vector3d d2 = j3Right - j1Right;
// check min dist
for(int i = 0; i < 3; i++)
{
if(abs(d1(i)) < MIN_DIST)
d1(i) = 0.0;
if(abs(d2(i)) < MIN_DIST)
d2(i) = 0.0;
}
// cout << "d1 " << d1(0) << "\t" << d1(1) << "\t" << d1(2) << endl;
// cout << "d2 " << d2(0) << "\t" << d2(1) << "\t" << d2(2) << endl;
// set relative coordinates
j1Right(0) = 0;
j1Right(1) = 0;
j1Right(2) = 0;
j2Right(0) = -d1(2);
j2Right(1) = d1(1);
j2Right(2) = -d1(0);
j3Right(0) = -d2(2);
j3Right(1) = d2(1);
j3Right(2) = -d2(0);
// write into arm
arm_right->setJ1Coord(j1Right);
arm_right->setJ2Coord(j2Right);
arm_right->setJ3Coord(j3Right);
// set bool that all markers for the right arm have been found
arm_right->setArmFound();
if(COUT_JOINT_REL_POS == ON)
{
cout << "rel pos SHOULDER_RIGHT: " << j1Right(0) << "\t\t" << j1Right(1) << "\t\t" << j1Right(2) << endl;
cout << "rel pos ELBOW_RIGHT: " << j2Right(0) << "\t" << j2Right(1) << "\t" << j2Right(2) << endl;
cout << "rel pos WRIST_RIGHT: " << j3Right(0) << "\t" << j3Right(1) << "\t" << j3Right(2) << endl;
cout << endl << endl;
}
}
}
