多视几何_计算一副图像上的点在另一福图像上的对应点

深碍√TFBOYSˉ_ 2022-12-29 02:27 66阅读 0赞

# 功能说明: #

*  输入: 图像i上的点m, 图像点m在该图像坐标系下的深度, 图像i(2维的)对应的相机坐标系(3维的) 到 图像j对应的相机坐标系 之间的变换Tji
 *  输出: 图像j上与图像m对应的点m’

# 图示: #

![左边为图像i,右边为图像j][i_j]

# 实现: #

//其中: mK为相机的内参, mKinv 为相机内参的逆.  
// 对应的点是否在图像平面内，在本函数中未判断，使用时需要先确定在图像界限内

def get_correspondence_v(point_u, d_u, Tji):
            """
            find correspondence point_v in frame j for  point_u in frame i 
            Args:
                point_u (numpy, (2,) ): (x,y) point u in frame i
                d_u (float): the depth value of point_u in frame i
                Tji (numpy, 4x4): the transformation from frame i to  frame j
    
            Returns:
                point_v (numpy, (2,) ): (x',y') point v in frame j
            """
    
            homo_point_u = np.array([ [point_u[0]], [point_u[1]], [1] ]) # shape:(3,1)
    
            point3d_in_frame_i = np.dot(self.mKinv, homo_point_u) *d_u # shape:(3,1)
            homo_point3d_in_frame_i = np.array([   	[point3d_in_frame_i[0][0]], # shape:(4,1)
     												[point3d_in_frame_i[1][0]], 
     												[point3d_in_frame_i[2][0]], 
    												[1] ])                                                
            homo_point3d_in_frame_j = np.dot(Tji, homo_point3d_in_frame_i) # shape:(4,1)
            point3d_in_frame_j = np.array([  	[homo_point3d_in_frame_j[0][0]], # shape:(3,1)
     											[homo_point3d_in_frame_j[1][0]], 
     											[homo_point3d_in_frame_j[2][0]] ]) 
            tmp =  np.dot(self.mK, point3d_in_frame_j) # shape:(3,1)
            point_v =  np.array( [round( tmp[0][0]/tmp[2][0] ) , round( tmp[1][0]/tmp[2][0] ) ] ) # shape:(2,)
    
            return point_v

[i_j]: /images/20221120/40006857e09444b8a49eb12b4b69f360.png