多传感器融合算法，基于Radar,Camera和Lidar标定

SensorFusion

已于 2024-09-12 09:56:57 修改

阅读量3.3k

点赞数 3

分类专栏：多传感器融合文章标签：算法自动驾驶计算机视觉

于 2021-01-03 14:18:22 首次发布

本文链接：https://blog.csdn.net/nh54zyt/article/details/112134514

版权

多传感器融合专栏收录该内容

6 篇文章

订阅专栏

传感器设备的标定
在这里插入图片描述

1：单目标定

单目标定opencv不稳定,matlab更好
Ubuntu下matlab安装方法
https://blog.csdn.net/m0_38087936/article/details/103342731
在这里插入图片描述

ImageSize：图像大小
Radial Distortion：径向畸变
Tangential Distortion：切向畸变
World Points：世界坐标系下的点
World Units：世界坐标下的单位
Estimate Skew：估计倾斜
Num Radial Distortion Coefficient：径向畸变系数个数
Estimate Tangential Distortion：估计切向畸变
Translation Vectors：平移向量
Reprojection Errors：重投影误差
Detected Keypoints：检测到的关键点
Rotation Vectors：旋转向量
Num Patterns：模态数
Intrinsics：内参
Intrinsic Matrix：内参矩阵
Focal Length：焦距
Principal Point：主点偏移
Skew：偏斜
Mean Reprojection Error：平均重投影误差
Reprojected Points：重投影点
Rotation Matrices：旋转矩阵
————————————————
在这里插入图片描述

注意参数：

这里对matlab得到的结果进行一个简要的说明：

Matlab中



RadialDistortion 是k1 k2 k3， 

TangentialDistortion是p1 p2

内参矩阵:
ans =
  955.8925（fx）           0                        0
  
         0              955.4439（fy）              0
         
  296.9006（cx）  215.9074（cy）    1.0000

Opencv中


内参矩阵:
ans =
  955.8925（fx）           0                       296.9006（cx）
  
         0              955.4439（fy）                215.9074（cy）
         
     0                             0                          1.0000
畸变矩阵：
{k1 k2 p1 p2 k3 }

第一步；

点击先点击matlab中的APP（1），接着找到app右侧下三角，在图像处理中找到camera（2）进入标定工具箱
在这里插入图片描述

第二步

点击Add（3），导入准备好的棋盘图片
在这里插入图片描述

第三步，

全选图片后点击打开（4），导入图片
加粗样式
**
第四步，

选择棋盘格大小（5）（单位mm）后点击确定

————————————————
第五步，

选择相机型号，普通相机鱼眼相机畸变参数优化类型
在这里插入图片描述

其他：

在使用opencv中的undistort进行畸变矫正时，需要使用8个参数即fc1, fc2, cc1, cc2, kc1, kc2, kc3, kc4;
RadialDistorion中的参数分别是：kc1,kc2,kc5(不常用)
TangentialDistortion中的参数分别是：kc3,kc4
IntrinsicMatrix中的参数分别是：
fc1 不用 0
不用 fc2 0
cc1 cc2 1
opencv的畸变矫正程序如下：
double fc1, fc2, cc1, cc2, kc1, kc2, kc3, kc4;
fc1 = ;
fc2 = ;
cc1 = ;
cc2 = ;
kc1 = ;
kc2 = ;
kc3 =;
kc4 =;
Mat intrinsic_matrix = (Mat_(3, 3) << fc1, 0, cc1, 0, fc2, cc2, 0, 0, 1);
Mat distortion_coeffs = (Mat_(1, 4) << kc1, kc2, kc3, kc4);

2：双目标定

在这里插入图片描述

经过双目标定得到摄像头的各项参数后，
采用OpenCV中的stereoRectify(立体校正)得到校正旋转矩阵R、投影矩阵P、重投影矩阵Q，
再采用initUndistortRectifyMap函数得出校准映射参数，然后用remap来校准输入的左右图像。

双目标定参考文档（英文）：https://ww2.mathworks.cn/help/vision/ug/stereo-camera-calibrator-app.html

3：单目与Radar毫米波标定

方法一： 在这里插入图片描述

方法一：以图像坐标系为准，根据投影点图像像素坐标和前方车辆的宽度、距离信息在图像上建立感兴趣区域ROI。

方法二：以毫米波雷达坐标系为准，将摄像头坐标系进行旋转和平移，并与毫米波雷达坐标系对齐。对摄像头的原始图像进行逆透视变换时，选取的地面特征点均以毫米波雷达为原点，并测量其在世界坐标系（毫米波雷达坐标系）下的坐标。

在这里插入图片描述

方法二：
https://blog.csdn.net/leonardohaig/article/details/88724365
方法三：
方法四：

在这里插入图片描述

水平摆放https://blog.csdn.net/weixin_42631693/article/details/90042530

倾斜摆放
Radar坐标系俯仰角和偏航角到世界坐标系转换
Radar

4：Lidar与Radar标定

在这里插入图片描述

radar lidar camera分别检测跟踪，融合跟踪
radar_lidar

5：Lidar与Camera标定

什么是相机与激光雷达外参标定？

就是相机坐标系和激光雷达坐标系的TF变化。位置x，y，z 欧拉角 roll，pitch，yaw，6个变量构成一个4*4的旋转变换矩阵

标定的就是这个4维的旋转矩阵。

标定的方法有：

基于特征

基于运动观测

基于最大化互信息

基于深度学习

基于特征的方法是根据对应特征点求解PnP问题，需要标定板来获取特征

基于运动观测可以看作手眼标定问题，精度决定于相机和雷达的运动估计

基于最大化互信息认为图像灰度于反射强度具有相关性

基于深度学习需要长时间的训练并且泛化能力不高

定方法有两个指标：

精度

自动化程度

内参外参联合优化标定
在这里插入图片描述

https://github.com/OpenCalib/JointCalib

camera标定
https://github.com/enazoe/camera_calibration_cpp.git

开源标定算法
https://github.com/PJLab-ADG/SensorsCalibration.git

三维点云转到二维
原理为下图
在这里插入图片描述

如果考虑，z =0 ，即张正友求解法，标定板为一个平面建立坐标，原理如下：
在这里插入图片描述

那么内参和外参可以合并进行简化为H单应性矩阵，
那么如果已知内参和外参，可以将图像投影到三维空间，前提是三维空间同一个平面，
公式类似于上面

void CameraMapRail::Implement::MapImageToRail(std::vector<cv::Point2f> in,
                                              std::vector<cv::Point2f>& out) {
  int n = 0;
  bool flag = in.size() == out.size() ? true : false;
  for (auto i : in) {
    float x = (u_params[0] * i.x + u_params[1] * i.y + u_params[2]) /
              (u_params[3] * i.x + u_params[4] * i.y + u_params[5]);
    float y = (v_params[0] * i.x + v_params[1] * i.y + v_params[2]) /
              (v_params[3] * i.x + v_params[4] * i.y + v_params[5]);
    if (flag)
      out[n] = cv::Point2f(y, x);
    else
      out.emplace_back(cv::Point2f(y, x));
    n++;
  }
}

// 其中 u_params 可以通过内参外参推导出 H11 H12的数值

6：同步

在这里插入图片描述

7：ADAS radar和camera

在这里插入图片描述

可以简单的将车作为中间变量

即求Radar到Car，Camera到Car

那么Radar到Car：

在这里插入图片描述
那么Car到camera：

方法：
利用世界坐标系中的一系列的已知的点相对车体坐标系的三维点
及YXZ三维坐标（距离Car圆心）

在这里插入图片描述

整体的公式如下：在这里插入图片描述

|||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||

方法

在这里插入图片描述

7： PNP

如下5种PNP求解方法：
https://www.cnblogs.com/yepeichu/p/12670818.html
在这里插入图片描述

例如求激光和图像标定外参

https://blog.csdn.net/nh54zyt/article/details/112134514

https://blog.csdn.net/qq_37394634/article/details/104430656
直接法：
在这里插入图片描述

在这里插入图片描述

例如，如下代码：

先使用OpenCV的solvePnP()求解出运动，
然后再使用高斯牛顿法进行BA优化。

#include<iostream>
#include "common.h"
#include <Eigen/Core>
#include <g2o/core/base_vertex.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/solvers/csparse/linear_solver_csparse.h>
#include <g2o/types/sba/types_six_dof_expmap.h>


// BA求解
void bundleAdjustment(vector<Point3f> points_3d, vector<Point2f> points_2d, const Mat &K, Mat &R, Mat &t);


/**
 * 本程序演示了PnP求解相机位姿,BA优化位姿与3D空间点坐标
 * @param argc
 * @param argv
 * @return
 */
int main(int argc, char **argv) {

    if (argc != 4) {
        cout << "usage: pose_estimation_3d2d img1 img2 depth1" << endl;
        return 1;
    }
    //-- 读取图像
    Mat img_1 = imread(argv[1], CV_LOAD_IMAGE_COLOR);
    Mat img_2 = imread(argv[2], CV_LOAD_IMAGE_COLOR);

    vector<KeyPoint> key_points_1, key_points_2;
    vector<DMatch> matches;
    find_feature_matches(img_1, img_2, key_points_1, key_points_2, matches);
    cout << "一共找到" << matches.size() << "组匹配点" << endl;

//    建立3D点,深度图为16位无符号,单通道
    Mat d1 = imread(argv[3], CV_LOAD_IMAGE_UNCHANGED);
    Mat_<double> K(3, 3);
    K << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1;
    vector<Point3f> pts_3d;
    vector<Point2f> pts_2d;
    for (auto &match : matches) {
        ushort d = d1.ptr<unsigned short>(int(key_points_1[match.queryIdx].pt.y))
        [int(key_points_1[match.queryIdx].pt.x)];
        if (d == 0)
            continue;
        double dd = d / 1000.0;
        Point2d p1 = pixel2cam(key_points_1[match.queryIdx].pt, K);
        pts_3d.push_back(Point3d(p1.x * dd, p1.y * dd, dd));
        pts_2d.push_back(key_points_2[match.trainIdx].pt);
    }
    cout << "3d-2d pairs: " << pts_3d.size() << endl;

    Mat r, t;
//    调用OpenCV的PnP求解
    solvePnP(pts_3d, pts_2d, K, Mat(), r, t, false, cv::SOLVEPNP_EPNP);
    Mat R;
//    通过罗德里格斯公式将旋转向量转为旋转矩阵
    cv::Rodrigues(r, R);
    cout << "R=\n" << R << endl;
    cout << "t=\n" << t << endl;
//    BA
    bundleAdjustment(pts_3d, pts_2d, K, R, t);
    return 0;
}


/**
 * BA求解
 * @param points_3d
 * @param points_2d
 * @param K
 * @param R
 * @param t
 */
void bundleAdjustment(vector<Point3f> points_3d, vector<Point2f> points_2d,
                      const Mat &K, Mat &R, Mat &t) {
//    初始化g2o,pose维度为6,landmark维度为3
    typedef g2o::BlockSolver<g2o::BlockSolverTraits<6, 3>> Block;
    Block::LinearSolverType *linearSolver = new g2o::LinearSolverCSparse<Block::PoseMatrixType>();
    auto *solver_ptr = new Block(linearSolver);
    auto *solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
    g2o::SparseOptimizer optimizer;
    optimizer.setAlgorithm(solver);

//    vertex
    auto *pose = new g2o::VertexSE3Expmap();
    Eigen::Matrix3d R_mat;
    R_mat << R.at<double>(0, 0), R.at<double>(0, 1), R.at<double>(0, 2),
            R.at<double>(1, 0), R.at<double>(1, 1), R.at<double>(1, 2),
            R.at<double>(2, 0), R.at<double>(2, 1), R.at<double>(2, 2);
    pose->setId(0);
    pose->setEstimate(g2o::SE3Quat(R_mat, Eigen::Vector3d(t.at<double>(0, 0), t.at<double>(1, 0), t.at<double>(2, 0))));
    optimizer.addVertex(pose);
//    landmarks
    int index = 1;
    for (const Point3f &p:points_3d) {
        auto *point = new g2o::VertexSBAPointXYZ();
        point->setId(index++);
        point->setEstimate(Eigen::Vector3d(p.x, p.y, p.z));
        point->setMarginalized(true);
        optimizer.addVertex(point);
    }

//    parameter: camera intrinsics
    g2o::CameraParameters *camera = new g2o::CameraParameters(
            K.at<double>(0, 0), Eigen::Vector2d(K.at<double>(0, 2), K.at<double>(1, 2)), 0);
    camera->setId(0);
    optimizer.addParameter(camera);

//    edges
    index = 1;
    for (const Point2f &p:points_2d) {
        auto *edge = new g2o::EdgeProjectXYZ2UV();
        edge->setId(index);
        edge->setVertex(0, dynamic_cast<g2o::VertexSBAPointXYZ *>(optimizer.vertex(index)));
        edge->setVertex(1, pose);
        edge->setMeasurement(Eigen::Vector2d(p.x, p.y));
        edge->setParameterId(0, 0);
        edge->setInformation(Eigen::Matrix2d::Identity());
        optimizer.addEdge(edge);
        index++;
    }

    auto t1 = chrono::steady_clock::now();
    optimizer.setVerbose(true);
    optimizer.initializeOptimization();
    optimizer.optimize(100);
    auto t2 = chrono::steady_clock::now();
    auto time_used = chrono::duration_cast<chrono::duration<double >>(t2 - t1);
    cout << "optimization costs time: " << time_used.count() << " seconds." << endl;
    cout << "\nafter optimization:\n" << "T=\n" << Eigen::Isometry3d(pose->estimate()).matrix() << endl;
}