python 三边测量定位的实现代码_Python

python 三边测量定位的实现代码

2021-10-18 09:33Answerman33 Python

这篇文章主要介绍了python 三边测量定位的实现代码，具有很好的参考价值，希望对大家有所帮助。一起跟随小编过来看看吧

定位原理很简单，故不赘述，直接上源码，内附注释。（如果对您的学习有所帮助，还请帮忙点个赞，谢谢了）

				?

									#!/usr/bin/env python3

									# -*- coding: utf-8 -*-

									"""

									created on wed may 16 10:50:29 2018

									@author: dag

									"""

									import sympy

									import numpy as np

									import math

									from matplotlib.pyplot import plot

									from matplotlib.pyplot import show

									import matplotlib.pyplot as plt

									import matplotlib

									#解决无法显示中文问题，fname是加载字体路径，根据自身pc实际确定，具体请百度

									zhfont1 = matplotlib.font_manager.fontproperties(fname='/system/library/fonts/hiragino sans gb w3.ttc')

									#随机产生3个参考节点坐标

									maxy = 1000

									maxx = 1000

									cx = maxx*np.random.rand(3)

									cy = maxy*np.random.rand(3)

									dot1 = plot(cx,cy,'k^')

									#生成盲节点，以及其与参考节点欧式距离

									mtx = maxx*np.random.rand()

									mty = maxy*np.random.rand()

									plt.hold('on')

									dot2 = plot(mtx,mty,'go')

									da = math.sqrt(np.square(mtx-cx[0])+np.square(mty-cy[0]))

									db = math.sqrt(np.square(mtx-cx[1])+np.square(mty-cy[1])) 

									dc = math.sqrt(np.square(mtx-cx[2])+np.square(mty-cy[2]))

									#计算定位坐标  

									def triposition(xa,ya,da,xb,yb,db,xc,yc,dc): 

									    x,y = sympy.symbols('x y')

									    f1 = 2*x*(xa-xc)+np.square(xc)-np.square(xa)+2*y*(ya-yc)+np.square(yc)-np.square(ya)-(np.square(dc)-np.square(da))

									    f2 = 2*x*(xb-xc)+np.square(xc)-np.square(xb)+2*y*(yb-yc)+np.square(yc)-np.square(yb)-(np.square(dc)-np.square(db))

									    result = sympy.solve([f1,f2],[x,y])

									    locx,locy = result[x],result[y]

									    return [locx,locy]

									#解算得到定位节点坐标

									[locx,locy] = triposition(cx[0],cy[0],da,cx[1],cy[1],db,cx[2],cy[2],dc)

									plt.hold('on')

									dot3 = plot(locx,locy,'r*')

									#显示脚注

									x = [[locx,cx[0]],[locx,cx[1]],[locx,cx[2]]]

									y = [[locy,cy[0]],[locy,cy[1]],[locy,cy[2]]]

									for i in range(len(x)):

									    plt.plot(x[i],y[i],linestyle = '--',color ='g' )

									plt.title('三边测量法的定位',fontproperties=zhfont1)  

									plt.legend(['参考节点','盲节点','定位节点'], loc='lower right',prop=zhfont1)

									show() 

									derror = math.sqrt(np.square(locx-mtx) + np.square(locy-mty)) 

									print(derror)

输出效果图：

python 三边测量定位的实现代码

补充：python opencv实现三角测量（triangulation）

看代码吧~

				?

									import cv2

									import numpy as np

									import scipy.io as scio

									if __name__ == '__main__':

									    print("main function.")

									    #验证点

									    point = np.array([1.0 ,2.0, 3.0])

									    #获取相机参数

									    cams_data = scio.loadmat('/data1/dy/supersmpl/data/amafmvs_dataset/cameras_i_crane.mat')

									    pmats = cams_data['pmats']  # pmats(8, 3, 4) 投影矩阵 

									    p1 = pmats[0,::]

									    p3 = pmats[2,::]

									    #通过投影矩阵将点从世界坐标投到像素坐标

									    pj1 = np.dot(p1, np.vstack([point.reshape(3,1),np.array([1])]))

									    pj3 = np.dot(p3, np.vstack([point.reshape(3,1),np.array([1])]))

									    point1 = pj1[:2,:]/pj1[2,:]#两行一列，齐次坐标转化

									    point3 = pj3[:2,:]/pj3[2,:]

									    #利用投影矩阵以及对应像素点，进行三角测量

									    points = cv2.triangulatepoints(p1,p3,point1,point3)

									    #齐次坐标转化并输出

									    print(points[0:3,:]/points[3,:])