单帧重建是指车道线的重建,为何选择车道线是因为车道线输出稳定,每一帧图像都有,而且车道线特征很强,语义分割技术比较成熟。首先,需要对原图进行语义分割以提取车道线,如:
然后,在语义分割基础上进行折线拟合,我这里就直接采用了opencv直线拟合的方式,代码:
std::vector<Eigen::Vector2d> utils::Fitting2d(const std::vector<cv::Point2d>& geometry2d, cv::Mat K, cv::Mat D)
{
std::vector<Eigen::Vector2d> fitting_line;
if (geometry2d.size() < 2)
{
return fitting_line;
}
std::vector<cv::Point2d> undistort_points;
cv::undistortPoints(geometry2d, undistort_points, K, D, cv::noArray(), K);
int maxRow = 0, minRow = 10000, maxCol = 0, minCol = 10000;
for (cv::Point pix2d : undistort_points)
{
if (pix2d.x > maxCol) maxCol = pix2d.x;
if (pix2d.x < minCol) minCol = pix2d.x;
if (pix2d.y > maxRow) maxRow = pix2d.y;
if (pix2d.y < minRow) minRow = pix2d.y;
}
std::vector<cv::Vec2d> to_be_fittings;
for (size_t i = 0; i < undistort_points.size(); i++)
{
to_be_fittings.emplace_back(
undistort_points[i].x,
undistort_points[i].y);
}
cv::Vec4f line_args;
cv::fitLine(to_be_fittings,
line_args,
7,
0, 0.01, 0.01);
double cos_theta = line_args[0];
double sin_theta = line_args[1];
double x0 = line_args[2];
double y0 = line_args[3];
double x1 = x0 + cos_theta;
double y1 = y0 + sin_theta;
Eigen::Vector3d line = Eigen::Vector3d(x0, y0, 1.0).cross(Eigen::Vector3d(x1, y1, 1.0));
int sample_cnt = 20;
double sample_interval = (maxRow - minRow) / sample_cnt;
for(std::size_t i = 0; i <= sample_cnt; i++)
{
double row = maxRow - i * sample_interval;
Eigen::Vector3d row_sample_line = Eigen::Vector3d(minCol, row, 1.0).cross(Eigen::Vector3d(maxCol, row, 1.0));
Eigen::Vector3d cross_point = line.cross(row_sample_line);
if(std::fabs(cross_point.z()) > EPS)
{
cross_point /= cross_point.z();
fitting_line.emplace_back(cross_point.x(), cross_point.y());
}
}
return fitting_line;
}
接下来,利用前面(二)所得的四个参数,俯仰角pitch,翻滚角roll,偏航角yaw,以及车身高度h,就能将拟合的二维折线反投到三维空间中,代码:
bool utils::FittingPolyline(const std::vector<cv::Point2d>& ctour, cv::Mat K, cv::Mat D, const VMC::Camera &cam, std::vector<Eigen::Vector3d>& wSpline, double scope)
{
auto imgToBody = [&](Eigen::Vector2d xi, Eigen::Vector3d& c3d, double sight) -> bool
{
cv::Mat xi3 = (cv::Mat_<double>(3, 1) << xi.x(), xi.y(), 1.0);
cv::Mat xc3 = K.inv() * xi3;
cv::Vec2d p;
p[0] = xc3.at<double>(0, 0);
p[1] = xc3.at<double>(1, 0);
Eigen::Vector3d xc(p[0], p[1], 1.0);
Eigen::Vector3d trans = cam.mount.translation;
Eigen::Vector3d rot_xb = cam.mount.rotation.matrix() * xc;
if(std::fabs(rot_xb.z()) > EPS)
{
double scale = - trans.z() / rot_xb.z();
rot_xb = scale * rot_xb + trans;
double r = rot_xb.x();
if (r <= sight && r > 0)
{
c3d.x() = rot_xb.x();
c3d.y() = rot_xb.y();
c3d.z() = rot_xb.z();
return true;
}
}
return false;
};
std::vector<Eigen::Vector2d> spline = Fitting2d(ctour, K, D);
if (!spline.empty())
{
for (Eigen::Vector2d p : spline)
{
Eigen::Vector3d b3d;
if (imgToBody(p, b3d, scope))
{
wSpline.push_back(b3d);
}
}
}
if (wSpline.size() > 1)
{
geometry::Resample(wSpline, 1.0);
return true;
}
return false;
}
接下来,就该将三维折线与高精度地图进行配准以求得RT,从而实现横向定位了。
重建的效果大概是这样:
二维语义图:
三维重建图: