//  [python]
def unproject2_aligned_depth(p2d, Kdist, Mdist, distCoeffs):
    # st()
    # pn2d = torch.solve(Kdist, torch.cat((p2d[:,:2].T, torch.ones((1, p2d.shape[0]), device=device)), axis=0)).T
    
    pn2d, _ = torch.solve(torch.cat((p2d[:,:2].T, torch.ones((1, p2d.shape[0])).cuda().double()), axis=0), Kdist)
    pn2d = pn2d.T
    if vis_check_flag:
        print(f'pn2d.mean() {pn2d.mean()}')
    # pn2d = torch.linalg.solve(Kdist, torch.cat((p2d[:,:2].T, torch.ones((1, p2d.shape[0]), device=device)), axis=0)).T
    # print(f'pn2d.mean() {pn2d.mean()}')
    # st()
    # k = torch.cat([distCoeffs[:5], np.zeros(12-5)], axis=0)
    k = torch.cat((distCoeffs[:5], torch.zeros(12-5).cuda().double()), axis=0)
    # k = torch.cat((distCoeffs[:len(distCoeffs)], torch.zeros(12-len(distCoeffs)).cuda().double()), axis=0)
    x0 = pn2d[:,0]
    y0 = pn2d[:,1]
    x = x0
    y = y0
    # print(f'x {x}')
    # print(f'y {y}')
    for iter in range(5):
        r2 = x*x + y*y
        # st()
        icdist = (1 + ((k[7]*r2 + k[6])*r2 + k[5])*r2)/(1 + ((k[4]*r2 + k[1])*r2 + k[0])*r2)
        deltaX = 2*k[2]*x*y + k[3]*(r2 + 2*x*x)+ k[8]*r2+k[9]*r2*r2
        deltaY = k[2]*(r2 + 2*y*y) + 2*k[3]*x*y+ k[10]*r2+k[11]*r2*r2
        x = (x0 - deltaX)*icdist
        y = (y0 - deltaY)*icdist
    
    pn2d = torch.cat((x.unsqueeze(-1), y.unsqueeze(-1)), axis=1)
    
    if vis_check_flag:
        print(f'pn2d.mean() {pn2d.mean()}')
        print(f'p2d.mean() {p2d.mean()}')
        print(f'(p2d[:,2:3]*0.001).mean() {(p2d[:,2:3]*0.001).mean()}')
        print(f'(pn2d * (p2d[:,2:3]*0.001)).mean() {(pn2d * (p2d[:,2:3]*0.001)).mean()}')
        print(f'pn2d {pn2d}')
    
    p3d = torch.cat([pn2d * (p2d[:,2:3]*0.001), p2d[:,2:3]*0.001], axis=1)
    # p3d = np.concatenate([pn2d * (p2d[:,2:3]*0.001), p2d[:,2:3]*0.001], axis=1)
    
    # p3d = np.matmul(np.linalg.inv(np.row_stack([Mdist, [0,0,0,1]])), np.concatenate([p3d, np.ones((p3d.shape[0], 1))], axis=1).T).T
    
    p3d = torch.matmul(torch.inverse(torch.cat([Mdist, torch.DoubleTensor([[0,0,0,1]]).cuda()], dim=0)), torch.cat((p3d, torch.ones((p3d.shape[0], 1)).cuda().double()), axis=1).T).T
    # if vis_check_flag:
    #     aaa = torch.inverse(torch.cat([Mdist, torch.DoubleTensor([[0,0,0,1]]).cuda()], dim=0))
    #     bbb = torch.cat((p3d, torch.ones((p3d.shape[0], 1)).cuda().double()), axis=1).T
    #     ccc = torch.matmul(aaa, bbb).T
    #     print(f'aaa {aaa}')
    #     print(f'bbb {bbb}')
    #     print(f'ccc {ccc}')
    # st()
    if vis_check_flag:
        print(f'p3d.mean() {p3d.mean()}')
    # st()
    
    # print(p3d.mean(axis=0))
    # st()
    # p3d = np.matmul(np.linalg.inv(np.row_stack([Mdist, [0,0,0,1]])), torch.cat([p3d, np.ones((p3d.shape[0], 1))], axis=1).T).T
    # print(p3d.mean(axis=0))
    p3d = p3d[:,:3]
    # p3d = (inv([Mdist;[0 0 0 1]])*[p3d ones(size(p3d,1),1)]')'
    # p3d = (inv([cam.Mdist;[0 0 0 1]])*[p3d ones(size(p3d,1),1)]')';
    # p3d = [bsxfun(@times, pn2d, p2d(:,3)*0.001) p2d(:,3)*0.001];
    ### 估计没用
    # if pn2d.shape[1] == 1:
    #     pn2d = pn2d.T
    # st()
    # pn2d = [x y]
    # for iter=1:5
    #     r2 = x.*x + y.*y;
    #     icdist = (1 + ((k(1+7)*r2 + k(1+6)).*r2 + k(1+5)).*r2)./(1 + ((k(1+4)*r2 + k(1+1)).*r2 + k(1+0)).*r2);
    #     deltaX = 2*k(1+2)*x.*y + k(1+3)*(r2 + 2*x.*x)+ k(1+8)*r2+k(1+9)*r2.*r2;
    #     deltaY = k(1+2)*(r2 + 2*y.*y) + 2*k(1+3)*x.*y+ k(1+10)*r2+k(1+11)*r2.*r2;
    #     x = (x0 - deltaX).*icdist;
    #     y = (y0 - deltaY).*icdist;
    # end
    # k = [reshape(cam.distCoeffs(1:5),[],1); zeros(12-5,1)];
    # pn2d = ( cam.Kdist \ [p2d(:,1:2)'; ones(1, size(p2d,1))] )';
    return p3d