高精度地图应用（三）单帧重建

   单帧重建是指车道线的重建，为何选择车道线是因为车道线输出稳定，每一帧图像都有，而且车道线特征很强，语义分割技术比较成熟。首先，需要对原图进行语义分割以提取车道线，如：

   然后，在语义分割基础上进行折线拟合，我这里就直接采用了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，从而实现横向定位了。

重建的效果大概是这样：

二维语义图：

三维重建图：