OpenPose로 좌표값 받아오기

 OpenPose에서 제공해주는 예제 코드를 활용했고, visual studio로 Serial 통신을 하는 코드는 다른 분이 뚫어두신 코드를 활용했습니다.

 

전체 코드는 너무 길어 깃허드 링크로 걸어두었습니다.

https://github.com/Myoung-Jisoo/Robot-Arm-using-OpenPose/blob/main/Visual%20studio%202017/openpose.cpp

이미지의 Keypoints의 좌표를 띄운 모습

이미지의 각 좌표가 나타난 모습입니다.

 

그리고 OpenPose 기본 예제코드인 openpose demo의 실행결과는 다음과 같습니다.

OpenPose 실행결과

 영상에서 보시다시피 튀는 값들이 보입니다. 필터처리가 필요해 보입니다. 때문에 아래와 같이 미디언 필터를 이용해 처리했습니다. (연속으로 들어온 다섯개의 값들을 정렬 후에 가운데 값 사용)

			if (filter > 5) {
				filter = 0;
				// 크기순 정렬
				sort(shoulderX.begin(), shoulderX.end());
				sort(shoulderY.begin(), shoulderY.end());
				sort(elbowX.begin(), elbowX.end());
				sort(elbowY.begin(), elbowY.end());
				sort(wristX.begin(), wristX.end());
				sort(wristY.begin(), wristY.end());
				// 중간값 담기
				if ((abs(shoulder.x - shoulderX.at(2)) < 10) && (abs(shoulder.y - shoulderY.at(2)) < 10)) shoulder_count++;
				if (shoulder_count > 4) {
					f_shoulder.x = shoulder.x;
					f_shoulder.y = shoulder.y;
				}
				else {
					shoulder.x = shoulderX.at(2);
					shoulder.y = shoulderY.at(2);
					//shoulder_count = 0;
				}

				if ((abs(elbow.x - elbowX.at(2)) < 20) && (abs(elbow.y - elbowY.at(2)) < 20)) elbow_count++;
				if (elbow_count > 4) {
					f_elbow.x = elbow.x;
					f_elbow.y = elbow.y;
				}
				else {
					elbow.x = elbowX.at(2);
					elbow.y = elbowY.at(2);
				}

				wrist.x = wristX.at(2);
				wrist.y = wristY.at(2);
				// vector 비우기
				shoulderX.clear();
				shoulderY.clear();
				elbowX.clear();
				elbowY.clear();
				wristX.clear();
				wristY.clear();
			}

 

그리고 위에서 얻은 데이터들을 토대로 아래와 같이 각 서보모터의 값들을 구한 후에 Uart통신을 통해 atmega128로 전송해주었습니다.

void getAngleAndSend() {
	angle1 = acos(downLength / downLength_Max);

	
	//double dz = downLength * tan(angle1);
	//double a = sqrt(pow(dx2, 2) + pow(dz, 2));

	angle2 = asin((f_elbow.x - wrist.x) / downLength_Max);

	if ((elbow.y - wrist.y) > 0) angle1 = 3.14 - angle1; // 1.5708은 90도

	std::string angle = std::to_string((int)(angle1 * (180.0 / 3.14))) + "," + std::to_string((int)(angle2 * (180.0 / 3.14)));

	op::opLog("\n" + angle);

	angle = "s" + angle + "e";

	char Angle[100];
	strcpy(Angle, angle.c_str());

	if (uart.isOpen()) {
		op::opLog("connected!");
		uart.Write(Angle, angle.length());
	}
	else op::opLog("not connected :(");
}