Métodos de triangulación en fotogrametría (visión artificial)

Todo el mundo está familiarizado con la triangulación. En el campo de CV, el proceso de calcular puntos de objeto a partir de puntos de imagen se llama triangulación, pero en el campo de la fotogrametría, se llama intersección directa. Vale la pena señalar que una sola imagen no puede restaurar las coordenadas 3D del píxel, se requieren al menos dos imágenes para obtener las coordenadas reales del píxel (aquí se conoce la información de pose de las dos imágenes)

inserte la descripción de la imagen aquí

Hay muchos métodos de triangulación. Aquí presentamos la triangulación de dos cuadros, la triangulación de cuadros múltiples, la triangulación iterativa y la triangulación iterativa de cuadros múltiples con selección de peso (con mi código adjunto).

1. Triangulación de dos fotogramas

En opencv, la función triangulatePoints puede restaurar las coordenadas del punto tridimensional de acuerdo con la pose y los parámetros internos de los dos marcos.La triangulación en cv es de dos marcos y no tiene derechos.

Los parámetros de su función son los siguientes:

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. Triangulación de fotogramas múltiples

三角化严密解推导过程：
$\begin{array}{l} x-x_{0}=-f \frac{a_{1}\left( X-X_{s}\right)+b_{1}\left(Y-Y_{s}\right)+c_{1}\left(Z-Z_{s}\right)}{a_{3}\ izquierda(X-X_{s}\derecha)+b_{3}\izquierda(Y-Y_{s}\derecha)+c_{3}\izquierda(Z-Z_{s}\derecha)} \\ y- y_{0}=-f \frac{a_{2}\left(X-X_{s}\right)+b_{2}\left(Y-Y_{s}\right)+c_{2}\left (Z-Z_{s}\right)}{a_{3}\left(X-X_{s}\right)+b_{3}\left(Y-Y_{s}\right)+c_{3} \left(Z-Z_{s}\right)} \end{matriz}$
由共线条件方程得到三角化严密解法，将分母移到左边，得到
$\begin {matriz}{c} \left(x-x_{0}\right)\left[a_{3}\left(X-X_{s}\right)+b_{3}\left(Y-Y_{s }\right)+c_{3}\left(Z-Z_{5}\right)\right]= \\ -f\left[a_{1}\left(X-X_{s}\right)+b_ {1}\left(Y-Y_{s}\right)+c_{1}\left(Z-Z_{s}\right)\right] \\ \left(y-y_{0}\right)\ izquierda[a_{3}\izquierda(X-X_{s}\derecha)+b_{3}\izquierda(Y-Y_{s}\derecha)+c_{3}\izquierda(Z-Z_{s}\ derecha)\derecha]= \\ -f\izquierda[a_{2}\izquierda(X-X_{s}\derecha)+b_{2}\izquierda(Y-Y_{s}\derecha)+c_{2 }\left(Z-Z_{s}\right)\right] \end{matriz}$
整理可得：
$\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{matriz}$
Ejemplo:
$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}+\left(x-x_{b}\right) a_{3} X_{ s}+\left(x-x_{0}\right) b_{3} Y_{s}+\left\langle x-x_{b}\right) c_{3} Z_{s}$

$l_{4}=fa_{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}+\left(y-y_{0}\right) a_{3} X_{s}+\left(y-y_{0}\right) b_{3} Y_{s}+\left(y-y_{0}\right) c_{3} Z_{s}$

De lo anterior se puede ver que un píxel puede enumerar 2 ecuaciones, y para n píxeles con el mismo nombre, se pueden enumerar 2n ecuaciones (AX=B, la ecuación sobredeterminada se resuelve de acuerdo con los mínimos cuadrados), es decir, multi- triangulación de cuadros, el código es el siguiente:

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. Triangulación iterativa

El método es agregar factores a los coeficientes de la ecuación y ajustar continuamente los factores de los coeficientes. Mi código es el siguiente:

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. Triangulación iterativa de múltiples cuadros de selección de peso

Primero explique la iteración de selección de peso (algoritmo IGG).En la década de 1980, el primer método de iteración de selección de peso derivado de la estimación de la varianza posterior para localizar el error bruto se denominó "método Li Deren". Debido a la limitación de las condiciones objetivas en el trabajo de medición real, es difícil evitar por completo la existencia de errores graves o lograr la misma precisión de medición.En el proceso de ajuste, el peso se suele introducir como un índice para comparar el relativo precisión entre observaciones, y para los datos de observación con mayor precisión se les da mayor peso, de modo que se pueda evitar la interferencia de información dañina. Por ejemplo, cuando hacemos coincidir la coincidencia de imágenes, usamos ransac (también un algoritmo de estimación robusto) para eliminar los valores atípicos, pero cuando su punto con el mismo nombre está en varios fotogramas y solo hay uno (como la medición de posición de múltiples marco de semáforos), ransac ya no se puede usar. En este momento, el uso del algoritmo IGG puede evitar efectivamente el impacto de los errores. El documento se refiere a la estimación robusta de mínimos cuadrados totales de la intersección directa del espacio de múltiples imágenes. Tengo reprodujo el algoritmo del artículo. El código es el siguiente:

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 ellas, la función mutiTriangle y la función weight_mutiTriangle son las siguientes:

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

referencias:

1. Estimación robusta de mínimos cuadrados totales para la intersección directa en el espacio de imágenes múltiples[J].Li Zhongmei. Journal of Surveying and Mapping

Métodos de triangulación en fotogrametría (visión artificial)

Métodos de triangulación en fotogrametría (visión artificial)

Supongo que te gusta