Métodos de triangulação em fotogrametria (visão computacional)

Todo mundo está familiarizado com a triangulação. No campo do CV, o processo de calcular os pontos do objeto a partir dos pontos da imagem é chamado de triangulação, mas no campo da fotogrametria, é chamado de interseção direta. Vale a pena notar que uma única imagem não pode restaurar as coordenadas 3D do pixel, pelo menos duas imagens são necessárias para obter as coordenadas reais do pixel (a informação de pose das duas imagens é conhecida aqui)

insira a descrição da imagem aqui

Existem muitos métodos de triangulação. Aqui apresentamos a triangulação de dois quadros, a triangulação de vários quadros, a triangulação iterativa e a triangulação iterativa de vários quadros de seleção de peso (com meu código anexado).

1. Triangulação de dois quadros

No opencv, a função triangulatePoints pode restaurar as coordenadas tridimensionais do ponto de acordo com a pose e os parâmetros internos dos dois frames.A triangulação no cv é de dois frames e não tem direito.

Os parâmetros de sua função são os seguintes:

void cv::triangulatePoints	(	InputArray 	projMatr1, //P1 1 3*4
InputArray 	projMatr2, //P2 3*4
InputArray 	projPoints1, //pixel coordinates
InputArray 	projPoints2, // pixel coordinates
OutputArray 	points4D // 3D coordinates
)

2. Triangulação de vários quadros

3
$\begin{array}{l} x-x_{0}=-f \frac{a_{1}\left( X-X_{s}\direita)+b_{1}\esquerda(Y-Y_{s}\direita)+c_{1}\esquerda(Z-Z_{s}\direita)}{a_{3}\ esquerda(X-X_{s}\direita)+b_{3}\esquerda(Y-Y_{s}\direita)+c_{3}\esquerda(Z-Z_{s}\direita)} \\ y- y_{0}=-f \frac{a_{2}\esquerda(X-X_{s}\direita)+b_{2}\esquerda(Y-Y_{s}\direita)+c_{2}\esquerda (Z-Z_{s}\direita)}{a_{3}\esquerda(X-X_{s}\direita)+b_{3}\esquerda(Y-Y_{s}\direita)+c_{3} \left(Z-Z_{s}\right)} \end{array}$
由共线条件方程得到三角化严密解法,将分母移到左边，得到
$\begin {array}{c} \left(x-x_{0}\right)\left[a_{3}\left(X-X_{s}\right)+b_{3}\left(Y-Y_{s }\direita)+c_{3}\esquerda(Z-Z_{5}\direita)\direita]= \\ -f\esquerda[a_{1}\esquerda(X-X_{s}\direita)+b_ {1}\left(Y-Y_{s}\right)+c_{1}\left(Z-Z_{s}\right)\right] \\ \left(y-y_{0}\right)\ esquerda[a_{3}\esquerda(X-X_{s}\direita)+b_{3}\esquerda(Y-Y_{s}\direita)+c_{3}\esquerda(Z-Z_{s}\ direita)\direita]= \\ -f\esquerda[a_{2}\esquerda(X-X_{s}\direita)+b_{2}\esquerda(Y-Y_{s}\direita)+c_{2 }\left(Z-Z_{s}\right)\right] \end{array}$
整理可得:
$\begin{array}{l} l_{1} X+l_{ 3} Y+l_{3} Z-l_{x}=0 \\ L_{4} X+l_{5} Y+l_{6} Z-l_{y}=0 \end{array}$
Como:
$I_{1}=f a_{1}+\left(x-x_{0}\right) a_{3}$

$l_{2}=f b_{1}+\left(x-x_{0}\right) b_{3}$

$l_{3}=f c_{1}+\left(x-x_{0}\right) c_{3}$

$l_{x }=f a_{1} X_{s}+f b_{1} Y_{s}+f_{c}, Z_{s}+\esquerda(x-x_{b}\direita) a_{3} X_{ s}+\esquerda(x-x_{0}\direita) b_{3} Y_{s}+\esquerda\lângulo x-x_{b}\direita) c_{3} Z_{s}$

$l_{4}=f a_{2}+\left(y-y_{0}\right) a_{3}$

$L_{5}=f b_{2}+\left(y-y_{0}\right) b_{3}$

$l_{6}=f c_{2}+\left(y-y_{0}\right) c_{3}$

$l_ {y}=f a_{2} X_{s}+f b_{2} Y_{s}+f c_{2} Z_{s}+\esquerda(y-y_{0}\direita) a_{3} X_{s}+\esquerda(y-y_{0}\direita) b_{3} Y_{s}+\esquerda(y-y_{0}\direita) c_{3} Z_{s}$

Pode ser visto acima que um pixel pode listar 2 equações, e para n pixels com o mesmo nome, 2n equações podem ser listadas (AX=B, a equação sobredeterminada é resolvida de acordo com mínimos quadrados), ou seja, multi- triangulação de quadros, o código é o seguinte:

def pixelToCam(pts,K):
    '''

    :param pts: pixel coordinates
    :param K: camera params
    :return: camera coordinates
    '''
    camPts=np.zeros((1,2))
    camPts[0,0]=(pts[0,0]-K[0,2])/K[0,0]
    camPts[0,1]=(pts[0,1]-K[1,2])/K[1,1]
    return camPts
def getEquation(camPts,R,t):
    '''
    build equation ,one pixel point get 2 equations
    :param camPts: camera coordinates
    :param R: image pose-rotation ,world to camera
    :param t: image pose -translation,is camera center(t=-R.T*tvec)
    :return: equation coefficient
    '''
    A=np.zeros((2,3))
    b=np.zeros((2,1))
    A[0,0]=R[0,0]-camPts[0,0]*R[2,0]
    A[0,1]=R[0,1]-camPts[0,0]*R[2,1]
    A[0,2]=R[0,2]-camPts[0,0]*R[2,2]
    b[0,0]=t[0,0]*A[0,0]+t[0,1]*A[0,1]+t[0,2]*A[0,2]
    A[1,0]=R[1,0]-camPts[0,1]*R[2,0]
    A[1,1]=R[1,1]-camPts[0,1]*R[2,1]
    A[1,2]=R[1,2]-camPts[0,1]*R[2,2]
    b[1,0]=t[0,0]*A[1,0]+t[0,1]*A[1,1]+t[0,2]*A[1,2]
    return A,b

3. Triangulação iterativa

O método é adicionar fatores aos coeficientes da equação e ajustar continuamente os fatores dos coeficientes. Meu código é o seguinte:

def IterativeLinearLSTri(u1,P1,u2,P2):
    wi,wi1=1,1 #不断需要更新的因子
    X=np.zeros((4,1))
    for i in range(10): #迭代10次
        X_=linear_ls_triangulation(u1,P1,u2,P2) # 线性求解两帧的像素点的三维坐标
        X[1,0]=X_[0,1]
        X[2,0]=X_[0,2]
        X[3,0]=1
        p2x=np.dot(P1[2,:].reshape(1,4),X)[0,0]
        p2x1=np.dot(P2[2,:].reshape(1,4),X)[0,0]
        if abs(wi-p2x) <=0.001 and abs(wi1-p2x1)<=0.001 :
            break
        wi=p2x
        wi1=p2x1
        A = np.array([(u1[0]*P1[2, 0] - P1[0, 0])/wi, (u1[0]*P1[2, 1] - P1[0, 1])/wi,
                      (u1[0]*P1[2, 2] - P1[0, 2])/wi, (u1[1]*P1[2, 0] - P1[1, 0])/wi,
                      (u1[1]*P1[2, 1] - P1[1, 1])/wi, (u1[1]*P1[2, 2] - P1[1, 2])/wi,
                      (u2[0]*P2[2, 0] - P2[0, 0])/wi1, (u2[0]*P2[2, 1] - P2[0, 1])/wi1,
                      (u2[0]*P2[2, 2] - P2[0, 2])/wi1, (u2[1]*P2[2, 0] - P2[1, 0])/wi1,
                      (u2[1]*P2[2, 1] - P2[1, 1])/wi1,
                      (u2[1]*P2[2, 2] - P2[1, 2])/wi1]).reshape(4, 3)
        B = np.array([-(u1[0]*P1[2, 3] - P1[0, 3])/wi,
                      -(u1[1]*P1[2, 3] - P1[1, 3])/wi,
                      -(u2[0]*P2[2, 3] - P2[0, 3])/wi1,
                      -(u2[1]*P2[2, 3] - P2[1, 3])/wi1]).reshape(4, 1)
        
        ret, X_ = cv2.solve(A, B, flags=cv2.DECOMP_SVD)
        X[0,0]=X_[0,0]
        X[1,0]=X_[1,0]
        X[2,0]=X_[2,0]
        X[3,0]=1
    return X

4. Triangulação multiquadro iterativa de seleção de peso

Primeiro, explique a iteração de seleção de peso (algoritmo IGG). Na década de 1980, o primeiro método de iteração de seleção de peso derivado da estimativa de variância posterior para localizar o erro bruto foi denominado "método Li Deren". Devido à limitação das condições objetivas no trabalho de medição real, é difícil evitar completamente a existência de erros grosseiros ou obter a mesma precisão de medição. No processo de ajuste, o peso é geralmente introduzido como um índice para comparar o relativo precisão entre as observações, e para dados observacionais com maior precisão são dados pesos maiores, para que a interferência de informações prejudiciais possa ser evitada. Por exemplo, quando combinamos a correspondência de imagens, usaremos ransac (também um algoritmo de estimativa robusto) para eliminar outliers, mas quando seu ponto com o mesmo nome estiver em vários quadros e houver apenas um (como a medição de posição de vários semáforos de quadro), ransac não pode mais ser usado. Neste momento, o uso do algoritmo IGG pode efetivamente evitar o impacto de erros. reproduziu o algoritmo do papel. O código é o seguinte:

def IterationInsection(pts,K,R,t):
    # cam_xyz is  inital value
    # 这里假设像点x,y是等权的
    k0=1.5
    k1=2.5 # K1=2
    weight=np.identity(len(pts)*2)
    cam_xyz=mutiTriangle(pts,K,R,t)
    cam_xyz_pre=cam_xyz
    iteration=0
    while 1:
        d=np.zeros((len(pts),1))
        for i in range(len(R)):
            pro,J = cv2.projectPoints(cam_xyz.reshape(1, 1, 3), R[i], t[i], K, np.array([]))
            pro=pro.reshape(1,2)
            deltax=pro[0,0]-pts[i][0,0]
            deltay=pro[0,1]-pts[i][0,1]
            d[i,0]=np.sqrt(deltax**2+deltay**2)
        weight_temp=np.diag(weight)[::2].reshape(-1,1)
        delta=np.sqrt(np.sum(weight_temp*d**2)/(len(pts)-2))
        w=np.zeros((len(pts),1))
        for i in range(len(pts)):
            u=d[i]
            if abs(u)<k0*delta:
                w[i]=1
            elif abs(u)<k1*delta and abs(u)>=k0*delta:
                w[i]=delta/u
            elif abs(u)>=k1*delta:
                w[i]=0
        weight_temp=w
        weight_p=[val for val in weight_temp.reshape(-1,) for i in range(2)]
        weight_p=np.diag(weight_p)
        cam_xyz_curr=weight_mutiTriangle(pts,K,R,t,weight_p)
        dx=cam_xyz_curr[0,0]-cam_xyz_pre[0,0]
        dy=cam_xyz_curr[1,0]-cam_xyz_pre[1,0]
        dz=cam_xyz_curr[2,0]-cam_xyz_pre[2,0]
        # print(dx,dy,dz)
        if np.sqrt(dx**2+dy**2+dz**2)<0.01:
            break
        else:
            cam_xyz=cam_xyz_curr
            cam_xyz_pre=cam_xyz_curr
            weight=weight_p
            iteration+=1
#    print("d{0}".format(d))
    print("iteration is {0}\n".format(iteration))
    print("IGG....{0},{1},{2}".format(cam_xyz[0,0],cam_xyz[1,0],cam_xyz[2,0]))
    return cam_xyz,weight

Entre elas, a função mutiTriangle e a função weight_mutiTriangle são as seguintes:

def mutiTriangle(pts,K,R,t):
    if len(pts)>=4: #这里是假设至少track 4帧
        equa_A=[]
        equa_b=[]
        for i in range(len(pts)):
            camPts=pixelToCam(pts[i],K)
            t1=np.dot(np.linalg.inv(R[i]),-t[i]).reshape(1,3)
            A1,b1=getEquation(camPts,R[i],t1)
            equa_A.append(A1)
            equa_b.append(b1)
        AA=np.vstack(equa_A)
        bb=np.vstack(equa_b)
        P_ls=np.dot(np.linalg.inv(AA.T@AA),AA.T@bb)
        return P_ls
    else:
        print("tracker pixel point less 3,can not insection........")
        return None

def weight_mutiTriangle(pts,K,R,t,weight):
    if len(pts)>=4:
        equa_A=[]
        equa_b=[]
        for i in range(len(pts)):
            camPts=pixelToCam(pts[i],K)
            t1=np.dot(np.linalg.inv(R[i]),-t[i]).reshape(1,3)
            A1,b1=getEquation(camPts,R[i],t1)
            equa_A.append(A1)
            equa_b.append(b1)
        AA=np.vstack(equa_A)
        bb=np.vstack(equa_b)
        P_ls=np.dot(np.linalg.pinv(AA.T@weight@AA),AA.T@weight@bb)
        return P_ls
    else:
        print("tracker pixel point less 4,can not insection........")
        return None

referências:

1. Estimativa Robusta de Mínimos Quadrados Totais para Interseção Frontal em Espaço de Imagens Múltiplas [J] Li Zhongmei, Journal of Surveying and Mapping

Métodos de triangulação em fotogrametria (visão computacional)

Métodos de triangulação em fotogrametria (visão computacional)

Acho que você gosta