相机标定后得到相机视域，并根据相机视域求取摄像机位置和摄像机方位角

相机标定将图像坐标和地理坐标联系起来，可以得到相机视域，并根据视域可得到相机的点位置以及相机方位角

1、利用相机视域求相机位置，即视域两条线的交点，利用直线方程求解：

                                              

2、相机方位角的求解，思路是：首先求出视域中点坐标，中点坐标的连线与正北方向的夹角即为相机方位角。

                                                             

看了网上众多代码，参考其中一篇，改进后，在此附上方位角求解的c++版本算法：

/*根据两点经纬度坐标来求取该直线与正北方向的夹角，即方位角*/
#include
#include
#include
using namespace std;

int main()
{
     double pi=3.1415926;
	 //地球纵横轴半径
	 double Rc = 6378137;
	 double Rj = 6356725;
	double longitude1, latitude1, longitude2, latitude2;//经纬度
	double m_radlon1, m_radlat1, m_radlon2, m_radlat2;//转为弧度制
	double Ec1, Ed1, Ec2, Ed2;
	//输入两点经纬度坐标
	longitude1 = 119.983875;
	latitude1 = 31.81912509;
	longitude2 = 119.9837622;
	latitude2= 31.81839832;
    //转化为弧度制
	m_radlon1 = longitude1 * pi/180;
	m_radlat1 = latitude1 * pi / 180;
	m_radlon2 = longitude2 * pi / 180;
	m_radlat2 = latitude2 * pi / 180;
	//
	Ec1 = Rj + (Rc - Rj)*(90.0 - latitude1) / 90.0;
	Ed1 = Ec1 * cos(m_radlat1);
	Ec2 = Rj + (Rc - Rj)*(90.0 - latitude2) / 90.0;
	Ed2 = Ec2 * cos(m_radlat2);
	double dx, dy;
	dx = (m_radlon2 - m_radlon1)*Ed1;
	dy = (m_radlat2 - m_radlat1)*Ec1;
	double angle = 0;//方位角
	angle = atan(abs(dx / dy)) * 180 / pi;
	double dLo, dLa;
	dLo = longitude2 - longitude1;
	dLa = latitude2 - latitude1;
	if (dLo > 0 && dLa <= 0)
	{
		angle = (90 - angle) + 90;
	}
	else if (dLo <= 0 && dLa < 0)
	{
		angle += 180;
	}
	else if (dLo < 0 && dLa >= 0)
	{
		angle = (90 - angle) + 270;
	}
	else
	{
		angle = angle;
	}
	cout << "方位角是：" << angle << endl;
	return 0;
	}