高精度地图（五）纵向定位

    当横向定位成功后，无人车视野已经被限制住了，这时标牌等视觉要素跟高精地图的标牌只差了一个纵向上的偏差，对于我们人类来说是很容易找到对应关系的。

机器找到对应关系也很简单，这个就要用到著名的匈牙利算法，这里就不过多介绍了，请参考别的文章，我们只说过程：

1.将高精度地图通过

投影到图像上，如上图，这是5个矩形。目标检测同样有5个矩形（红色阴影），于是可以构建二部图

接下来就是确定每个标牌投影的矩形与目标检测每个矩形之间的权值，以确定最佳匹配关系，这里就简单使用中心点的位置以及长宽比这两者的和作为权值。

匹配结果如图，相同颜色代表同一个标牌：

 

得到匹配关系后，就建立一个pnp问题，将横向定位的pose继续代入计算纵向的pose，计算好后，纵向误差得到了较大改善：

 

这里需要说明的是，纵向定位需要限制自由度在车身的前后方向移动，先将高精度地图变换到车身坐标系中，然后放开两个自由度，我们只优化yaw和t1。

公式推导如下：

则有：

还有

再利用上节的结论：

根据链式求导法则：

再采用高斯牛顿法迭代即可

bool utils::Translate3d_2d(const std::vector<std::pair<Eigen::Vector2d, Eigen::Vector3d>>& pole_sign_matches,  
            const Camera& camera,
            Eigen::Matrix3d& R,
            Eigen::Vector3d& T, 
            double& rmse)
{
    if (pole_sign_matches.empty())
	{
		LOG("match_points is empty, translate failed!") << std::endl;
		return false;
	}

	Eigen::Matrix3d cRb = camera.mount.rotation.matrix();
	Eigen::Vector3d cTb = camera.mount.translation;

	int N = pole_sign_matches.size();
	std::vector<Eigen::Vector3d> q1(N);
    std::vector<Eigen::Vector2d> q2(N);
	for (int i = 0; i < N; i++)
	{
		q1[i] = pole_sign_matches[i].second;
		q2[i] = pole_sign_matches[i].first;
	}

	auto robustWeightCauchy = [=](double norm_res) -> double {
		return std::sqrt(1.0 / (1.0 + norm_res * norm_res));
	};

	double kappa = .0, t1 = .0;
	Eigen::MatrixXd A = Eigen::MatrixXd::Zero(2 * N, 2);
	Eigen::MatrixXd B = Eigen::MatrixXd::Zero(2 * N, 1);
	Eigen::MatrixXd W = Eigen::MatrixXd::Identity(2 * N, 2 * N);

	int iter = 0;
	while (iter < 20)
	{
		for (int i = 0; i < N; i++)
		{
			Eigen::Vector3d trans = geometry::RotationAroundZ(kappa) * q1[i] + Eigen::Vector3d(t1, 0, 0);
			Eigen::Vector3d cam_X = cRb.transpose() * (trans - cTb);
			  if(cam_X[2] == 0.0)
                continue;

            Eigen::Vector3d cam2D = camera.K * cam_X;
            Eigen::Vector2d uv(cam2D[0]/ cam2D[2], cam2D[1]/ cam2D[2]);
			double err_i_norm = (uv - q2[i]).norm();

			double x = trans.x();
			double y = trans.y();
			double z = trans.z();

			Eigen::MatrixXd c2uv = Eigen::MatrixXd::Zero(2, 3);
			c2uv << camera.fx() * (1 / cam_X[2]), 0.0, -camera.fx() * (cam_X[0] / (cam_X[2] * cam_X[2])),
				0.0, camera.fy() * (1 / cam_X[2]), -camera.fy() * (cam_X[1] / (cam_X[2] * cam_X[2]));

			Eigen::MatrixXd jacobian = Eigen::MatrixXd::Zero(3, 2);
			jacobian << -y, 1.0,
				x - t1, 0.0,
				0.0, 0.0;
			
			A.block(i * 2, 0, 2, 2) = c2uv * cRb.transpose() * jacobian;
			B.block(i * 2, 0, 2, 1) = q2[i] - uv;

			q1[i] = trans;
		}
		Eigen::Vector2d delta = (A.transpose() * W * A).ldlt().solve(A.transpose() * W *  B);
		double err = delta.norm();
		if (delta.norm() < EPS)
		{
			break;
		}
		kappa += delta(0);
		t1 += delta(1);
		iter++;
	}
	R = geometry::RotationAroundZ(kappa);
	T = Eigen::Vector3d(t1, 0, 0);
	return true;
}