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

Dear 丶 2022-11-28 10:40 206阅读 0赞

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

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

![20200817110757213.png][]                                ![20200817111004324.png][]

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

![20200817111512916.png][]                                            ![20200630230439271.png_pic_center][]

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

/*根据两点经纬度坐标来求取该直线与正北方向的夹角，即方位角*/
    #include<iostream>
    #include<math.h>
    #include<string.h>
    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;
    	}

[20200817110757213.png]: https://img-blog.csdnimg.cn/20200817110757213.png
[20200817111004324.png]: /images/20221124/612d7ce54d8f4952a87fda899b6c759e.png
[20200817111512916.png]: https://img-blog.csdnimg.cn/20200817111512916.png
[20200630230439271.png_pic_center]: /images/20221124/0422736a63c240fe8b8565c67dd17cea.png