定位原理很簡(jiǎn)單，故不贅述，直接上源碼，內(nèi)附注釋。（如果對(duì)您的學(xué)習(xí)有所幫助，還請(qǐng)幫忙點(diǎn)個(gè)贊，謝謝了）

#!/usr/bin/env python3# -*- coding: utf-8 -*-'''Created on Wed May 16 10:50:29 2018@author: dag'''import sympyimport numpy as npimport mathfrom matplotlib.pyplot import plotfrom matplotlib.pyplot import showimport matplotlib.pyplot as pltimport matplotlib#解決無(wú)法顯示中文問(wèn)題，fname是加載字體路徑，根據(jù)自身pc實(shí)際確定，具體請(qǐng)百度zhfont1 = matplotlib.font_manager.FontProperties(fname=’/System/Library/Fonts/Hiragino Sans GB W3.ttc’) #隨機(jī)產(chǎn)生3個(gè)參考節(jié)點(diǎn)坐標(biāo)maxy = 1000maxx = 1000cx = maxx*np.random.rand(3)cy = maxy*np.random.rand(3)dot1 = plot(cx,cy,’k^’) #生成盲節(jié)點(diǎn)，以及其與參考節(jié)點(diǎn)歐式距離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])) #計(jì)算定位坐標(biāo) 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] #解算得到定位節(jié)點(diǎn)坐標(biāo)[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(’三邊測(cè)量法的定位’,fontproperties=zhfont1) plt.legend([’參考節(jié)點(diǎn)’,’盲節(jié)點(diǎn)’,’定位節(jié)點(diǎn)’], loc=’lower right’,prop=zhfont1)show() derror = math.sqrt(np.square(locx-mtx) + np.square(locy-mty)) print(derror)

輸出效果圖：

補(bǔ)充：python opencv實(shí)現(xiàn)三角測(cè)量（triangulation）

import cv2import numpy as npimport scipy.io as scioif __name__ == ’__main__’: print('main function.') #驗(yàn)證點(diǎn) point = np.array([1.0 ,2.0, 3.0]) #獲取相機(jī)參數(shù) 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,::] #通過(guò)投影矩陣將點(diǎn)從世界坐標(biāo)投到像素坐標(biāo) 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,:]#兩行一列，齊次坐標(biāo)轉(zhuǎn)化 point3 = pj3[:2,:]/pj3[2,:] #利用投影矩陣以及對(duì)應(yīng)像素點(diǎn)，進(jìn)行三角測(cè)量 points = cv2.triangulatePoints(P1,P3,point1,point3) #齊次坐標(biāo)轉(zhuǎn)化并輸出 print(points[0:3,:]/points[3,:])

以上為個(gè)人經(jīng)驗(yàn)，希望能給大家一個(gè)參考，也希望大家多多支持好吧啦網(wǎng)。如有錯(cuò)誤或未考慮完全的地方，望不吝賜教。