相机与激光雷达联合标定（二）：联合标定（未完待续）

一. Matlab

Matlab2020及其以上版本，

哔哩哔哩：激光雷达和相机的标定

CSDN：2022使用MATLAB进行相机和激光雷达的标定（有重大更新）

二. OpenCV

我们假设已经求出了相机的内参K，并且已知点在图像上的坐标以及点在激光雷达下的坐标。

import math
import cv2 as cv
import numpy as np
import open3d as o3d
from matplotlib import pyplot as plt


PointsInImg = np.array([[1041.,  144.],
                        [1376.,  463.],
                        [ 906.,  967.],
                        [ 568.,  654.],
                        [ 861.,  131.],
                        [1347.,  613.],
                        [1037.,  949.],
                        [ 525.,  454.]])

PointsInLid = np.array([[ 0.1372 ,  1.54206,  0.27411],
                        [ 0.35176,  1.55096, -0.07609],
                        [-0.04767,  1.54776, -0.49878],
                        [-0.30885,  1.53051, -0.18308],
                        [-0.08583,  1.5382 ,  0.27606],
                        [ 0.3382 ,  1.55958, -0.16804],
                        [ 0.0803 ,  1.51905, -0.48176],
                        [-0.30631,  1.4914 , -0.0582 ]])

# cv.solvePnP
Intrinsics = np.array([[1693.812,0       ,984.510],
                       [0       ,1700.520,511.520],
                       [0       ,0       ,      1]])
Dist = np.array([[0.0933864283576713,-0.552732596154694,0,0]])

_,r,t = cv.solvePnP(PointsInLid,PointsInImg,Intrinsics,Dist,flags=1)
"""
SOLVEPNP_ITERATIVE = 0,
SOLVEPNP_EPNP      = 1, //!< EPnP: Efficient Perspective-n-Point Camera Pose Estimation @cite lepetit2009epnp
SOLVEPNP_P3P       = 2, //!< Complete Solution Classification for the Perspective-Three-Point Problem 
SOLVEPNP_DLS       = 3, //!< A Direct Least-Squares (DLS) Method for PnP  @cite hesch2011direct
SOLVEPNP_UPNP      = 4, //!< Exhaustive Linearization for Robust Camera Pose and Focal Length Estimation 
SOLVEPNP_AP3P      = 5, //!< An Efficient Algebraic Solution to the Perspective-Three-Point Problem 
SOLVEPNP_MAX_COUNT      //!< Used for count
"""

# 将旋转向量rvec转换为旋转矩阵R
R, _ = cv.Rodrigues(r)
Rt = np.concatenate((R, t.reshape((-1,1))), axis=1)
Translation = np.concatenate((Rt,np.array([[0,0,0,1]])), axis=0)
"""print(Translation)
[[ 0.9979607   0.05368444 -0.03453153 -0.12919706]
 [-0.05351042  0.4086693  -0.91111253 -0.72221526]
 [-0.03480059  0.91110229  0.41070857  0.07698976]
 [ 0.          0.          0.          1.        ]]"""

import cv2
import numpy as np
import pandas as pd
import open3d as o3d
from mayavi import mlab
from matplotlib import pyplot as plt


# 已知相机内参K
# [[fx,  0 , cx],
#  [0 ,  fy, cy],
#  [0 ,  0 , 1 ]]
Intrinsics = np.array([[1693.812,0       ,984.510],
                       [0       ,1700.520,511.520],
                       [0       ,0       ,      1]])

# 已知激光雷达到相机的变换矩阵 T
Translation = np.array([[ 0.99796070,  0.05368444, -0.034531530, -0.12919706],
                        [-0.05351042,  0.40866930,  -0.91111253, -0.72221526],
                        [-0.03480059,  0.91110229,  0.410708570,  0.07698976],
                        [ 0.,          0.,          0.,           1.,        ]])

PointsInLid_dataframe = pd.read_csv("D:\\PyCharm_project\\B2-JiaoDianJianCe\\biaodingban-231104-001.csv")

PointsInLid_array = np.asarray(PointsInLid_dataframe[['Points:0','Points:1','Points:2']])
# 只保留'intensity','vertical_angle','Points:0','Points:1','Points:2'

PointsInLid_hsoe = np.concatenate((PointsInLid_array, np.ones((PointsInLid_array.shape[0], 1))), axis=1)
# 将点云坐标系变成齐次坐标系
Lid2Cam = np.dot(Translation, PointsInLid_hsoe.T).T
Intrinsics_extend = np.concatenate((Intrinsics, np.zeros((Intrinsics.shape[0], 1))), axis=1)
Cam2Img = np.dot(Intrinsics_extend, Lid2Cam.T).T
PointsInImg = np.int0(Cam2Img/Cam2Img[:, -1:])

img=cv2.imread('D:\\PyCharm_project\\B2-JiaoDianJianCe\\video001.jpg') #原图为彩色图，可将第二个参数变为0，为灰度图
img_RGB = cv2.cvtColor(img,cv2.COLOR_BGR2RGB)


for i in PointsInImg: # type: ignore
    x,y,_ = i.ravel()
    #print((x,y))
    cv2.circle(img_RGB,(x,y),2,(255,0,0),-1)
plt.imshow(img_RGB)
plt.show()