深度相機(jī)和激光雷達(dá)是智能汽車上常用的傳感器。但深度相機(jī)具有特征難以提取，容易受到視角影響。激光雷達(dá)存在數(shù)據(jù)不夠直觀且容易被吸收，從而丟失信息。因此在自動駕駛領(lǐng)域，需要對于不同傳感器做數(shù)據(jù)的融合和傳感器的標(biāo)定。

相機(jī)內(nèi)參標(biāo)定

內(nèi)參標(biāo)定的原理和方法比較簡單，由于只有焦距是未知量，因此計算焦距，求得內(nèi)參。

相機(jī)的畸變

畸變屬于成像的幾何失真,它是由于焦平面上不同區(qū)域?qū)τ跋竦姆糯舐什煌纬傻漠嬅媾で冃维F(xiàn)象。在內(nèi)參標(biāo)定時需要獲取相機(jī)的畸變向量矩陣。

相機(jī)的外參標(biāo)定

?

?

?利用Atuoware獲取融合標(biāo)定參數(shù)

• 啟動16線激光雷達(dá)和深度相機(jī)
• 錄制標(biāo)定過程bag包（過程中定時改變標(biāo)定板位置）
• 編譯標(biāo)定工具箱calibration_camera_lidar
• 編譯非線性庫nlopt
• 利用calibration_camera_lidar進(jìn)行聯(lián)合標(biāo)定
• 保存外參矩陣、內(nèi)參矩陣和畸變矩陣
• 編寫點云投影融合圖像程序
• 完成標(biāo)定與驗證

具體的操作可以參考：【Autoware】激光雷達(dá)-攝像頭聯(lián)合標(biāo)定1- Calibration Tool Kit - 簡書 (jianshu.com)https://www.jianshu.com/p/5e5902b46f3d

編寫雷達(dá)相機(jī)融合標(biāo)定程序

以下是基于松靈小車的深度相機(jī)和激光雷達(dá)融合代碼：

創(chuàng)建.h文件，包含頭文件，初始化發(fā)布、訂閱者和點云圖像融合類。

#pragma once

#include <vector>
#include <deque>

#include "ros/ros.h"
#include <sensor_msgs/PointCloud2.h>

#include <image_transport/image_transport.h>
#include "opencv2/opencv.hpp"
#include <cv_bridge/cv_bridge.h>

#include <pcl/point_types.h>//支持點類型頭文件
#include <pcl/point_cloud.h> //支持點類型頭文件
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/filters/passthrough.h>
#include <pcl/common/transforms.h>

using namespace std;

class ILProjection {
    private:
        // 節(jié)點句柄
        ros::NodeHandle nh;
        // 圖像訂閱發(fā)布
        image_transport::ImageTransport it;
        // 圖像訂閱者
        image_transport::Subscriber image_sub;
        // 雷達(dá)訂閱者
        ros::Subscriber laser_sub;
        // 圖像發(fā)布者
        image_transport::Publisher image_pub;
        // 雷達(dá)發(fā)布者
        ros::Publisher laser_pub;
        // 圖像融合顯示訂閱者
        image_transport::Subscriber imageShow_sub;

        //使用隊列實現(xiàn)同步
        std::deque<sensor_msgs::ImageConstPtr> imageDeque;
        std::deque<sensor_msgs::PointCloud2ConstPtr> laserDeque;


        cv::Mat extrinsic_mat;  // 外參矩陣
        cv::Mat camera_mat;     // 內(nèi)參矩陣
        cv::Mat dist_coeff;     // 畸變矩陣
        cv::Mat rotate_mat;     // 旋轉(zhuǎn)矩陣
        cv::Mat transform_vec;  // 平移向量

        Eigen::Matrix4d laserToImageTransform; // 坐標(biāo)變化矩陣

        int color[21][3] = 
        {
            {255, 0, 0}, {255, 69, 0}, {255, 99, 71}, 
            {255, 140, 0}, {255, 165, 0}, {238, 173, 14},
            {255, 193, 37}, {255, 255, 0}, {255, 236, 139},
            {202, 255, 112}, {0, 255, 0}, {84, 255, 159},
            {127, 255, 212}, {0, 229, 238}, {152, 245, 255},
            {178, 223, 238}, {126, 192, 238}, {28, 134, 238},
            {0, 0, 255}, {72, 118, 255}, {122, 103, 238} 
        };
        float color_distance;   //根據(jù)平面距離對激光雷達(dá)點進(jìn)行著色的步長(z)
        
    public:
        ILProjection();
        ~ILProjection();
        void imageCb(const sensor_msgs::ImageConstPtr &msg);
        void laserCb(const sensor_msgs::PointCloud2ConstPtr &msg);
        // 加載相機(jī)參數(shù)
        //加載相機(jī)參數(shù)
        void loadProjectionParam();
        // 判斷是否能進(jìn)行映射
        void doProjection();
        // 實現(xiàn)映射
        void projection(pcl::PointCloud<pcl::PointXYZI>::Ptr&ccloud,cv::Mat&img);
        // 激光雷達(dá)濾波
        void filterCloudPoint(pcl::PointCloud<pcl::PointXYZI>::Ptr& cloud_in,pcl::PointCloud<pcl::PointXYZI>::Ptr& cloud_out);
        //openCV顯示圖像
        void imageShowCb(const sensor_msgs::ImageConstPtr &msg);
        void imageShow();
};

創(chuàng)建主函數(shù)文件，寫融合標(biāo)定類的實現(xiàn)方法

#include "ILProjection.h"
#include <iostream>

// TODO 相機(jī)參數(shù)文件路徑需要修改 (絕對路徑)
// 小車路徑: /home/agilex/songling-car-task2/src/image_laser_projection/param/Calibrate-data
const string path = "/home/ubuntu/songling-car-task2/src/image_laser_projection/param/Calibrate-data";

ILProjection::ILProjection():it(nh) {
    color_distance = 1.2;
    loadProjectionParam();
    image_sub = it.subscribe("/camera/color/image_raw", 1, &ILProjection::imageCb, this);
    laser_sub = nh.subscribe<sensor_msgs::PointCloud2>("/rslidar_points",1,&ILProjection::laserCb, this);
    image_pub = it.advertise("/image_laser_projection/image", 1);
    laser_pub = nh.advertise<sensor_msgs::PointCloud2>("/image_laser_projection/laser", 1);
}

ILProjection::~ILProjection() {}

void ILProjection::imageCb(const sensor_msgs::ImageConstPtr &msg) {
    imageDeque.push_back(msg);
    doProjection();
}

void ILProjection::laserCb(const sensor_msgs::PointCloud2ConstPtr &msg) {
    laserDeque.push_back(msg);
    doProjection();
}

void ILProjection::loadProjectionParam(){
    //打開標(biāo)定結(jié)果文件
    cv::FileStorage fs_read(path, cv::FileStorage::READ);
    fs_read["CameraExtrinsicMat"] >> extrinsic_mat; //從文件里讀取4x4外參矩陣
    fs_read["CameraMat"] >> camera_mat; //從文件里讀取3x3相機(jī)內(nèi)參矩陣
    fs_read["DistCoeff"] >> dist_coeff; //從文件里讀取1x5畸變矩陣
    fs_read.release(); //關(guān)閉文件
    
    // 下列代碼段從外參矩陣中讀取旋轉(zhuǎn)矩陣、平移向量
	rotate_mat=cv::Mat(3, 3, cv::DataType<double>::type); // 將旋轉(zhuǎn)矩陣賦值成3x3矩陣
    // 取前三行前三列
    for(int i=0;i<3;i++)
    {
        for(int j=0;j<3;j++)
        {
            rotate_mat.at<double>(i,j)=extrinsic_mat.at<double>(j,i);
        }
    }
    transform_vec=cv::Mat(3, 1, cv::DataType<double>::type); //將平移向量賦值成3x1矩陣
    transform_vec.at<double>(0)=extrinsic_mat.at<double>(1,3);
    transform_vec.at<double>(1)=extrinsic_mat.at<double>(2,3);
    transform_vec.at<double>(2)=-extrinsic_mat.at<double>(0,3);

    for(int i=0;i<4;i++)
    {
        for(int j=0;j<4;j++)
        {
            laserToImageTransform(i,j) = extrinsic_mat.at<double>(j,i);
        }
    }
}

void ILProjection::doProjection() {
    // 當(dāng)圖像或雷達(dá)數(shù)據(jù)隊列為空時，直接退出。
    if(imageDeque.empty() || laserDeque.empty()) {
        return;
    }

    // 從隊列中獲取圖像消息
    sensor_msgs::ImageConstPtr imageMsgPtr = imageDeque.front();
    // 定義圖像接收指針
    cv_bridge::CvImagePtr cv_ptr;
    try {
        // 接收消息
        cv_ptr = cv_bridge::toCvCopy(imageMsgPtr, sensor_msgs::image_encodings::BGR8);
    }
    catch (cv_bridge::Exception &e) {
        ROS_ERROR("cv_bridge exception: %s", e.what());
        return;
    }

    //從隊列中獲取激光雷達(dá)消息
    sensor_msgs::PointCloud2ConstPtr laserMsgPtr = laserDeque.front();
    // 定義點云
    pcl::PointCloud<pcl::PointXYZI>::Ptr lasercloud (new pcl::PointCloud<pcl::PointXYZI>);
    pcl::PointCloud<pcl::PointXYZI>::Ptr imageCloud (new pcl::PointCloud<pcl::PointXYZI>);
    pcl::PointCloud<pcl::PointXYZI>::Ptr laserFilteredCloud (new pcl::PointCloud<pcl::PointXYZI>);
    // 將接受到的雷達(dá)點云消息轉(zhuǎn)化為接收點云數(shù)據(jù)類型
    pcl::fromROSMsg(*laserMsgPtr,*lasercloud);
    // 點云濾波
    filterCloudPoint(lasercloud,laserFilteredCloud);
    // 將激光雷達(dá)點云由激光雷達(dá)坐標(biāo)轉(zhuǎn)為攝像機(jī)坐標(biāo)
    pcl::transformPointCloud(*laserFilteredCloud, *imageCloud, laserToImageTransform);
    
    // 刪除較老的數(shù)據(jù) (時間戳轉(zhuǎn)為浮點數(shù)后，越大則越新)
    double imageTime = imageMsgPtr->header.stamp.toSec();
    double laserTime = laserMsgPtr->header.stamp.toSec();

    if(imageTime > laserTime) {
        laserDeque.pop_front();
    }
    else {
        imageDeque.pop_front();
    }
    // 調(diào)用融合函數(shù)
    projection(imageCloud,cv_ptr->image);
}

void ILProjection::projection(pcl::PointCloud<pcl::PointXYZI>::Ptr&ccloud,cv::Mat&img) {
    std::vector<cv::Point3d> lidar_points;
    std::vector<cv::Scalar> dis_color;

    lidar_points.reserve(ccloud->size()+1);
    dis_color.reserve(ccloud->size()+1);
    
    //保留相機(jī)正前方的點(z>0)
    for(int i=0;i<=ccloud->points.size();i++)
    {
        if(ccloud->points[i].z > 0) {
            lidar_points.push_back(cv::Point3d(ccloud->points[i].x, ccloud->points[i].y, ccloud->points[i].z));
            int color_order = int(ccloud->points[i].z / color_distance);
            if(color_order > 20) {
                color_order = 20;
            }
            dis_color.push_back(cv::Scalar(color[color_order][2], color[color_order][1], color[color_order][0]));
        }
    }

    vector<cv::Point2d> projectedPoints; //該vector用來存儲投影過后的二維點

    // 投影核心函數(shù)
    // 第一個參數(shù)為原始3d點
    // 第二個參數(shù)為旋轉(zhuǎn)矩陣
    // 第三個參數(shù)為平移向量
    // 第四個參數(shù)為相機(jī)內(nèi)參矩陣
    // 第五個參數(shù)為投影結(jié)果
    cv::Mat rMat = cv::Mat::eye(3, 3, CV_64FC1);
    cv::Mat tVec = cv::Mat::zeros(3, 1, CV_64FC1);
    cv::projectPoints(lidar_points,rMat,tVec,camera_mat,dist_coeff,projectedPoints);

    //遍歷投影結(jié)果
    for (int i = 0; i<projectedPoints.size(); i++)
    {
        cv::Point2d p = projectedPoints[i];
        // 由于圖像尺寸為480x640，所以投影后坐標(biāo)處于圖像范圍內(nèi)的點才在圖像中進(jìn)行標(biāo)示
        if (p.y<480&&p.y>=0&&p.x<640&&p.x>=0)
        {
            //cout << p.x << " " << p.y << " " << dis_color[i] << endl;
        	//標(biāo)示的方式是在對應(yīng)位置畫圓圈
            cv::circle(img, p, 1, dis_color[i], 2, 8, 0);
        }
    }

    //利用cv_bridge將opencv圖像轉(zhuǎn)為ros圖像
    sensor_msgs::ImagePtr msg=cv_bridge::CvImage(std_msgs::Header(),"bgr8",img).toImageMsg(); 
    image_pub.publish(msg); //image_pub是在函數(shù)外定義的一個ros圖像發(fā)布器，將標(biāo)示后的圖像發(fā)布
}

void ILProjection::filterCloudPoint(pcl::PointCloud<pcl::PointXYZI>::Ptr& cloud_in,pcl::PointCloud<pcl::PointXYZI>::Ptr& cloud_out) {
    pcl::PointCloud<pcl::PointXYZI>::Ptr temp (new pcl::PointCloud<pcl::PointXYZI>);

    //創(chuàng)建X軸濾波器對象
    pcl::PassThrough<pcl::PointXYZI>  xPass;
    //輸入點云
    xPass.setInputCloud(cloud_in);			
    //設(shè)置在Z軸方向上進(jìn)行濾波
    xPass.setFilterFieldName("x");
    //設(shè)置濾波范圍為0~1,在范圍之外的點會被剪除
    xPass.setFilterLimits(0.0, 1000.0);
    //是否反向過濾,也就是保留范圍以外的點，默認(rèn)為false
    //xPass.setFilterLimitsNegative (true);
    xPass.setFilterLimitsNegative(false);
    //執(zhí)行濾波
    xPass.filter(*temp);

    //創(chuàng)建Y軸濾波器對象
    pcl::PassThrough<pcl::PointXYZI>  yPass;
    //輸入點云
    yPass.setInputCloud(temp);			
    //設(shè)置在Z軸方向上進(jìn)行濾波
    yPass.setFilterFieldName("y");
    //設(shè)置濾波范圍為0~1,在范圍之外的點會被剪除
    yPass.setFilterLimits(-0.6, 0.6);
    //是否反向過濾,也就是保留范圍以外的點，默認(rèn)為false
    //yPass.setFilterLimitsNegative (true);
    yPass.setFilterLimitsNegative(false);
    //執(zhí)行濾波
    yPass.filter(*cloud_out);
}

void ILProjection::imageShowCb(const sensor_msgs::ImageConstPtr &msg){
    cv_bridge::CvImagePtr image_msg_bridge;
    image_msg_bridge = cv_bridge::toCvCopy(msg,sensor_msgs::image_encodings::BGR8);
    cv::Mat image_cv = image_msg_bridge->image;
    cv::imshow("view",image_cv);
    cv::waitKey(3);
}

void ILProjection::imageShow(){
    imageShow_sub = it.subscribe("/image_laser_projection/image",1, &ILProjection::imageShowCb, this);
}

利用彈窗，直接顯示融合標(biāo)定后的結(jié)果

#include "ILProjection.h"

int main(int argc,char *argv[])
{
    // 初始化節(jié)點
    ros::init(argc,argv,"image_laser_projection");
    // 創(chuàng)建句柄
    ros::NodeHandle nh;

    ILProjection imageLaserProjection;

    imageLaserProjection.imageShow();
    
    ros::spin();
    return 0;
}

配置庫文件，運行。

實現(xiàn)效果

?代碼倉庫參考：

git@e.coding.net:cqu-top-one/SmartCarSoftware/songling-car-task2.git文章來源地址http://www.zghlxwxcb.cn/news/detail-499725.html

到了這里，關(guān)于深度相機(jī)和激光雷達(dá)的融合標(biāo)定（Autoware）的文章就介紹完了。如果您還想了解更多內(nèi)容，請在右上角搜索TOY模板網(wǎng)以前的文章或繼續(xù)瀏覽下面的相關(guān)文章，希望大家以后多多支持TOY模板網(wǎng)！