当前位置:   article > 正文

多传感器融合算法,基于Radar,Camera和Lidar标定_radar 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
声明:本文内容由网友自发贡献,不代表【wpsshop博客】立场,版权归原作者所有,本站不承担相应法律责任。如您发现有侵权的内容,请联系我们。转载请注明出处:https://www.wpsshop.cn/w/你好赵伟/article/detail/832344
推荐阅读
  

闽ICP备14008679号