毫米波雷达与视觉融合———联合标定及外参标定问题

毫米波雷达与视觉融合———联合标定及外参标定问题

微信公众号：幼儿园的学霸
个人的学习笔记，关于OpenCV,关于机器学习, …。问题或建议，请公众号留言;

最近在做ADAS项目，早期时选择的方案是利用纯视觉实现，后面发现纯视觉在测距上有一定的困难，同时纯视觉方案在arm开发板上的检测速率不高，因此考虑采用用毫米波和视觉融合来实现ADAS功能。利用毫米波进行障碍物位置确定，然后利用视觉进行目标判别。此处对相关思路及方案进行整理。当然，个人认为智能驾驶中传感器融合将会是一个必然的趋势。

目录

传感器介绍

首先，简单介绍下自动驾驶常用的传感器：
1.视觉类摄像机（包括单目、双目立体视觉、全景视觉及红外相机）
2.雷达类测距传感器（激光雷达、毫米波雷达、超声波雷达等）

其中，单目相机主要用于特征类符号的检测与识别，如车道线检测、交通标志识别、交通灯识别、行人和车辆检测等，尽管目前来说视觉检测可靠性并不是很高，但是基于机器学习/深度学习的视觉计算在自动驾驶普及之日一定会是必不可少的部分；双目视觉应用于室外场景目前尚不多见，毕竟双目的同步和标定是个大问题，双目匹配也是一个耗时的问题，在我做的双目视觉车道线检测中，双目立体匹配耗时和车道线检测耗时基本相当，没有GPU等硬件加速其耗时将影响程序的实时性；
毫米波雷达可能是当前最受欢迎的传感器了，毫米波，是工作在毫米波波段（millimeter wave），工作频率在30～100GHz，波长在1～10mm之间的电磁波，通过向障碍物发射电磁波并接收回波来精确探测目标的方向和距离，其全天候全天时以及准确的测速测距深受开发者的喜爱；当然，激光雷达也是一个在自动驾驶领域非常重要的传感器了，Lidar利用激光来进行对目标进行探测，通过每分钟600转或1200转的进行扫射，它能非常详细的获得一个实时的三维点云数据，包括目标的三维坐标、距离、方位角、反射激光的强度、激光编码、时间等等，常用的有单线、4线、16线、32线、64线、128线束的，是一个高精度的传感器，而且其稳定性好、鲁棒性高，然而，它的成本却让众多厂商在落地的路上望而却步，另外，激光受大气及气象影响大，大气衰减和恶劣天气使作用距离降低，大气湍流会降低激光雷达的测量精度，激光束窄的情况难以搜索目标和捕获目标。一般先有其他设备实施大空域、快速粗捕目标，然后交由激光雷达对目标进行精密跟踪测量。

ADAS思路

实现ADAS的技术主要有三类，分别是基于视觉传感器、毫米波雷达和激光雷达。由于成本限制因素，国内主要使用前两种方式。视觉传感器和毫米波雷达实现对ADAS功能的原理不同：毫米波雷达主要是通过对目标物发送电磁波并接收回波来获得目标物体的距离、速度和角度。视觉方案稍复杂，以单目视觉方案为例，它需要先进行目标识别，然后根据目标在图像中的像素大小来估算目标的距离。
这两类技术各有优劣。总体来讲，摄像头方案成本低，可以识别不同的物体，在物体高度与宽度测量精度、车道线识别、行人识别准确度等方面有优势，是实现车道偏离预警、交通标志识别等功能不可缺少的传感器，但作用距离和测距精度不如毫米波雷达，并且容易受光照、天气等因素的影响。毫米波雷达受光照和天气因素影响较小，测距精度高，但难以识别车道线、交通标志等元素。另外，毫米波雷达通过多普勒偏移的原理能够实现更高精度的目标速度探测。
于是就有了第三种方案，将摄像头和雷达进行融合，相互配合共同构成汽车的感知系统，取长补短，实现更稳定可靠的ADAS功能。

毫米波和视觉融合思路

我在项目中主要方案是利用毫米波雷达来辅助视觉。
基本的思路是将毫米波雷达返回的目标点投影到图像上，围绕该点并结合先验知识，生成一个矩形的感兴趣区域，然后我们只对该区域内进行目标检测。它的优点是可以迅速地排除大量不会有目标的区域，极大地提高识别速度。而且对于前碰撞系统（FCWS），它可以迅速排除掉雷达探测到的非车辆目标，增强结果的可靠性，最后，可以利用毫米波返回的目标的距离、角度、速度信息来进行碰撞时间（collision time）计算，以达到预警功能，实现行人/车辆在预警时间阈值内预警，避免单目视觉距离测量及障碍物速度估计不准的问题。
当然，这个方案也有很明显的缺点：
1）首先，这个方法实现起来有难度。理想情况下雷达点出现在车辆中间。首先因为雷达提供的目标横向距离不准确，再加上摄像头标定的误差，导致雷达的投影点对车的偏离可能比较严重。我们只能把感兴趣区域设置的比较大。感兴趣区域过大后导致里面含有不止一辆车，这个时候目标就会被重复探测，这会造成目标匹配上的混乱。交通拥挤的时候尤其容易出来这种情况。在实际测试中这个问题非常明显；
2）噪声问题。对于性能比较差的毫米波，返回的目标点中包含了大量的噪声点，将这些点投影到图像上将会存在大量的矩形框，反而造成了程序的耗时；
3）另一方面是这种方法本质上只是对雷达目标的一种验证，无法充分发挥视觉的作用。雷达和摄像头的视野其实并不完全重合，导致毫米波检测到的目标在图像上并没有出现，或者图像上存在的目标毫米波反而检测不到。

联合标定思路分析

建立精确的毫米波雷达坐标系、三维世界坐标系、摄像机坐标系、图像坐标系和像素坐标系之间的坐标转换关系，是实现毫米波和视觉融合的关键。毫米波雷达与视觉传感器在空间的融合就是将不同传感器坐标系的测量值转换到同一个坐标系中。由于ADAS前向视觉系统以视觉为主，因此只需将毫米雷达坐标系下的测量点通过坐标系转换到摄像机对应的像素坐标系下即可实现两者空间同步。此处首先讲解两传感器之间的标定问题：将毫米波检测的目标转换到图像上。

对于毫米波雷达和摄像头考虑如下安装位置，分别建立坐标系：

我们的目的是将毫米波的坐标转换到图像的对应位置上去。在相机位置处建立三维世界坐标系，在点的转换过程中可以分为以下几个步骤：

• a)毫米波坐标系下的坐标转换到以相机为中心的世界坐标系中
• b）将世界坐标系的坐标转换到相机坐标系
• c)将相机坐标系的坐标转换到图像坐标系
  下面对上面步骤予以讲解。

毫米波坐标到世界坐标的转换

毫米波可以得到目标的x,y坐标信息，没有目标的z坐标信息，因此，可以将毫米波坐标系Om到世界坐标系Ow的转换看做二维X-Y坐标系的转换，Om和Ow之间的关系不外乎平移和旋转。

如上图所示，根据数学知识可以推导毫米波坐标到世界坐标系的转换关系为：
$$\left[\begin{array}{c}X\\ Y\\ 1\end{array}\right]=\left[\begin{array}{ccc}cos\theta & -sin\theta & X_w\\ sin\theta & cons\theta & Y_w\\ 0& 0& 1\end{array}\right]\ast \left[\begin{array}{c}{X}_{wave}\\ Y_{wave}\\ 1\end{array}\right]$$
转换矩阵是由两部分组成：由角度所带来的旋转矩阵以及平移产生的平移矩阵。其中平移矩阵的平移量可以理解为毫米波设备在世界坐标系的坐标，即：毫米波到相机的距离。因此平移矩阵可以很好求出，直接利用卷尺就可以测量得到，比较麻烦的旋转矩阵的求解，后面会有求解方法。当然，如果毫米波设备安装位置合适，也可以理解旋转矩阵为0。

世界坐标到像素的转换

毫米波坐标转换到世界坐标系后，接下来便是向像素坐标的转换。我们得到的世界坐标值是二维的，只有x,y值，没有z值，这个可以利用先验知识予以给定。假设毫米波得到的点是物体的中心点，利用物体的宽度和高度信息，便可以得到其z坐标信息。由于我的项目是用于车辆和行人检测，因此我假设目标的宽度为W=1.6m，高度H=1.8m，当然，也可以采用其他值，在我看的北理的一篇论文中，目标的宽度和高度设置值和我的一样。由此，可以得到目标点的世界坐标为(x,y,-H/2),注意z值是负值。
由于我们的目的是在图像上将目标框出，因此需要的是目标的左上顶点和右下顶点的坐标值，将这两个点转换到图像上，即可得到目标的矩形区域。

根据3个已知变量的值加上相机外参，可以很容易求出像素坐标(u,v两个未知量)。这个过程涉及的矩阵变换在网上资料比较多。

由于上面两个转换过程涉及到了外参问题，转换前提是需要先已知外参。但是，我们也可以反过来考虑，如果标定正确，那么是不是意味着外参也正确呢。

具体实现

思路：我们可以通过假设一系列的外参值，来观察图像上的矩形框是否位于期望位置，如果是，那么我们就认为假设的外参是正确的，可以接受的。
上面假设外参的过程会涉及到大量值的尝试，我们可以通过代码来帮我们实现这个过程，代码如下：

//====================================================================//
// Created by liheng on 19-3-7.
//Program:动态调节相关参数，查找最优的外参，得到毫米波坐标到图像坐标的转换
//Data:2019.3.7
//Author:liheng
//Version:V1.0
//====================================================================//

#include "typedef.h"
#include<opencv2/opencv.hpp>
#include<iostream>
#include "WaveRadar2Image.h"
#include "CGetVideo.h"

//网口相机
//CGetVideo m_getVideo(18072414); // 网口相机编号:17369069  12.5：18072414
CGetVideo m_getVideo("/home/liheng/car_distance_22m.avi");

void onChange(int value,void* param)
{
    //获取waveX,waveY
    //X:0-10,对应-5--5
    //Y:0-10 对应0-50
    float k_waveX = (-5 - 5.0) / (0-10.0);
    float b_waveX = -5.0 - k_waveX * 0;

    //获取pitch,yaw角度
    //0-3000,0对应-30.0;3000对应30.0;
    //x     0       3000.0
    //y    -30.0     30.0
    float k_pitch = (-30.0-30.0)/(0-3000.0);
    float b_pitch = -30.0 - k_pitch*0;

    float k_yaw = (-30.0-30.0)/(0-3000.0);
    float b_yaw = -30.0 - k_yaw*0;


    float waveX = (float)cv::getTrackbarPos("waveX/m","TrackBar");
    waveX = k_waveX*waveX + b_waveX;

    float waveY = (float)cv::getTrackbarPos("waveY/m","TrackBar");

    float pitch = (float)cv::getTrackbarPos("pitch/°(-30°-30°)","TrackBar");
    pitch = k_pitch*pitch + b_pitch;

    float yaw = (float)cv::getTrackbarPos("yaw/°(-30°-30°)","TrackBar");
    yaw = k_yaw*yaw + b_yaw;

    //获取theta
    float k_theta = (-30.0-30.0)/(0-3000.0);
    float b_theta = -30.0 - k_theta*0;

    float theta = (float)cv::getTrackbarPos("theta/°(-30°-30°)","TrackBar");
    theta = k_theta*theta + b_theta;

    //====================图像处理========================//
    cv::Mat posInImage,posInCamera;
    cv::Mat src=*(cv::Mat*)param;
    cv::Mat dst;dst.release();
    {
        ADAS::CameraPara cameraPara;
        cameraPara.fu = 2270.512;
        cameraPara.fv = 2271.165;
        cameraPara.cu = 669.0744;//
        cameraPara.cv = 382.6168;
        cameraPara.height = 1750;//mm
        cameraPara.pitch = pitch*(CV_PI*1.0/180.0);
        cameraPara.yaw  = yaw*(CV_PI*1.0/180.0);;
        cameraPara.roll = 0;
        cameraPara.image_width = 1280;
        cameraPara.image_height = 720;

        waveY = 22.0;

        cameraPara.camera2wave_radian = theta*(CV_PI*1.0/180.0);
        cameraPara.waveInCamera.x = 0;//mm;
        cameraPara.waveInCamera.y = 0;
        cameraPara.objectHeight = 1000.0;//mm
        cameraPara.objectWidth = 1500.0;//mm



        WaveRadar2Image waveRadar2Image(cameraPara);
        cv::Mat posInWave = (cv::Mat_<float>(2,1)<<
                waveX,
                waveY);



        waveRadar2Image.TransformWRadar2Image2(posInWave,posInImage,posInCamera);

        dst = src.clone();

        if( dst.channels() == 1 )
            cv::cvtColor(dst,dst,CV_GRAY2BGR);

        cv::Mat temp(100,dst.cols,CV_8UC3,cv::Scalar(255,255,255));
        cv::vconcat(dst,temp,dst);

        int nObjectNum = posInImage.cols;
        for(int i=0; i!=nObjectNum;++i)
        {
            cv::rectangle(dst,cv::Rect(posInImage.at<float>(0,i),
                                       posInImage.at<float>(1,i),
                                       posInImage.at<float>(2,i),
                                       posInImage.at<float>(3,i)),cv::Scalar(0,255,0),2);

            cv::circle(dst,cv::Point(posInImage.at<float>(0,i)+posInImage.at<float>(2,i)/2,
                                     posInImage.at<float>(1,i)+posInImage.at<float>(3,i)/2),3,cv::Scalar(0,255,0),4);

            //std::cout<<"width in pixel="<<posInImage.at<float>(2,i)<<std::endl;
        }



        cv::circle(dst,cv::Point(cameraPara.cu,cameraPara.cv),6,cv::Scalar(0,0,255),10);
    }




    //cv::rectangle(dst,cv::Point(0,0),cv::Point(220,110),cv::Scalar(255,255,0),1);


    int nHeight = dst.rows;
    int nWidth = dst.cols;
    cv::line(dst,cv::Point(nWidth/2,0),cv::Point(nWidth/2,nHeight),cv::Scalar(0,0,255),2);
    cv::line(dst,cv::Point(0,nHeight/2),cv::Point(nWidth,nHeight/2),cv::Scalar(0,0,255),2);

    cv::line(dst,cv::Point(nWidth/4,0),cv::Point(nWidth/4,nHeight),cv::Scalar(0,0,255),1);
    cv::line(dst,cv::Point(nWidth*3/4,0),cv::Point(nWidth*3/4,nHeight),cv::Scalar(0,0,255),1);


    cv::line(dst,cv::Point(0,nHeight/4),cv::Point(nWidth,nHeight/4),cv::Scalar(0,0,255),1);
    cv::line(dst,cv::Point(0,nHeight*3/4),cv::Point(nWidth,nHeight*3/4),cv::Scalar(0,0,255),1);

    char info[256];
    sprintf(info,"waveX:%.2f",waveX);
    cv::putText(dst,info,cv::Point(0,20),cv::FONT_HERSHEY_SIMPLEX,0.8,cv::Scalar(0,255,0),2);

    sprintf(info,"waveY:%.2f",waveY);
    cv::putText(dst,info,cv::Point(0,45),cv::FONT_HERSHEY_SIMPLEX,0.8,cv::Scalar(0,255,0),2);

    sprintf(info,"theta:%.1f",theta);
    cv::putText(dst,info,cv::Point(0,70),cv::FONT_HERSHEY_SIMPLEX,0.8,cv::Scalar(0,255,0),2);

    sprintf(info,"cameraX:%.2f",posInCamera.at<float>(0,0));
    cv::putText(dst,info,cv::Point(0,95),cv::FONT_HERSHEY_SIMPLEX,0.8,cv::Scalar(0,255,0),2);

    sprintf(info,"cameraY:%.2f",posInCamera.at<float>(1,0));
    cv::putText(dst,info,cv::Point(0,120),cv::FONT_HERSHEY_SIMPLEX,0.8,cv::Scalar(0,255,0),2);



    sprintf(info,"pitch:%.1f",pitch);
    cv::putText(dst,info,cv::Point(0,145),cv::FONT_HERSHEY_SIMPLEX,0.8,cv::Scalar(0,255,0),2);


    sprintf(info,"yaw:%.1f",yaw);
    cv::putText(dst,info,cv::Point(0,170),cv::FONT_HERSHEY_SIMPLEX,0.8,cv::Scalar(0,255,0),2);


    cv::imshow("TrackBar",dst);
}

int main()
{
    cv::Mat src;
    //src = cv::imread("../000658.png",0);//读入灰度图
    cv::namedWindow("TrackBar",CV_WINDOW_AUTOSIZE);

    //创建滑动条
    int waveX = 5;
    cv::createTrackbar("waveX/m","TrackBar",&waveX,10,onChange,&src);
    waveX = 5;
    cv::createTrackbar("waveY/m","TrackBar",&waveX,10,onChange,&src);

    waveX = 1500;
    cv::createTrackbar("pitch/°(-30°-30°)","TrackBar",&waveX,3000,onChange,&src);
    cv::createTrackbar("yaw/°(-30°-30°)","TrackBar",&waveX,3000,onChange,&src);

    //毫米波和相机
    waveX = 1500;
    cv::createTrackbar("theta/°(-30°-30°)","TrackBar",&waveX,3000,onChange,&src);

    //onChange(0,&src);


    //加载视频进行测试
    //cv::VideoCapture videoCapture;
    //videoCapture.open("/home/zxkj/视频/18_11_11_11_1_59.avi");
    //videoCapture.open("/home/liheng/CLionProjects/kitti/01/left/%06d.png");
    //if( !videoCapture.isOpened() )
    //    return -1;


    int nWaitTime = 0;
    while (true)
    {

        src.release();
        m_getVideo.GetVideoFrame(src);
        //videoCapture >> src;
        if( src.empty() )
            break;

        onChange(0,&src);

        char chKey = cv::waitKey(nWaitTime);
        if (chKey == 27) break;
        if (chKey == ' ') nWaitTime = !nWaitTime;


    }

}

由于编写代码是为了项目所用，因此其中的坐标转换部分不能公开啦。
但是思路在上面中已经列出，可以按照这个思路，很方便用代码实现这个过程。提供一个捷径，世界坐标系到像素坐标系的转换也可以参考这篇论文:http://www.vision.caltech.edu/malaa/publications/aly08realtime.pdf 。里面的实现是我所参考的。
最终，程序运行后界面显示如下：

通过拖动滑动条，当界面上矩形框到期待位置后，可以采用此时的相机外参以及毫米波与相机的夹角作为计算参数。

至此，毫米波雷达与相机的联合标定已结束，完成了空间上雷达检测目标匹配至图像的工作。接下来可进行下一步的工作。

---

下面的是我的公众号二维码图片，欢迎关注。