摄影测量转角系统转换——OPK转换至POK下

摄影测量转角系统OPK转换至POK下

最近学习了一种新的角度单位，gon
gon 定义. Gon，百分度制 (GRAD)角度单位，用G表示。. 1G = 直角的1%。.
因为做实验用的数据是这个角度单位，但是咱们一般都是用弧度制rad，所以要先进行角度单位的转换。

转角系统

代码

输入的pos文件如下
第一行是数据量
第二行分别是：
序号 w（gon） p(gon) k(gon) X Y Z （120忘了是啥了，不重要）

#include<stdio.h>
#include<iostream>
#include<fstream>

using namespace std;

#define SPGT_PI           3.1415926535897932384626433832795       /* PAI */
#define PI_3              1.0471975511965977461542144610932

#define SPGT_R2D          (180.0/SPGT_PI)  // 57.295779513082320876798154814105
#define SPGT_D2R          (SPGT_PI/180.0)  // 0.01745329251994329576923690768489

typedef struct imgInfo {
	long index;
	double w;
	double p;
	double k;
	double X;
    double Y;
	double Z;
}imgIn;

static inline double* OnPK2M(double o, double p, double k, double* r)
{
	double cosp = cos(p); double cosw = cos(o); double cosk = cos(k);
	double sinp = sin(p); double sinw = sin(o); double sink = sin(k);
	r[0] = cosp * cosk;
	r[1] = -sink * cosp;
	r[2] = -sinp;
	r[3] = -sinw * sinp * cosk + cosw * sink;
	r[4] = sinw * sinp * sink + cosw * cosk;
	r[5] = -sinw * cosp;
	r[6] = cosw * sinp * cosk + sinw * sink;
	r[7] = -sink * cosw * sinp + sinw * cosk;
	r[8] = cosw * cosp;
	return r;
}

inline double* OPK2M(double o, double p, double k, double* r)
{ return OnPK2M(o, -p, k, r); }

inline void M2nPOK(double* r, double* p, double* o, double* k) {
	*o = asin(-r[5]);         // r[5] = -sinw;
	*p = atan2(-r[2], r[8]);  // r[2] = -sinp*cosw; r[8] = cosp*cosw;
	*k = atan2(r[3], r[4]);  // r[3] =  cosw*sink; r[4] = cosw*cosk;
}

int main() {
	ifstream fp("F:\\experiment\\Images\\pos.txt");
	int imgNum;
	float f;
	
	fp >> imgNum;
	imgIn* pos = new imgIn[imgNum];
	for (int i = 0; i < imgNum; i++) {
		fp >> pos[i].index >> pos[i].w >> pos[i].p >> pos[i].k >> pos[i].X >> pos[i].Y >> pos[i].Z >> f;
		//单位转变（gon->rad)
		pos[i].p = pos[i].p * 0.9 * SPGT_D2R;
		pos[i].w = pos[i].w * 0.9 * SPGT_D2R;
		pos[i].k = pos[i].k * 0.9 * SPGT_D2R;
	}
	fp.close();

	//转角系统opk->pok
	double M_1[9];
	for (int i = 0; i < imgNum; i++) {
		OPK2M(pos[i].w, pos[i].p, pos[i].k, M_1);
		M2nPOK(M_1, &pos[i].p, &pos[i].w, &pos[i].k);
	}
	ofstream outf("pos.txt");
	outf.setf(ios::fixed);
	outf.precision(5);
	for (int i = 0; i < imgNum; i++) {
		outf << pos[i].index << " " << pos[i].p << " " << pos[i].w << " " << pos[i].k << " " << pos[i].X << " " << pos[i].Y << " " << pos[i].Z<<endl;
	}
	outf.close();
	delete[] pos;
	return 0;
}

最后输出

最后输出即为pok坐标系下的坐标。