本文介绍了雷达（如大陆ARS408和Livox Mid-40）在自动驾驶中的作用，探讨了传统方法融合算法（如Apollo）以及深度学习融合算法（如CenterFusion）在多传感器融合中的挑战，如雷达点云与目标匹配的复杂性。同时，详细阐述了多相机融合，特别是长短焦和双目相机的标定方法，包括单应性矩阵的计算及其在相机位姿估计中的应用。文章还提供了代码示例，展示了如何从基础矩阵、本质矩阵到单应矩阵的计算过程，以及如何利用这些矩阵进行图像拼接。最后，提到了多雷达融合在增强感知能力方面的潜力。
1：雷达Radar
① 大陆ARS408 参数：
② livox mid40 参数：
mid-40的连接使用测试
https://blog.csdn.net/Summer_CX/article/details/116657887
2：传统方法融合算法
如apollo
https://zhuanlan.zhihu.com/p/33852112
3：深度学习融合算法
如：
CenterFusion
https://arxiv.org/pdf/2011.04841.pdf
难点：
为了实现这一点，一种简单的方法是将每个雷达探测点映射到图像平面，并将其与一个对象关联（如果该点映射到该对象的二维边界框内）。这不是一个非常可靠的解决方案，因为雷达探测和目标之间没有一对一的映射。
在图像中，场景中的许多对象生成多个雷达检测，也有一些雷达检测与任何对象都不对应。另外，由于雷达检测的z维不精确（或根本不存在），映射雷达检测可能会在其对应对象的2D边界框之外结束。最后，被遮挡目标的雷达检测将映射到图像中的同一个区域，这使得在二维图像平面上区分它们变得困难。
4：多相机的融合，长短焦/双目相机融合
相机到相机标定
基本方法：
根据长焦相机投影到短焦相机的融合图像进行判断，绿色通道为短焦相机图像，红色和蓝色通道是长焦投影后的图像，目视判断检验对齐情况。在融合图像中的融合区域，选择场景中距离较远处（50米以外）的景物进行对齐判断，能够重合则精度高，出现粉色或绿色重影（错位），则存在误差，当误差大于一定范围时（范围依据实际使用情况而定），标定失败，需重新标定（正常情况下，近处物体因受视差影响，在水平方向存在错位，且距离越近错位量越大，此为正常现象。垂直方向不受视差影响）。
结果示例：如下图所示，图2为满足精度要求外参效果，图3为不满足精度要求的现象，请重新进行标定过程。
双目标定RT求解环节，长短焦标定：
最近在做多相机图像拼接，可以将多相机分为多个双目相机来拼接，这里记录一下双目相机标定的一些原理。
基于标定板的两个相机的标定，是基于特征点平面情况。
如图为双目相机与标定板之间的关系。
由上关系：
双目之间的位姿（长焦和短焦之间的位姿）：
视觉slam求位姿：
基础矩阵和本质矩阵都是3×3的矩阵，它们之间不过是差了个相机内参，因此使用时效果完全一样。
void pose_estimation_2d2d ( std::vector<KeyPoint> keypoints_1,
std::vector<KeyPoint> keypoints_2,
std::vector< DMatch > matches,
Mat& R, Mat& t )
{
// 相机内参,TUM Freiburg2
Mat K = ( Mat_<double> ( 3,3 ) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1 );
//-- 把匹配点转换为vector<Point2f>的形式
vector<Point2f> points1;
vector<Point2f> points2;
for ( int i = 0; i < ( int ) matches.size(); i++ )
{
points1.push_back ( keypoints_1[matches[i].queryIdx].pt );
points2.push_back ( keypoints_2[matches[i].trainIdx].pt );
}
//-- 计算基础矩阵
Mat fundamental_matrix;
fundamental_matrix = findFundamentalMat ( points1, points2, CV_FM_8POINT );
cout<<"fundamental_matrix is "<<endl<< fundamental_matrix<<endl;
//-- 计算本质矩阵
Point2d principal_point ( 325.1, 249.7 ); //相机光心, TUM dataset标定值
double focal_length = 521; //相机焦距, TUM dataset标定值
Mat essential_matrix;
essential_matrix = findEssentialMat ( points1, points2, focal_length, principal_point );
cout<<"essential_matrix is "<<endl<< essential_matrix<<endl;
//-- 计算单应矩阵
Mat homography_matrix;
homography_matrix = findHomography ( points1, points2, RANSAC, 3 );
cout<<"homography_matrix is "<<endl<<homography_matrix<<endl;
//-- 从本质矩阵中恢复旋转和平移信息.
recoverPose ( essential_matrix, points1, points2, R, t, focal_length, principal_point );
cout<<"R is "<<endl<<R<<endl;
cout<<"t is "<<endl<<t<<endl;
}
————————————————————————————————————————————————————————————————————————————————————————————————————————————————————————
单应性矩阵应用：
长焦和短焦摄像头的组合感知，
长焦小视场角远距离，短焦大视场角短距离。
例如：双目标定方法
https://blog.csdn.net/plateros/article/details/102665505
可以由单应性矩阵求解出 旋转 平移关系：
表示就是不同视角的相机的关系；
#include <iostream>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>
// #include "extra.h" // use this if in OpenCV2
using namespace std;//标准库 命名空间
using namespace cv; //opencv库命名空间
// ./pose_estimation_2d2d 1.png 2.png
/****************************************************
* 本程序演示了如何使用2D-2D的特征匹配估计相机运动
* **************************************************/
//特征匹配 计算匹配点对
void find_feature_matches (
const Mat& img_1, const Mat& img_2, // & 为引用 直接使用 参数本身 不进行复制 节省时间
std::vector<KeyPoint>& keypoints_1,//
std::vector<KeyPoint>& keypoints_2,
std::vector< DMatch >& matches );//keypoint Descriptors Match 描述子匹配
//位置估计 计算旋转和平移
void pose_estimation_2d2d (
std::vector<KeyPoint> keypoints_1,
std::vector<KeyPoint> keypoints_2,
std::vector< DMatch > matches,
Mat& R, Mat& t );
// 像素坐标转相机归一化坐标
Point2d pixel2cam ( const Point2d& p, const Mat& K );
int main ( int argc, char** argv )
{
if ( argc != 3 )//命令行参数 1.png 2.png
{
cout<<"usage: pose_estimation_2d2d img1 img2"<<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> keypoints_1, keypoints_2;//关键点
vector<DMatch> matches;//特征点匹配对
find_feature_matches ( img_1, img_2, keypoints_1, keypoints_2, matches );//得到匹配点对
cout<<"一共找到了"<<matches.size() <<"组匹配点"<<endl;
//-- 估计两张图像间运动
Mat R,t;//旋转和平移 第一张图 到第二章图的坐标变换矩阵和平移矩阵
pose_estimation_2d2d ( keypoints_1, keypoints_2, matches, R, t );
//-- 验证E=t^R*scale 变叉乘 为 矩阵乘法 三阶行列式
// https://www.cnblogs.com/monoSLAM/p/5349497.html
// a b c
// d e f = bf - ce , cd - af , ae -bd = [0 - c b; c 0 -a; -b a 0 ] * [ d e f]
// 向量 t = [ a1 a2 a3] 其
// 叉乘矩阵 = [0 -a3 a2;
// a3 0 -a1;
// -a2 a1 0 ]
Mat t_x = ( Mat_<double> ( 3,3 ) << //t向量的 叉乘矩阵
0, -t.at<double> ( 2,0 ), t.at<double> ( 1,0 ),
t.at<double> ( 2,0 ), 0, -t.at<double> ( 0,0 ),
-t.at<double> ( 1.0 ), t.at<double> ( 0,0 ), 0 );
cout<<"本质矩阵E = t^R= t叉乘矩阵 * R = "<<endl<<t_x*R<<endl;
//-- 验证对极约束
//相机内参数
// [fx 0 cx
// 0 fy cy
// 0 0 1]
Mat K = ( Mat_<double> ( 3,3 ) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1 );// 相机内参,TUM Freiburg2
for ( DMatch m: matches )//Descriptors Match 描述子匹配
{
Point2d pt1 = pixel2cam ( keypoints_1[ m.queryIdx ].pt, K );//像素坐标转相机归一化坐标 x1 = K逆* p1 x2 = K逆* p2 相机坐标系下 归一化平面上的点
Mat y1 = ( Mat_<double> ( 3,1 ) << pt1.x, pt1.y, 1 );// 归一化平面上的点 齐次表示
Point2d pt2 = pixel2cam ( keypoints_2[ m.trainIdx ].pt, K );</