1、關于标定檔案
本人采用的是autoware的标定工具實作的标定，标定檔案如下所示。此處應注意autoware的标定檔案與其他一般性的标定檔案相比的不同之處，autoware的标定檔案的特殊性及其使用可參見我的其他部落格。
2、利用openc的projectpoints函數實作坐标轉換。
查詢opencv文檔中關于ProjectPoints2函數的說明，相關參數的解釋部分展示如下
void cvProjectPoints2( const CvMat* object_points, const CvMat* rotation_vector,
const CvMat* translation_vector, const CvMat* intrinsic_matrix,
const CvMat* distortion_coeffs, CvMat* image_points,
CvMat* dpdrot=NULL, CvMat* dpdt=NULL, CvMat* dpdf=NULL,
CvMat* dpdc=NULL, CvMat* dpddist=NULL );
*object_points：*物體點的坐标，為3xN或者Nx3的矩陣，這兒N是視圖中的所有所有點的數目。
*rotation_vector：*旋轉向量，1x3或者3x1。由旋轉向量通過羅德裡格斯變換得到。
*translation_vector：*平移向量，1x3或者3x1。
[ f x 0 c x 0 f x c y 0 0 1 ] \left[ \begin{matrix} fx & 0 & cx \\ 0 & fx & cy \\ 0 & 0 & 1 \end{matrix} \right] ⎣⎡fx000fx0cxcy1⎦⎤
*intrinsic_matrix：*錄影機内參數矩陣A
*distortion_coeffs：*形變參數向量，4x1或者1x4，為[k1,k2,p1,p2]。如果是NULL，所有形變系數都設為0。
*image_points：*輸出數組，存儲圖像點坐标。大小為2xN或者Nx2，這兒N是視圖中的所有點的數目。
*dpdrot、dpdt、dpdf、dpdc、dpddist：*都為可選參數。
在python中調用該函數如下，注意代碼各參數與上述說明的對應關系：
執行個體如下：
#-*- coding:utf-8 -*-
import pcl
import cv2 as cv
from pylab import *
cloud = pcl.load('/home/song/pcl_test/test.pcd')
points_3d = []
for i in range(0, cloud.size):
x_raw = float(cloud[i][0])
y_raw = float(cloud[i][1])
z_raw = float(cloud[i][2])
point_3d = []
point_3d.append(x_raw)
point_3d.append(y_raw)
point_3d.append(z_raw)
points_3d.append(point_3d)
#輸入projectpoints函數的各項參數數值。
cube = np.float64(points_3d)
rvec = np.float64([1.15230820843793, -1.268126207316489, 1.238632946715113])
tvec = np.float64([-1.6263959455325000e-01, -1.6604515286304664e-02, -9.2349015818599600e-01])
camera_matrix = np.float64([[1.0251232727914867e+03, 0, 2.6218085751008294e+02],
[0, 1.0180807639500672e+03, 2.0907527955373126e+02],
[0, 0, 1]])
distCoeffs = np.float64([-4.6155288063657413e-01, 2.2493781629153636e-01,
2.5584176644194096e-03, 7.5604590879245655e-04, 3.1795661983727808e-01])
point_2d, _ = cv.projectPoints(cube, rvec, tvec, camera_matrix, distCoeffs)
print(point_2d)
結果如下：
3、将二維坐标投影到圖檔上
這一步主要是對上一步得到的所有二維坐标進行過濾去除相機後方的點和超出圖像尺寸外的點。再者就是利用scatter函數借助colormap對投影的點進行着色。完整代碼如下：
#-*- coding:utf-8 -*-
import pcl
import numpy as np
import cv2 as cv
from PIL import Image
from pylab import imshow
from pylab import array
from pylab import plot
from pylab import title
from pylab import *
import matplotlib.pyplot as plt
x=[]
y=[]
distance=[] #存放需要投影點轉換成二維前的雷達坐标的x坐标（距離資訊），以此為依據對投影點進行染色。
distance_3d=[] #存放轉換前雷達坐标的x坐标（距離資訊）。
cloud = pcl.load('/home/song/pcl_test/test.pcd')
im = Image.open('/home/song/pcl_test/test.jpg')
pix = im.load()
points_3d = []
# print('Loaded ' + str(cloud.width * cloud.height) +
# ' data points from test_pcd.pcd with the following fields: ')
for i in range(0, cloud.size):
x_raw = float(cloud[i][0])
y_raw = float(cloud[i][1])
z_raw = float(cloud[i][2])
point_3d = []
point_3d.append(x_raw)
point_3d.append(y_raw)
point_3d.append(z_raw)
if x_raw>0:
points_3d.append(point_3d)
distance_3d.append(x_raw)
cube = np.float64(points_3d)
rvec = np.float64([1.15230820843793, -1.268126207316489, 1.238632946715113])
tvec = np.float64([-1.6263959455325000e-01, -1.6604515286304664e-02, -9.2349015818599600e-01])
camera_matrix = np.float64([[1.0251232727914867e+03, 0, 2.6218085751008294e+02],
[0, 1.0180807639500672e+03, 2.0907527955373126e+02],
[0, 0, 1]])
distCoeffs = np.float64([-4.6155288063657413e-01, 2.2493781629153636e-01,
2.5584176644194096e-03, 7.5604590879245655e-04, 3.1795661983727808e-01])
point_2d, _ = cv.projectPoints(cube, rvec, tvec, camera_matrix, distCoeffs)
m=-1
for point in point_2d:
m=m+1
x_2d = point[0][0]
y_2d = point[0][1]
if 0<=x_2d<=640 and 0<=y_2d<=480:
x.append(x_2d)
y.append(y_2d)
distance.append(-distance_3d[m]*100)#數值取反是為了讓colormap顔色由紅到藍顯示而非由藍到紅
RGB=pix[x_2d,y_2d]
print('x,y,z,(r,g,b):',([x_2d,y_2d,distance_3d[m]],RGB))
x=np.array(x)
y=np.array(y)
plt.scatter(x, y, c=distance, cmap='jet',s=4,marker='.')
plt.imshow(im)
plt.show()
結果如下：上面的為autoware投影的效果圖，下面的為自己手動投影的效果圖。
由于本次實驗标定時抽取幀數較少，标定效果較差，本文重點在于功能的實作。另外對于點雲的染色，采用目前的染色方案，效果不是很好，仍在研究，可自己設計。