KMS01 : 자작 Algorithm Tayo Algorithm

 

이번에는 옛날에 만든 알고리즘을 하나 올려보도록 하곘습니다. 참고로 제가 2019년에 주소창 최우수상 탄 알고리즘이 바로 이겁니다.

---

알고리즘 설명

이 알고리즘은 평면방정식과 원주각의 응용버전으로서 1대의 카메라의 높이, 설치각, 촬영각등이 주어졌을때 카메라의 사진상에 있는 점의 위치를 삼차원의 점의 위치를 계산할 수 있습니다.

이때 삼차원에서 카메라가 바라보는 방향을 -x, 높이성분을 y, 나머지 수평성분을 z라고 합니다. 카메라의 위치는 원점(0, 0, 0)입니다.

​

예를 들어, 카메라가 45도를 바라보게 설치되어있고, 카메라가 수직으로 60도, 수평으로 60도를 촬영할 수 있고,
설치높이가 10m일때를 가정한다면 물체가 카메라가 찍은 사진의 중앙에 위치한다면 물체의 위치는

라고 할 수 있습니다. (프로그램에서는 -5.7735, -10, 0으로 출력됨)

​프로그램의 증명은 다음과 같습니다.

---

수학적인 증명

아래 워드에 알고리즘의 원리가 어느 정도 적혀있습니다.

KMS 1정리-tayo Algorithm.docx

0.24MB

---

소스코드

KMS01.c

#include "KMS01.h"
#include <stdio.h>

point MakePoint(const box Bbox) {
	point V;
	V.x = (Bbox.p1.x + Bbox.p2.x) / 2;
	V.y = (Bbox.p1.y > Bbox.p2.y) ? Bbox.p2.y : Bbox.p1.y;
	return V;
}

// ************************ Grapg Funntion Code ************************ //


point3D GetLocaion(const point P) {
	point R;
	point S;

	point3D Val;

	R = XY(P.y);
	S = XZ(P.x, R.x);

	//cout << "R : " << R.x << " " << R.y << endl;
	//cout << "alpa : " << alpa.x << " " << alpa.y << endl;

	Val.x = R.x;
	Val.y = R.y;
	Val.z = S.y;

	return Val;
}

point XY(const double Pix) {
	const double AngleV = Angle2 * Pix / MPY;
	const double a = TAN(90 - (Angle1 - ((double)Angle2 / 2) + AngleV));
	point Val;
	/* y = ax, y = -H
	  -H = ax x = -H/a; */
	Val.x = -1 * (double)(H - H3) / a;
	Val.y = a * Val.x;
	return Val;
}

point XZ(const double Pix, const double x1) {
	const double AngleV = AngleII * Pix / MPX;
	double a = TAN(fabs(AngleV - (((double)AngleII) / 2))) * (((AngleV - ((double)AngleII / 2)) > 0) ? -1 : 1);
	point Val;
	/*	y = ax
		x = x1
		(x1, TAN(AngleV - 0.5AngleII)x1)
	*/
	Val.x = x1;
	Val.y = a * Val.x;
	return Val;
}

double GetDis(const point3D P) {
	return sqrt(SQ(P.x) + SQ(P.z));
}

velocity getVelocity(point3D P) {
	static point3D BP = { 0, 0, 0 };
	velocity V;

	V.x = (P.x - BP.x) / ((double)Trigt / 1000);
	V.y = (P.z - BP.z) / ((double)Trigt / 1000);

	BP = P;
	return V;
}

/*velocity getValPer(velocity V) {
	static velocity BV = { 0, 0 };
	velocity Val;

	Val.x = V.x / BV.x * 100;   gfs
	Val.y = V.y / BV.y * 100;

	BV = V;
	return Val;
}*/

point3D KMS01_BOX(const box B) {
	point3D DP;
	velocity V;
	double D;
	DP = GetLocaion(MakePoint(B));
	printf("3D point(%f, %f, %f) \n", DP.x, DP.y, DP.z);
	printf("distance : %f \n", GetDis(DP));
	D = GetDis(DP);
	V = getVelocity(DP);
	printf("velocity : (%f, %f) \n", V.x, V.y);
	printf("\n");

	return DP;
}

point3D KMS01(const point P) {
	point3D DP;
	velocity V;
	double D;
	DP = GetLocaion(P);
	//printf("3D point(%f, %f, %f) \n", DP.x, DP.y, DP.z);
	//printf("distance : %f \n", GetDis(DP));
	D = GetDis(DP);
	V = getVelocity(DP);
	//printf("velocity : (%f, %f) \n", V.x, V.y);
	//printf("\n");

	return DP;
}

KMS01.h

#pragma once
#ifndef __KMS01__
#define __KMS01__

#include <math.h>

//made by tomksnag BY-NC-SA

// ************************ base difines ************************ //

#define TRUE 1
#define FALSE 0

// ************************ Simple culculations ************************ //

#define PI	3.141592653589723238
#define SQ(a)	((a)*(a))
#define TAN(theta)	(tan((theta) * PI / 180.0))

// ************************ Consts ************************ //

#define MPY		20 //사진 Y축 최댓값
#define MPX		20 //사진 X축 최갯값
#define Angle1	30 //카메라 설치각 (Angle) 호도각 아님
#define Angle2	60 //카메라 촬영각 (Angle)
#define H		12 //카메라 설치높이(m)
#define H3		2 //차량 높이(m)
#define AngleII	60 //카메라 y축 촬영각

#define Trigt	10 //트리그 시간(두 점 사이의 시간)

// ************************ Class ************************ //

typedef struct _point {
	double x;
	double y;
}point;

typedef struct _point3D {
	double x;
	double y;
	double z;
}point3D;

typedef struct _box {
	point p1;
	point p2;
}box;


typedef struct _velocity {
	double x;
	double y;
}velocity;

// ************************ Base Valuable ************************ //


// ************************ Graph Function ************************ //

point MakePoint(const box Bbox);

point3D GetLocaion(const point P); //점의 위치를 통해 3D위치를 알아낸다. (원점 = CAM)
point XY(const double Pix); // GetLocation 하위함수
point XZ(const double Pix, const double x1); // GetLocation 하위함수

double GetDis(const point3D P); //점가 CAM사이의 위치를 알아낸다.
velocity getVelocity(point3D P); //속도를 알아낸다.
//velocity getValPer(velocity V); // acselation을 알아낸다.

int dicision( point3D P, const double D, const velocity VL);

point3D KMS01_BOX(const box B);
point3D KMS01(const point B);

#endif

main.c

#include <stdio.h>
#include "KMS01.h"

int main(void) {
	int x, y;
	point3D V;
	point P;
	while (1) {
		printf("f1st loc : ");
		scanf_s("%d %d", &x, &y);
		P.x = x;
		P.y = y;
		V = KMS01(P);
		printf("LOC(%f, %f, %f) \n \n", V.x, V.y, V.z);
	}
}

중요한 함수들은 모두 설명이 붙어있기에 설명은 생략합니다. KMS01()은 사진상의 점의 위치 [point]를 인자로 받아 실제 점의 위치[3Dpoint]를 반환합니다.

---

다른 프로그램

소인수분해 프로그램 Ver3

 

kms-program.tistory.com/6