Triangulation methods in photogrammetry (computer vision)

Everyone is familiar with triangulation. In the field of CV, the process of calculating object points from image points is called triangulation, but in the field of photogrammetry, it is called forward intersection. It is worth noting that a single image cannot restore the 3D coordinates of the pixel, at least two images are required to obtain the real coordinates of the pixel (the pose information of the two images is known here)

insert image description here

There are many methods of triangulation. Here we introduce two-frame triangulation, multi-frame triangulation, iterative triangulation, and weight- selection iterative multi-frame triangulation (with my code attached).

1. Two-frame triangulation

In opencv, the function triangulatePoints can restore the three-dimensional point coordinates according to the pose and internal parameters of the two frames. The triangulation in cv is two frames and has no right.

Its function parameters are as follows:

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. Multi-frame triangulation

三角化严密解推导过程：
$\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}\left(X-X_{s}\right)+b_{3}\left(Y-Y_{s}\right)+c_{3}\left(Z-Z_{s}\right)} \\ 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{array}$
由共线条件方程得到三角化严密解法，将分母移到左边，得到
$\begin{array}{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)\left[a_{3}\left(X-X_{s}\right)+b_{3}\left(Y-Y_{s}\right)+c_{3}\left(Z-Z_{s}\right)\right]= \\ -f\left[a_{2}\left(X-X_{s}\right)+b_{2}\left(Y-Y_{s}\right)+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}$
其中：
$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}=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}+\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}$

It can be seen from the above that one pixel can list 2 equations, and for n pixels with the same name, 2n equations can be listed (AX=B, the overdetermined equation is solved according to least squares), that is, multi-frame triangulation, the code is as follows :

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. Iterative triangulation

The method is to add factors to the coefficients of the equation and continuously adjust the factors of the coefficients. My code is as follows:

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. Weight selection iterative multi-frame triangulation

First explain the weight selection iteration (IGG algorithm). In the 1980s, the first weight selection iteration method derived from the posterior variance estimation to locate the gross error was named "Li Deren method". Due to the limitation of objective conditions in the actual measurement work, it is difficult to completely avoid the existence of gross errors or to achieve the same accuracy measurement. In the process of adjustment, weight is usually introduced as an index to compare the relative accuracy between observations, and for Observational data with higher precision are given higher weights, so that the interference of harmful information can be avoided. For example, when we match image matching, we will use ransac (also a robust estimation algorithm) to eliminate outliers, but when your point with the same name is on multiple frames and there is only one (such as the position measurement of multi-frame traffic lights) , ransac can no longer be used. At this time, using the IGG algorithm can effectively avoid the impact of the error point. The paper refers to the robust total least squares estimation of multi-image space forward intersection. I reproduce the algorithm of the paper. The code is as follows :

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

Among them, the mutiTriangle function and weight_mutiTriangle function are as follows:

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

references:

1. Robust Total Least Squares Estimation for Forward Intersection in Multiple Image Space[J]. Li Zhongmei. Journal of Surveying and Mapping

Triangulation methods in photogrammetry (computer vision)

Triangulation methods in photogrammetry (computer vision)

Guess you like