1.1 手眼标定

1.求转换矩阵B

import numpy as np


# 三维和二维之间的理论，暂时不用


# [u,v,1]=[ [a11,a12,a13,b1],
#          [a21,a22,a23,b2],
#          [a31,a32,a33,b3]] *[x,y,z,1]    矩阵共12个未知量，其中[u,v]是目标的相机坐标，[x,y,z]是雷达坐标，
#          将要求的矩阵是将雷达坐标转换为相机坐标的转换矩阵

#  Y    =       (A,b)        *    X     ----> 即 Y= AX+b ---->  X=A-1(Y-b)   #思路：如左边的矩阵方程，
#            求出(A,b)后反过来求出相机的二维坐标到雷达的三维坐标的转化矩阵，从而得到两个坐标的相互转换关系

# 由上面的矩阵方程可得
# a11*x+a12*y+a13*z+b1=u
# a21*x+a22*y+a23*z+b2=v
# a31*x+a32*y+a33*z+b3=1

# 将上面的方程组变形为求解的形式后可得：输入雷达坐标(x,y,z),相机坐标(u,v)
    # a11*x+a12*y+a13*z+b1*1+ a21*0+a22*0+a23*0+b2*0+ a31*0+a32*0+a33*0+b3*0=u
    # a11*0+a12*0+a13*0+b1*0+ a21*x+a22*y+a23*z+b2*1+ a31*0+a32*0+a33*0+b3*0=v
    # a11*0+a12*0+a13*0+b1*0+ a21*0+a22*0+a23*0+b2*0+ a31*x+a32*y+a33*z+b3*1=1

# 以上方程得到线性方程矩阵A为[[x,y,z,1,0,0,0,0,0,0,0,0],
#                      [0,0,0,0,x,y,z,1,0,0,0,0],
#                      [0,0,0,0,0,0,0,0,x,y,z,1]]
# 以上方程得到的b为[[u],[v],[1]]

# 用以上方程求解矩阵的参数(共12个未知量,至少需要4组坐标能得唯一解，多于4组更好，此时会有无解的情况，此时会求拟合的相对最优解)

# 输入(x,y,z),(u,v)
def input_camera_lidar(listdir, image_size):
    A = []
    b = []
    for item in listdir:
        xyz = item[0]
        uv = item[1]

        x = xyz[0]
        y = xyz[1]
        z = xyz[2]
        # r=xyz[3]

        # 雷达坐标转换如下（x,y,z）=====>(z/x,y/x,1),这种转换是因为不同距离x下相同的（y,z)在像素坐标系中的坐标不同，所以必须有这样一步处理
        x0, y0 = z / x, y / x
        # 建立方程组
        # a11*x0+a12*y0+b1*1+ a21*0+a22*0+b2*0+ a31*0+a32*0+b3*0=u
        # a11*0+a12*0+b1*0+ a21*x0+a22*y0+b2*1+ a31*0+a32*0+b3*0=v
        # a11*0+a12*0+b1*0+ a21*0+a22*0+b2*0+ a31*x0+a32*y0+b3*1=1
        A1 = [[x0, y0, 1, 0, 0, 0, 0, 0, 0], [0, 0, 0, x0, y0, 1, 0, 0, 0], [0, 0, 0, 0, 0, 0, x0, y0, 1]]
        # u=uv[0]-image_size[0]/2
        # v=uv[1]-image_size[1]/2
        # v=[[371.33757634,0.,306.01237024],[0.,374.79184398,208.74053634],[  0.,0.,1.]]

        #转换为图像坐标系
        u = uv[0] - image_size[0] / 2
        v = uv[1] - image_size[1] / 2

        b1 = [[u], [v], [1]]
        # print("b",b1)

        A.append(A1[0])
        A.append(A1[1])
        A.append(A1[2])

        b.append(b1[0])
        b.append(b1[1])
        b.append(b1[2])
    print(A)
    print(b)

    ss = np.linalg.pinv(A).dot(b)
    # print(ss)
    B = [[ss[0][0], ss[1][0], ss[2][0]],
         [ss[3][0], ss[4][0], ss[5][0]],
         [ss[6][0], ss[7][0], ss[8][0]],
         ]
    return B


if __name__ == "__main__":
    image_size = [480, 640]

    listdir = [[[2243.7, -896.7,  -219.7], [423, 268]],
               [[ 2261.4, -899.8, 429.1], [424, 167]],
               [[ 2263.5,  301.9,  435.6], [236, 163]],
               [[ 2237.3, 309.4, -243.6], [233, 270]],
               [[2953.3,  -862.3,  405.4], [392, 184]],

               ]  # 这个坐标根据实际测量获得,[[3029, 1620, -449.6,R], [115, 276]]   [[, ,], [,]]

    B = input_camera_lidar(listdir, image_size)
    print(B)

2.求验证矩阵

import numpy as np


def output_lidars(lidar_index,B,image_size,imggg):

    print("...................相机坐标......................")
    lidar_index = np.array(lidar_index)
    lidar_indexs=lidar_index.copy()

    l_i = [lidar_index[:, -1:] / lidar_index[:,:1],lidar_index[:, 1:2] / lidar_index[:,:1]]
    l_i = np.array(l_i)
    s=l_i.reshape(l_i.shape[0],l_i.shape[1])
    ll=np.ones((1,lidar_index.shape[0]))
    sr=np.r_[s, ll]

    b1=[[image_size[0]/2],[image_size[1]/2],[1]]
    b1=np.array(b1)
    b1=b1.reshape(3,1)

    lidar_index=np.array(sr)
    B=np.array(B)

    ss = np.dot(B, lidar_index)+b1
    outs=ss[:-1,:].T
    #..................以上代码将点云坐标转换为了相机坐标,将对以下两个坐标进行处理............................
        #outs:点云坐标转换后的相机坐标
        #lidar_indexs:点云坐标
    # 找在框imggg内的坐标
    print(outs)
    s1 = outs[:, :1] > imggg[0]
    s2 = outs[:, :1] < imggg[1]
    s3 = outs[:, -1:] > imggg[2]
    s4 = outs[:, -1:] < imggg[3]
    s = (s1 * s2 * s3 * s4)
    s = s.reshape(s.shape[0])
    # 获得满足以上4个条件的坐标位置
    num_index = np.where(s == True)
    # 根据坐标位置提取满足目标坐标位置的点云坐标
    datas_lidar = lidar_indexs[num_index]
    return datas_lidar

if __name__ == "__main__":

    B =[[9.158053666595578, -355.71742549373624, 42.84214066984505], [-353.76086909075116, -4.310800181940799, -88.183369802313], [-2.1371793224034263e-13, -7.222000775186643e-14, 1.0000000000000104]]

    image_size = [480, 640]
    imggg=np.array([200,400,100,300])#目标的坐标框：

    lidar_index = [
        [2243.7, -896.7, -219.7],
        [2261.4, -899.8, 429.1],
        [2263.5, 301.9, 435.6],
        [2237.3, 309.4, -243.6],
        [2953.3,  -862.3,  405.4],
        [ 2896.4, 318.0, 393.4],
                 ]

    datas_lidar=output_lidars(lidar_index,B,image_size,imggg)