bug fix

[18128381510]

对比DenseDepthMap，作者代码存在问题，具体表现为会出现nan值，且生成的深度图不准确，修改后的代码示例如下：

# Pts:(n,3),lidarOnImageCoord:(u,v,d)
# n:img_h, m:img_w
# grid: A grid size of 4 means a 9x9 neighbourhood is used and weighted depth information is calculated according to the distance of the neighbourhood.
def dense_map(Pts, n, m, grid, min_depth_limit=0.01):
    ng = 2 * grid + 1

    # Initialize matrices
    mX = np.full((n, m), np.inf)
    mY = np.full((n, m), np.inf)
    mD = np.zeros((n, m))

    # Compute linear indices
    linearindex = np.ravel_multi_index((np.round(Pts[:, 1]).astype(int), np.round(Pts[:, 0]).astype(int)), (n, m))

    # Populate matrices
    mX.flat[linearindex] = Pts[:, 0] - np.round(Pts[:, 0])
    mY.flat[linearindex] = Pts[:, 1] - np.round(Pts[:, 1])
    mD.flat[linearindex] = Pts[:, 2]

    # Prepare KmX, KmY, KmD
    KmX = np.zeros((ng, ng, n - ng, m - ng))
    KmY = np.zeros((ng, ng, n - ng, m - ng))
    KmD = np.zeros((ng, ng, n - ng, m - ng))

    for i in range(ng):
        for j in range(ng):
            KmX[i, j] = mX[i:n - ng + i, j:m - ng + j] - grid - 1 + i
            KmY[i, j] = mY[i:n - ng + i, j:m - ng + j] - grid - 1 + j
            KmD[i, j] = mD[i:n - ng + i, j:m - ng + j]

    S = np.zeros(KmD[0, 0].shape)
    Y = np.zeros(KmD[0, 0].shape)

    for i in range(ng):
        for j in range(ng):
            s = 1.0 / np.sqrt(KmX[i, j] ** 2 + KmY[i, j] ** 2)
            Y += s * KmD[i, j]
            S += s

    S[S == 0] = 1
    out = np.zeros((n, m))
    out_mask = np.zeros((n, m), dtype=np.double)
    out[grid + 1: -grid, grid + 1: -grid] = Y / S
    out_mask[out>min_depth_limit]=1

    return out,out_mask

# lidar_points:(n,3) in world
# R:W2C (3,3)
# t:W2C (3)
def generate_dense_lidar_depth(R,t,K,lidar_points,img_h,img_w,near_depth_limit=0.01,grid_size=4):
    xyd = R @ (lidar_points.T) + t[:, None]  # (3,n)
    uvd = K @ xyd  # (3,n)
    uvd_in_image = np.concatenate(((uvd[:2, :] / uvd[2:, :]), uvd[2:, :]), axis=0)  # (3,n)
    # Normalize to get the pixel coordinates
    uv = np.round(uvd_in_image[:2, :]).astype(np.int64) # (2,n)

    mask_in = (uv[0, :] >= 0) * (uv[0, :] < img_w) * \
              (uv[1, :] >= 0) * (uv[1, :] < img_h) * \
              (uvd[2, :] > near_depth_limit)  # (n)

    uvd_in_image = uvd_in_image[:, mask_in]  # (3,n_f)

    dense_depth,depth_mask = dense_map(uvd_in_image.T,img_h,img_w,grid_size,min_depth_limit=near_depth_limit)
    return dense_depth,depth_mask