1.数学模型
1.1 坐标系（标准的坐标系）
以自车后轴中心在地面的垂直投影点为原点建立车体坐标系，其定义如下图1所示，其中x轴指向车辆正前方，y轴指向车辆左侧，z轴指向天空。
图1 车体坐标系
以当前帧车体坐标系原点为原点，车体坐标系x轴在地平面投影为x轴建立地面坐标系。其y轴在地平面上垂直x轴向右，z轴垂直与地平面向上。理想条件下车体坐标系和地面坐标系完全重合。
1.2 地面位置的影响（这个对应了这篇博客）
从2D车道线到3D车道线的转换，最简单的方法是利用相机的外参和地平面假设求取车道线位置，即假设偏航角、俯仰角和横滚角的误差均为0。自动驾驶车体坐标系下3D点的投影公式如下：
图2 车体坐标系下3D点的投影公式
上图的公式中有三个方程，但是有s,x,y,z四个不变量，原本是不可解的。但由于车道线点都在地面上，所有的Z取值均为0，因此公式变得可解了。求解该公式，结果如图3绿色点所示。可见其与真值有较大的偏差，主要原因是当车辆发生颠簸或者在上下坡区域下，真实地平面和理想地平面不一致，地平面假设不成立了。
图3 黄色点为车道线3D位置真值，绿色点为直接使用标定参数求解的3D车道线点，一个方格大小为20m*20m
1.3 地面位置表示方法
1.1节中我们提到理想条件下车体坐标系和地平面坐标系完全重合， 但是当车辆发生颠簸或者在上下坡区域下，车辆坐标系和地面坐标系之间存在三个欧拉角偏差，将偏航角、俯仰角和横滚角的误差分别记为y 、p、r。通过这三个误差项我们可以求取真实地平面和理想地平面之间的位置差异，对这三个角我们统称为动态外参。此外，由于地面不一定是平的，我们也可以用二次曲线来近似拟合地面。
2 地平面位置重构
2.1 3D车道线坐标误差分析
图2我们提到在理想条件下，如何使用2D车道线点计算出对应的3D车道线点坐标。由于地平面位置是动态变化的，真实地平面位置和理想地平面位置存在三个动态外参的误差，我们可以得到如下公式：（也就是说增加了一个RPY的偏移量）
上式在理想地平面的基础上叠加了动态外参的影响，由于车道线在地平面上，所以z坐标为0。相机标定参数是确定的，我们可以得到结论，3D车道线点的坐标是动态外参、2d车道线点坐标的函数：（根据2D深度学习算出来的uv以及偏差量可以计算出x和y值）
假设车道线检测网络的结果基本正确，即（u,v）坐标是正确的。则3D车道线点坐标的误差取来源于三个动态外参估计的误差。（然后说明了x的误差来源是三个动态外参估计的误差，所以这里的偏导数要乘以error）
struct AlignmentErrorPoint {
AlignmentErrorPoint (double* camera_extrinsic_error): observed(camera_extrinsic_error) {
}
template <typename T>
bool operator()(const T* const camera_extrinsic,
const T* const camera_extrinsic_delta,
const T* const point,
const T* const point3d,
T* residuals) const {
// camera_extrinsic[0,1,2] are the angle-axis rotation.
T p[3];
camera_extrinsic[0]+=(camera_extrinsic_delta[0]+observed[0]);
camera_extrinsic[1]+=(camera_extrinsic_delta[1]+observed[1]);
camera_extrinsic[2]+=(camera_extrinsic_delta[2]+observed[2]);
ceres::AngleAxisRotatePoint(camera_extrinsic, point, p);
// camera_extrinsic[3,4,5] are the translation.
p[0] += camera_extrinsic[3];
p[1] += camera_extrinsic[4];
p[2] += camera_extrinsic[5];
// The error is the difference between the predicted and observed position.
residuals[0] = T(point3d[1]) - p[1];
residuals[0] = T(point3d[0]) - p[0];
return true;
}
double* observed;
};
2.2 车道线结构化特征误差
按照施工标准，车道线方程为螺旋曲线或直线，在一定范围内可用三次曲线拟合。设被观测到的车道线总数为n，在当前帧地面坐标系下，记第i条车道线方程如下：（这里是标准的三次曲线方程）
假设一共n根车道线，每根车道线取m个点。根据三次方程，构建车道线拟合误差如下：（拟合结果，对应了残差）
struct AlignmentErrorCurve {
AlignmentErrorCurve (double* camera_extrinsic_error): observed(camera_extrinsic_error) {
}
template <typename T>
bool operator()(const T* const camera_extrinsic,
const T* const camera_extrinsic_delta,
const T* const point,
const T* const curve,
T* residuals) const {
// camera_extrinsic[0,1,2] are the angle-axis rotation.
T p[3];
camera_extrinsic[0]+=(camera_extrinsic_delta[0]+observed[0]);
camera_extrinsic[1]+=(camera_extrinsic_delta[1]+observed[1]);
camera_extrinsic[2]+=(camera_extrinsic_delta[2]+observed[2]);
ceres::AngleAxisRotatePoint(camera_extrinsic, point, p);
// camera_extrinsic[3,4,5] are the translation.
p[0] += camera_extrinsic[3];
p[1] += camera_extrinsic[4];
p[2] += camera_extrinsic[5];
// The error is the difference between the predicted and observed position.
residuals[0] = T(p[1]) - T(curve[0]*ceres::pow(p[0],3))+T(curve[1]*ceres::pow(p[0],2))+T(curve[2]*ceres::pow(p[0],1))+T(curve[3]);
return true;
}
double* observed;
};
对n根车道线进行多帧观测，观测窗口长度为k。将n根车道线在当前帧地面坐标系下的三次方程参数以及每帧车辆坐标系与地面坐标系之间的欧拉角偏差记为待求解参数，如下式所示：
大多数情况下车道线之间是平行的，可以通过一些简单前处理获取车道线之间的平行特性。车道线是曲线，曲线平行的定义如下：（这部分讲述了如何计算曲线的平行度）
根据以上公式可以构建平行误差; parallel
2.1节中的3D车道线点误差、拟合误差和平行误差加权相加，定义为3D车道线的观测误差。通过求解使得观测误差最小的参数 ，最终可获得理想的3D车道线参数方程和动态外参。
通过求解以上方程，可以求取地平面位置。为了验证效果，我们做重投影实验如下图4。我们的真值车道线点只标注了BEV视图下的横纵坐标，没有高度信息。
图4：黄色点：使用相机外参投影车道线真值 红色点：使用相机外参+ 动态外参投影车道线真值
2.3 帧间信息的使用
上一节中我们提到了使用单帧信息构建地平面位置，但是单帧信息往往是不足够的。比如上一节中的平行假设，在分叉线场景下是不成立的。因此单帧信息只能给与一个初步的观测，需要融合帧间信息。
2.3.1 多帧重投影误差
其中由于车道线是静态物体，世界坐标系下车道线参数一直保持不变。利用帧间位姿变换将历史帧车道线点转换到当前帧车体坐标系后，可以构建多帧重投影误差。假设观测窗口长度为k：
/* Template class for BA
/* 测量值：在前一个相机坐标系下的特征点坐标，在下一帧图像上测量到的这些特征点的像素坐标。相机内参数K是固定的。
/* operator()中，待优化的参数包含了平移旋转，ceres_rot是旋转，形式是轴角，
/* ceres_trans是平移
*/
struct ReprojectionError {
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
ReprojectionError(Eigen::Vector3d point,Eigen::Vector2d pixel,Eigen::Matrix3d K)
: point_(point), pixel_(pixel),K_(K) {
}
template <typename T>
bool operator()(const T* const ceres_rot, const T* const ceres_trans, T* residuals) const {
//step1: 读取平移旋转的数据，转换成为Eigen的格式
T p1[3];
T p2[3];
p1[0] = T(point_(0));
p1[1] = T(point_(1));
p1[2] = T(point_(2));
//step2: 将世界坐标系下的点坐标转换为相机坐标系的坐标
//旋转
ceres::AngleAxisRotatePoint(ceres_rot, p1, p2);
//平移
p2[0] += ceres_trans[0];
p2[1] += ceres_trans[1];
p2[2] += ceres_trans[2];
// cout << "After rotation and translation point: (" << p2[0] << ", "
// << p2[1] << ", "
// << p2[2] << ") " << endl;
// cout << "K_ is: " << endl << K_ << endl;
//Step3: 计算投影到图像空间下的坐标
Eigen::Vector<T,3> pixel_point;
pixel_point(0) = K_(0,0) * p2[0] + K_(0,2) * p2[2];
pixel_point(1) = K_(1,1) * p2[1] + K_(1,2) * p2[2];
pixel_point(2) = K_(2,2) * p2[2];
if(pixel_point(2)==0.0) {
LOG(ERROR) << "Depth of map point should not be 0";
return false;
}
pixel_point(0) /= pixel_point(2); //x
pixel_point(1) /= pixel_point(2); //y
//cout << "predicted pixel: (" << pixel_point(0) << "," << pixel_point(1) << ")" << endl;
// The error is the difference between the predicted and observed position.
residuals[0] = pixel_point(0) - pixel_[0];
residuals[1] = pixel_point(1) - pixel_[1];
//cout << "pixel residual: (" << residuals[0] << ", " << residuals[1] << endl;
return true;
}
// Factory to hide the construction of the CostFunction object from
// the client code.
static ceres::CostFunction* Create(const Eigen::Vector3d point,
const Eigen::Vector2d pixel,
const Eigen::Matrix3d K) {
return (new ceres::AutoDiffCostFunction<ReprojectionError, 2, 3, 3>(
new ReprojectionError(point,pixel,K)));
}
Eigen::Vector3d point_;
Eigen::Vector2d pixel_;
Eigen::Matrix3d K_;
};
其中 K 为相机内参; 和 是车体坐标系与相机坐标系之间的旋转矩阵和平移向量； 是第i车体坐标系与地面坐标系之间的旋转矩阵，其由第i车辆坐标系与地面坐标系之间的欧拉角偏差计算得到； 是第i车道线神经网络检测得到的车道线点的像素坐标; X 是当前帧地面坐标系下 3D 车道线点坐标， 是利用定位信息计算到的 3D 车道线点在第i地面坐标系下的坐标。
2.3.2 构造里程计
多帧重投影信息需要用到帧间位姿变换，一般自动驾驶组合导航系统都会给出六自由度位姿帧间变换信息，这些信息来自于IMU、GPS、轮速计等。在乘用车中，IMU很多时候会安装在底盘，相机一般安装在车体。底盘与车体之间由弹簧连接，是非刚性连接。在这种情况下组合导航的帧间角度信息就不可靠了，需要一个视觉里程计来计算帧间角度变化。本文使用特征点匹配求取帧间位姿变化，构建了一个简单的视觉里程计，如图4 所示。（本周会写一个知乎博客将代码开源。）
引入视觉里程计后，2.3.1节中的多帧重投影误差变为如下形式：
求解帧间误差，可以获取平滑的帧间位姿变换。我们拿出帧间pitch角做对比，如下图所示：
2.2.3 多帧求解 将多帧误差和单帧误差组合起来，构造误差函数。以上误差函数可以使用图优化求解，使用ceres库可以获取较好的结果。求解多帧误差后可以成功稳定构建地平面位置，同时求出效果较好的车道线参数方程。
3 结果
结果如下，视频中画面下方为场景图片，上方为BEV视角下的车道线。BEV车道线中粉色点是直接使用相机标定参数lift2D车道线点生成的3D车道线，黄色点是mobileye车道线，绿色为本文方法生成的车道线。由于缺乏真值，我们以mobileye为baseline来评价本文方法性能。
从视频中我们可以看到，在遇到颠簸、上下坡或者刹车时，红色车道线位置精度差，位置波动也较大。绿色车道线位置与黄色接近，由于mobileye的位置偏差一般不会太大，因此可以大致认为本文方法生成的车道线的位置精度较高。从位置噪声上看，mobileye车道线波动较本文方法略大，本文方法有一定优势。此外，mobileye车道线输出范围较小（5-55m），本文方法车道线输出范围较mobileye要大（0-80m）。
4 地面重构效果
如下图所示，下面为图像，上面为BEV视角下地面重构结果。图中红线之间的间隔为10m
5.地面重构的意义
如下图所示，地面重构有以下用途
a.地面重构后可更好与lidar点云融合，可以作为基于bev fusion的道路结构认知方法的预处理结果
b.可以利用接地点算出目标的位置，如下图中的锥桶（bev视图下的绿色点）
