激光雷達和單目攝像頭的外參pnp方法標定

使用簡單的pnp方法標定世界座標系下的激光雷達3D Point 和攝像頭座標系下的2D 旋轉矩陣和位移

相關工具：

• Numpy
• Matplotlib
• CV2
• ROS（cv_bridge/TF2/ros_numpy/sensor.msgs/message_filters/image_geomotry）

主要思路是：

從激光雷達和相機保存的點雲和圖片利用matplotlib的gui交互將可視化的角點保存爲numpy格式文件，之後利用鍵盤的handle採集保存多張圖片和點雲。將保存好的numpy文件導入到slovePnPRansac進行計算，最後在終端打印結果

solvePnPRansac相關：

/*  max 註釋
*   函數功能：用ransac的方式求解PnP問題
*
*   參數：
*   [in]    _opoints                參考點在世界座標系下的點集；float or double
*   [in]    _ipoints                參考點在相機像平面的座標；float or double
*   [in]    _cameraMatrix           相機內參
*   [in]    _distCoeffs             相機畸變係數
*   [out]   _rvec                   旋轉矩陣
*   [out]   _tvec                   平移向量
*   [in]    useExtrinsicGuess       若果求解PnP使用迭代算法，初始值可以使用猜測的初始值（true），也可以使用解析求解的結果作爲初始值（false）。
*   [in]    iterationsCount         Ransac算法的迭代次數，這只是初始值，根據估計外點的概率，可以進一步縮小迭代次數；（此值函數內部是會不斷改變的）,所以一開始可以賦一個大的值。
*   [in]    reprojectionErrr        Ransac篩選內點和外點的距離閾值，這個根據估計內點的概率和每個點的均方差（假設誤差按照高斯分佈）可以計算出此閾值。
*   [in]    confidence              此值與計算採樣（迭代）次數有關。此值代表從n個樣本中取s個點，N次採樣可以使s個點全爲內點的概率。
*   [out]   _inliers                返回內點的序列。爲矩陣形式
*   [in]    flags                   最小子集的計算模型；
*                                                 SOLVEPNP_ITERATIVE(此方案，最小模型用的EPNP，內點選出之後用了一個迭代)；
*                                                 SOLVE_P3P(P3P只用在最小模型上，內點選出之後用了一個EPNP)      
*                                                 SOLVE_AP3P(AP3P只用在最小模型上，內點選出之後用了一個EPNP)
*                                                 SOLVE_EPnP(最小模型上&內點選出之後都採用了EPNP)
*    返回值：
*         成功返回true，失敗返回false；
*/
bool solvePnPRansac(InputArray _opoints, InputArray _ipoints,
	InputArray _cameraMatrix, InputArray _distCoeffs,
	OutputArray _rvec, OutputArray _tvec, bool useExtrinsicGuess,
	int iterationsCount, float reprojectionError, double confidence,
	OutputArray _inliers, int flags)

主要功能函數：

按[ENTER]啓動GUI，並通過選擇攝像機和LiDAR框架中棋盤格的四個角點來選擇相應的點。選擇了16個相應的點用於在棋盤的不同位置和深度進行校準。下面顯示了這樣一套這樣的點。OpenCV的PnP RANSAC方法用於查找相機和LiDAR之間的旋轉和平移變換。由於OpenCV的功能在內部對圖像進行校正，因此可以從未校正的圖像中拾取2D點。

• 讀取之前保存的npy格式文件
• 檢查Point 2D和Point 3D的shape是否能進行矩陣運算
• 檢查角點樹是否達到五個點
• 導入相機模型參數
• 開始計算，計算成功打印結果

Calibrate the LiDAR and image points using OpenCV PnP RANSAC

Requires minimum 5 point correspondences

Inputs:
    points2D - [numpy array] - (N, 2) array of image points
    points3D - [numpy array] - (N, 3) array of 3D points

Outputs:
    Extrinsics saved in PKG_PATH/CALIB_PATH/extrinsics.npz

def calibrate(points2D=None, points3D=None):
    # Load corresponding points
    folder = os.path.join(PKG_PATH, CALIB_PATH)
    if points2D is None: points2D = np.load(os.path.join(folder, 'img_corners.npy'))
    if points3D is None: points3D = np.load(os.path.join(folder, 'pcl_corners.npy'))
    
    # Check points shape
    #檢查角點的shape
    assert(points2D.shape[0] == points3D.shape[0])
    #檢查角點的數量【0】代表第一張圖像
    if not (points2D.shape[0] >= 5):
        rospy.logwarn('PnP RANSAC Requires minimum 5 points')
        return

    # Obtain camera matrix and distortion coefficients
    camera_matrix = CAMERA_MODEL.intrinsicMatrix()
    dist_coeffs = CAMERA_MODEL.distortionCoeffs()

    # Estimate extrinsics
    success, rotation_vector, translation_vector, _ = solvePnPRansac(points3D,
                                                                     points2D, camera_matrix, dist_coeffs, flags=SOLVEPNP_ITERATIVE)
    if not success: rospy.logwarn('Optimization unsuccessful')

    # Convert rotation vector
    rotation_matrix = Rodrigues(rotation_vector)[0]
    euler = euler_from_matrix(rotation_matrix)
    
    # Save extrinsics
    np.savez(os.path.join(folder, 'extrinsics.npz'),
        euler=euler, R=rotation_matrix, T=translation_vector.T)

    # Display results
    print('Euler angles (RPY):', euler)
    print('Rotation Matrix:', rotation_matrix)
    print('Translation Offsets:', translation_vector.T)

採集並保存Point 2D：

• 從ROS讀取到的msgs得經過cv_bridge轉化爲cv2格式
• 利用矯正參數矯正傳過來的圖像
• matplotlib創建GUI
• 利用event採集角點並顯示
• 用save函數保存當前圖片和角點文件

Runs the image point selection GUI process

Inputs:
    img_msg - [sensor_msgs/Image] - ROS sensor image message
    now - [int] - ROS bag time in seconds
    rectify - [bool] - to specify whether to rectify image or not 是否重新矯正

Outputs:
    Picked points saved in PKG_PATH/CALIB_PATH/img_corners.npy 將生成的numpy腳墊保存爲相應 numpy格式文件

def extract_points_2D(img_msg, now, rectify=False):
    # Log PID
    rospy.loginfo('2D Picker PID: [%d]' % os.getpid())

    # Read image using CV bridge
    try:
        #將ROS CV_bridge轉換來的格式轉化爲cv2格式
        img = CV_BRIDGE.imgmsg_to_cv2(img_msg, 'bgr8')
        #當發生異常是返回日誌e
    except CvBridgeError as e: 
        rospy.logerr(e)
        return

    # Rectify image 重新矯正攝像頭參數
    if rectify: CAMERA_MODEL.rectifyImage(img, img)
    #
    disp = cvtColor(img.copy(), COLOR_BGR2RGB)

    # Setup matplotlib GUI
    fig = plt.figure()

    ax = fig.add_subplot(111)
    ax.set_title('Select 2D Image Points - %d' % now.secs)
    ax.set_axis_off()
    ax.imshow(disp)

    # Pick points
    picked, corners = [], []
    def onclick(event):
        x = event.xdata
        y = event.ydata
        if (x is None) or (y is None): return

        # Display the picked point
        picked.append((x, y))
        corners.append((x, y))
        rospy.loginfo('IMG: %s', str(picked[-1]))

        if len(picked) > 1:
            # Draw the line
            temp = np.array(picked)
            ax.plot(temp[:, 0], temp[:, 1])
            ax.figure.canvas.draw_idle()

            # Reset list for future pick events
            del picked[0]

    # Display GUI
    fig.canvas.mpl_connect('button_press_event', onclick)
    plt.show()

    # Save corner points and image
    rect = '_rect' if rectify else ''
    if len(corners) > 1: del corners[-1] # Remove last duplicate
    save_data(corners, 'img_corners%s.npy' % (rect), CALIB_PATH)
    save_data(img, 'image_color%s-%d.jpg' % (rect, now.secs), 
        os.path.join(CALIB_PATH, 'images'), True)

Point 3D文件的保存：

Runs the LiDAR point selection GUI process

Inputs:
    velodyne - [sensor_msgs/PointCloud2] - ROS velodyne PCL2 message
    now - [int] - ROS bag time in seconds

Outputs:
    Picked points saved in PKG_PATH/CALIB_PATH/pcl_corners.npy

def extract_points_3D(velodyne, now):
    # Log PID
    rospy.loginfo('3D Picker PID: [%d]' % os.getpid())

    # Extract points data
    #將雷達點雲轉換爲numpy格式通過ros_numpy
    points = ros_numpy.point_cloud2.pointcloud2_to_array(velodyne)
    #轉爲list
    points = np.asarray(points.tolist())

    # Select points within chessboard range
    inrange = np.where((points[:, 0] > 0) &
                       (points[:, 0] < 2.5) &
                       (np.abs(points[:, 1]) < 2.5) &
                       (points[:, 2] < 2))
    points = points[inrange[0]]
    print(points.shape)
    if points.shape[0] > 5:
        rospy.loginfo('PCL points available: %d', points.shape[0])
    else:
        rospy.logwarn('Very few PCL points available in range')
        return

    # Color map for the points
    cmap = matplotlib.cm.get_cmap('hsv')
    colors = cmap(points[:, -1] / np.max(points[:, -1]))

    # Setup matplotlib GUI
    fig = plt.figure()
    ax = fig.add_subplot(111, projection='3d')
    ax.set_title('Select 3D LiDAR Points - %d' % now.secs, color='white')
    ax.set_axis_off()
    ax.set_facecolor((0, 0, 0))
    #畫散點圖
    ax.scatter(points[:, 0], points[:, 1], points[:, 2], c=colors, s=2, picker=5)

    # Equalize display aspect ratio for all axes
    max_range = (np.array([points[:, 0].max() - points[:, 0].min(), 
        points[:, 1].max() - points[:, 1].min(),
        points[:, 2].max() - points[:, 2].min()]).max() / 2.0)
    mid_x = (points[:, 0].max() + points[:, 0].min()) * 0.5
    mid_y = (points[:, 1].max() + points[:, 1].min()) * 0.5
    mid_z = (points[:, 2].max() + points[:, 2].min()) * 0.5
    ax.set_xlim(mid_x - max_range, mid_x + max_range)
    ax.set_ylim(mid_y - max_range, mid_y + max_range)
    ax.set_zlim(mid_z - max_range, mid_z + max_range)

    # Pick points
    picked, corners = [], []
    def onpick(event):
        ind = event.ind[0]
        x, y, z = event.artist._offsets3d

        # Ignore if same point selected again
        if picked and (x[ind] == picked[-1][0] and y[ind] == picked[-1][1] and z[ind] == picked[-1][2]):
            return
        
        # Display picked point
        picked.append((x[ind], y[ind], z[ind]))
        corners.append((x[ind], y[ind], z[ind]))
        rospy.loginfo('PCL: %s', str(picked[-1]))

        if len(picked) > 1:
            # Draw the line
            temp = np.array(picked)
            ax.plot(temp[:, 0], temp[:, 1], temp[:, 2])
            ax.figure.canvas.draw_idle()

            # Reset list for future pick events
            del picked[0]

    # Display GUI
    fig.canvas.mpl_connect('pick_event', onpick)
    plt.show()

    # Save corner points
    if len(corners) > 1: del corners[-1] # Remove last duplicate
    save_data(corners, 'pcl_corners.npy', CALIB_PATH)

主要監聽節點：

Inputs:
    camera_info - [str] - ROS sensor camera info topic
    image_color - [str] - ROS sensor image topic
    velodyne - [str] - ROS velodyne PCL2 topic
    camera_lidar - [str] - ROS projected points image topic

Outputs: None


```python
def listener(camera_info, image_color, velodyne_points, camera_lidar=None):
    # Start node
    rospy.init_node('calibrate_camera_lidar', anonymous=True)
    rospy.loginfo('Current PID: [%d]' % os.getpid())
    rospy.loginfo('Projection mode: %s' % PROJECT_MODE)
    rospy.loginfo('CameraInfo topic: %s' % camera_info)
    rospy.loginfo('Image topic: %s' % image_color)
    rospy.loginfo('PointCloud2 topic: %s' % velodyne_points)
    rospy.loginfo('Output topic: %s' % camera_lidar)

    # Subscribe to topics
    info_sub = message_filters.Subscriber(camera_info, CameraInfo)
    image_sub = message_filters.Subscriber(image_color, Image)
    velodyne_sub = message_filters.Subscriber(velodyne_points, PointCloud2)

    # Publish output topic
    image_pub = None
    if camera_lidar: image_pub = rospy.Publisher(camera_lidar, Image, queue_size=5)

    # Synchronize the topics by time
    ats = message_filters.ApproximateTimeSynchronizer(
        [image_sub, info_sub, velodyne_sub], queue_size=5, slop=0.1)
    ats.registerCallback(callback, image_pub)

    # Keep python from exiting until this node is stopped
    try:
        rospy.spin()
    except rospy.ROSInterruptException:
        rospy.loginfo('Shutting down')

if __name__ == '__main__':

    # Calibration mode, rosrun
    if sys.argv[1] == '--calibrate':
        camera_info = '/sensors/camera/camera_info'
        image_color = '/sensors/camera/image_color'
        velodyne_points = '/sensors/velodyne_points'
        camera_lidar = None
        PROJECT_MODE = False
    # Projection mode, run from launch file
    else:
        camera_info = rospy.get_param('camera_info_topic')
        image_color = rospy.get_param('image_color_topic')
        velodyne_points = rospy.get_param('velodyne_points_topic')
        camera_lidar = rospy.get_param('camera_lidar_topic')
        PROJECT_MODE = bool(rospy.get_param('project_mode'))

    # Start keyboard handler thread
    if not PROJECT_MODE: start_keyboard_handler()

    # Start subscriber
    listener(camera_info, image_color, velodyne_points, camera_lidar)

最後結果：

點對應關係保存如下：

圖像點： lidar_camera_calibration/calibration_data/lidar_camera_calibration/img_corners.npy
激光雷達lidar_camera_calibration/calibration_data/lidar_camera_calibration/pcl_corners.npy

校準後的外部存儲如下：

lidar_camera_calibration/calibration_data/lidar_camera_calibration/extrinsics.npz

'euler' : Euler Angles (RPY rad)
'R' : Rotation Matrix
'T' : Translation Offsets (XYZ m)

矯正後的外參包括：

Rotation Matrix

-9.16347982e-02  -9.95792677e-01  -8.74577923e-05
 1.88123595e-01  -1.72252569e-02  -9.81994299e-01
 9.77861226e-01  -9.00013023e-02   1.88910532e-01

Euler Angles (RPY rad)

-0.44460865  -1.35998386   2.0240699
Translation Offsets (XYZ m)