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

传感器设备的标定

1：单目标定

单目标定opencv不稳定,matlab更好
Ubuntu下matlab安装方法
https://blog.csdn.net/m0_38087936/article/details/103342731

但是得注意参数：

cameraParams.IntrinsicMatrix:
3293.35909906506 fc1	0	0
-4.35172616366572	3293.54167494101fc2	0
1537.90640626082 vo	1049.28114385394 v1	1


cameraParams.RadialDistortion:  k1 k2 k5
-0.0777901104375618	0.174514579680910	-0.0774194053626076


cameraParams.TangentialDistortion:  k3 k4
-0.000207967884843062	-0.00215186224296790
1
2
3
4
5
6
7
8
9
10
11
12

在使用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);
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19

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的数值
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18

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;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134