高精地图应用（四）横向定位

最新推荐文章于 2024-01-16 15:26:11 发布

神气爱哥

最新推荐文章于 2024-01-16 15:26:11 发布

阅读量1.6k

点赞数 1

分类专栏：自动驾驶实战计算机视觉自动驾驶

本文链接：https://blog.csdn.net/qingcaichongchong/article/details/108039508

版权

计算机视觉同时被 3 个专栏收录

15 篇文章 4 订阅

订阅专栏

自动驾驶实战

8 篇文章 12 订阅

订阅专栏

自动驾驶

5 篇文章 1 订阅

订阅专栏

做完单帧匹配后，能得到局部的单帧重建车道线，这时以GPS位置为半径搜索高精度地图，横向定位的目的就是将单帧重建出来的车道线向高精度地图配准，之所以叫横向定位是因为车道线只能确定无人车处于那条车道，而不能确定其距离某个路标有多远。

由于GPS信号的误差大概在10米左右，一般都不会定位到当前车道，可能会错1-2个车道，如上图所示。并且由于偏航角bearing是通过与上一帧的向量确定的，并不太准，这时单帧重建结果与高精度地图还会存在一个角度上的偏差，当然这里的定位误差与偏航角误差可以通过卡尔曼滤波进行平滑与预测，这个后面会说到。总之，虽然它们存在一个偏差，但是这个偏差是符合刚性变换的，可以通过一个刚性变换RT纠正过来的。

要求RT，需要得到同名点，首先需要线线匹配，这里应用了一个相似度算法，步骤如下：

1、将单帧重建车道线编号，如s0、s1、s2、s3，同时将高精度地图车道线也进行编号，如h0、h1、h2、h3。这时可能匹配的组合就是(s0,h0)、(s0, h1)、(s0, h2)、(s0, h3) ... ... (s3, h3)，共有16种组合，将其记为n0, n1, n2 ... n15，每一个组合都代表了一种匹配可能，我们就是要从中找到最符合实际情况的4对组合。

2、将n0, n1, ... n15也两两组合起来，如(n0, n0)、(n0, n1)，..., (n15, n15)，这样的组合共有225对，对每一对计算相似度

，记为score0-0,scpre0-1,....score15-15, 显然这里有些组合是不存在的，如(n0, n1)代表(s0, h0)，(s0, h1)这样的匹配组合，s0不可能与h0,h1同时匹配，那就将(n0，n1)的相似度score0-1设为0，凡是不存在的组合相似度都设为0，对存在的组合计算相似度。

如上图所示，高精度地图上下最短距离分别为s2，s1，单帧重建上下最短距离分别为d2，d1,则定义几何相似度为：

geometry_score = exp(-std::fabs((d2/s2 + d1/ s1) / 2.0 - 1.0));

同时定义属性相似度：

double property_score = 0.0;

if (l1.mark_type == r1.mark_type)

      property_score += 0.5;

if (l2.mark_type == r2.mark_type)

      property_score += 0.5;

总得分：score = geometry_score + property_score

将（nk, np）与其他所有的组合进行相似度计算，并将相似度之和作为其总体的相似度scorek-p，对(n0, n0) , (n0, n1),...(n15, n15)每一对组合都如法炮制，最有可能的匹配组合就是他们之中的最大值。因此，最优匹配组合就选出来了。

3、对于次优组合的选择，要先更新组合相似度值s0-0,...s15-15，更新的原则是，如果与最优匹配组合有冲突，则相似度设置为0，否则不变，然后重新进行第二步计算，选出次优的。如果次优组合的score与最优组合的score相差不多，则代表这可能这种匹配方式是有歧义的，这发生在单帧重建结果与高精度地图不是1对1时，有歧义的话，很可能同名点会找错，遇到这种情况应该忽略掉这次匹配，靠后续的粒子滤波去大概估一个位置。

4、重复2~3过程，直到所有的组合都被选出来。

完成的代码如：

double utils::Score(const DlLane3D &l1,
					const DlLane3D &l2,
					const hdmap_lane &r1,
					const hdmap_lane &r2)
{
	if (!geometry::IsOverlap(l1.geometry, l2.geometry))
		return 0.0;

	if (!geometry::IsOverlap(r1.geometry, r2.geometry))
		return 0.0;

	double head_disL, tail_disL, head_disR, tail_disR;
	geometry::GetHeadTailDistance(l1.geometry, l2.geometry, head_disL, tail_disL);
	geometry::GetHeadTailDistance(r1.geometry, r2.geometry, head_disR, tail_disR);

	double ratioL = std::fabs(head_disR) > EPS ? head_disL / head_disR : head_disL / EPS;
	if (std::fabs(head_disL - head_disR) > 2.0)
		return 0.0;

	double ratioR = std::fabs(tail_disR) > EPS ? tail_disL / tail_disR : tail_disL / EPS;
	if (std::fabs(tail_disL - tail_disR) > 2.0)
		return 0.0;

	double ratio = (ratioL + ratioR) / 2.0;
	double geometry_score = std::exp(-std::fabs(ratio - 1.0));

	double property_score = 0.0;
	if (l1.mark_type == r1.mark_type)
		property_score += 0.5;
	if (l2.mark_type == r2.mark_type)
		property_score += 0.5;

	return geometry_score + property_score;
}

bool utils::Matching(const std::vector<DlLane3D> &dlLanes, const std::vector<hdmap_lane> &hdLanes, std::vector<match_pair> &match_results)
{
	std::vector<match_pair> candidate_match_pairs;
	for (std::size_t i = 0; i < dlLanes.size(); i++)
	{
		for (std::size_t j = 0; j < hdLanes.size(); j++)
		{
			if (geometry::IsOverlap(dlLanes[i].geometry, hdLanes[j].geometry))
			{
				candidate_match_pairs.emplace_back(i, j);
			}
		}
	}

	if (candidate_match_pairs.empty())
	{
		LOG("candidate_match_pairs is empty, match failed!") << std::endl;
		return false;
	}

	unsigned int dim = candidate_match_pairs.size();
	Eigen::MatrixXd affineMtx = Eigen::MatrixXd::Zero(dim, dim);
	for (std::size_t i = 0; i < dim; i++)  
	{
		unsigned int idL1 = candidate_match_pairs[i].source;
		unsigned int idR1 = candidate_match_pairs[i].target;

		for (std::size_t j = i + 1; j < dim; j++)
		{
			unsigned int idL2 = candidate_match_pairs[j].source;
			unsigned int idR2 = candidate_match_pairs[j].target;

			if ((idL1 == idL2) || (idR1 == idR2))
			{
				continue; //not satisfy the one to one match condition
			}

			DlLane3D l1 = dlLanes[idL1];
			DlLane3D l2 = dlLanes[idL2];
			hdmap_lane r1 = hdLanes[idR1];
			hdmap_lane r2 = hdLanes[idR2];
			affineMtx(i, j) = affineMtx(j, i) = Score(l1, l2, r1, r2);
		}
	}

	std::vector<unsigned int> match_array;
	auto is_exist = [&](unsigned int id) -> bool {
		for (size_t i = 0; i < match_array.size(); i++)
		{
			if (match_array[i] == id)
				return true;
		}
		return false;
	};

	auto is_conflict = [&](unsigned int id) -> bool {
		for (size_t i = 0; i < match_array.size(); i++)
		{
			if (candidate_match_pairs[match_array[i]].source == candidate_match_pairs[id].source || candidate_match_pairs[match_array[i]].target == candidate_match_pairs[id].target)
			{
				return true;
			}
		}
		return false;
	};

	auto is_ambiguous = [&](const std::vector<std::pair<unsigned int, double>> &sort_candidates, double simility_delta) -> bool {
		unsigned int sId = sort_candidates[0].first;
		for (std::size_t i = 1; i < sort_candidates.size(); i++)
		{
			match_pair match_point = candidate_match_pairs[sort_candidates[i].first];
			if (match_point.source == candidate_match_pairs[sId].source || match_point.target == candidate_match_pairs[sId].target)
			{
				double score0 = sort_candidates[0].second;
				double score1 = sort_candidates[i].second;
				double score_delta = score0 - score1;
				if (score_delta < simility_delta)
				{
					return true;
				}
				break;
			}
		}
		return false;
	};
	unsigned int num = 0;
	while (num < dim)
	{
		std::vector<std::pair<unsigned int, double>> sort_candidates;
		for (unsigned int i = 0; i < dim; i++)
		{
			sort_candidates.emplace_back(i, 0.0);
		}
		for (unsigned int i = 0; i < dim; i++)
		{
			if (!is_exist(i) && !is_conflict(i))
			{
				for (int j = 0; j < dim; j++)
				{
					if (!is_conflict(j) || is_exist(j))
					{
						sort_candidates[i].second += affineMtx(i, j);
					}
				}
			}
		}

		std::sort(sort_candidates.begin(), sort_candidates.end(),
				  [=](const std::pair<unsigned int, double> &lhs, const std::pair<unsigned int, double> &rhs) {
					  return lhs.second > rhs.second;
				  });

		if (sort_candidates[0].second < EPS)
		{
			break;
		}

		if (num == 0 && sort_candidates.size() > 1 && hdLanes.size() != dlLanes.size())
		{
			if (is_ambiguous(sort_candidates, 0.7))
			{
				LOG("the matching is ambiguous, matching failed!") << std::endl;
				return false;
			}
		}

		unsigned int sId = sort_candidates[0].first;
		match_array.push_back(sId);
		num++;
	}

	for (unsigned sId : match_array)
	{
		match_results.push_back(candidate_match_pairs[sId]);
	}
	return true;
}

5、接下来就很简单了，让相互匹配的线线之间做垂足，就可得到同名点集合matching_points。

得到同名点后，下一步就是计算RT，由于同名点的精度并不是很高（收到语义分割、已经pitch、yaw、roll等误差的影响），我们并不想优化RT全部的自由度，而是有选择性的优化，即不优化roll，只优化pitch、yaw、t1、t2、t3这5个自由度。传统的ICP方法没法固定某一个自由度，这里采用的是非线性优化的方法，效率略有下降，但好处是可以只对部分自由度进行优化。

$X'=R*X+T$

X'表示X经过一次平移旋转后的结果, $T=(t1,t2,t3)^{T}$ , $R=R_{\phi }*R_{\omega }*R_{\kappa}$ , $X=(x,y,z)^{T}$

$R_{\phi}$ 表示绕X轴旋转 $\phi$ 角的旋转矩阵:

$R_{\phi}=\begin{pmatrix} 1& 0& 0\\ 0& cos(\phi)& -sin(\phi)\\ 0& sin(\phi) & cos(\phi) \end{pmatrix}$

$R_{\omega}$ 表示绕Y轴旋转 $\omega$ 角的旋转矩阵:

$R_{\omega}=\begin{pmatrix} cos(\omega) & 0.0 & -sin(\omega)\\ 0.0 & 1.0& 0.0\\ sin(\omega) & 0.0& cos(\omega)\end{pmatrix}$

$R_{\kappa}$ 表示绕Z轴旋转 $\kappa$ 角的旋转矩阵:

$R_{\kappa}=\begin{pmatrix} cos(\kappa) & -sin(\kappa) & 0.0\\ sin(\kappa) & cos(\kappa) & 0.0\\ 0.0 & 0.0 & 1.0 \end{pmatrix}$

假设将R设为矩阵模式，如下：

$R=\begin{pmatrix} a1 & b1& c1\\ a2 & b2& c2\\ a3 & b3& c3 \end{pmatrix}$

则有：

$a1=cos(\omega)*cos(\kappa )$

$b1=-cos(\omega)*sin(\kappa)$

$c1=-sin(\omega)$

$a2=-sin(\phi)*sin(\omega)*cos(\kappa)+cos(\phi)*sin(\kappa)$

$b2=sin(\phi)*sin(\omega)*sin(\kappa)+cos(\phi)*cos(\kappa)$

$c2=-sin(\phi)*cos(\omega)$

$a3=cos(\phi)*sin(\omega)*cos(\kappa)+sin(\phi)*sin(\kappa)$

$b3=-cos(\phi)*sin(\omega)*sin(\kappa)+sin(\phi)*cos(\kappa)$

$c3=cos(\phi)*cos(\omega)$

写成完整形式为:

$\begin{pmatrix} x'\\ y'\\ z' \end{pmatrix}=R_{\phi}*R_{\omega}*R_{\kappa}*\begin{pmatrix} x\\ y\\ z\end{pmatrix}+\begin{pmatrix} t1\\ t2\\ t3 \end{pmatrix}$

根据泰勒公式展开：

$X'-X=\frac{\partial X'}{t1}*d_{t1}+\frac{\partial X'}{t2} * d_{t2}+\frac{\partial X'}{t3}*d_{t3}+\frac{\partial X'}{\phi}* d_{\phi} + \frac{\partial X'}{\omega}*d_{\omega}+\frac{\partial X'}{\kappa}*d_{\kappa}$ （式1）

而：

$\frac{\partial X'}{t1}=\begin{pmatrix} 1.0\\ 0.0\\ 0.0 \end{pmatrix}$

$\frac{\partial X'}{t2}=\begin{pmatrix} 0.0\\ 1.0\\ 0.0 \end{pmatrix}$

$\frac{\partial X'}{t3}=\begin{pmatrix} 0.0\\ 0.0\\ 1.0 \end{pmatrix}$

旋转矩阵稍复杂一些，推导过程如下：

$\frac{\partial X'}{\phi}=\frac{\partial R_{\phi}}{\phi}*R_{\omega}*R_{\kappa}*\begin{pmatrix} x\\ y\\ z \end{pmatrix}=\frac{\partial R_{\phi}}{\phi}*R_{\phi}^{T}*R_{\phi}*R_{\omega}*R_{\kappa}*\begin{pmatrix} x\\ y\\ z \end{pmatrix}=\frac{\partial R_{\phi}}{\phi}*R_{\phi}^{T}*\begin{pmatrix} x'-t1\\ y'-t2\\ z'-t3 \end{pmatrix}$

而：

$\frac{\partial R_{\phi}}{\phi}*R_{\phi}^T=\begin{pmatrix} 0.0 & 0.0 & 0.0\\ 0.0 & -sin(\phi) & -cos(\phi)\\ 0.0 & cos(\phi) & -sin(\phi) \end{pmatrix}*\begin{pmatrix} 1.0& 0.0 & 0.0\\ 0.0& cos(\phi)& sin(\phi)\\ 0.0& -sin(\phi) & cos(\phi) \end{pmatrix}=\begin{pmatrix} 0.0 & 0.0 & 0.0\\ 0.0 & 0.0 & -1.0\\ 0.0 & 1.0 & 0.0 \end{pmatrix}$

代入前式得：

$\frac{\partial X'}{\partial \phi}=\begin{pmatrix} 0.0\\ -z'+t_3\\ y'-t_2 \end{pmatrix}$

同理

$\frac{\partial X'}{\partial \omega}=R_{\phi}*\frac{\partial R_{\omega}}{\partial \omega}*R_{\kappa}*\begin{pmatrix} x\\ y\\ z \end{pmatrix}=R_{\phi}*\frac{\partial R_{\omega}}{\partial {\omega}}*R_{\omega}^{T}*R_{\phi}^T*\begin{pmatrix} x'-t1\\ y'-t2\\ z'-t3 \end{pmatrix}$

而：

$\frac{\partial R_{\omega}}{\partial \omega}*R_{\omega}^T=\begin{pmatrix} -sin(\omega)& 0.0 & -cos(\omega)\\ 0.0 & 0.0 & 0.0\\ cos(\omega) & 0.0 & -sin(\omega) \end{pmatrix}*\begin{pmatrix} cos(\omega)& 0.0 & sin(\omega)\\ 0.0& 1.0& 0.0\\ -sin(\omega)&0.0& cos(\omega) \end{pmatrix}=\begin{pmatrix} 0.0 & 0.0 & -1.0\\ 0.0 & 0.0 & 0.0\\ 1.0 & 0.0 & 0.0 \end{pmatrix}$

代入前式得：

$\frac{\partial R_{\omega}}{\partial \omega}=\begin{pmatrix} 0.0 & sin(\phi) & -cos(\phi)\\ -sin(\phi) & 0.0 & 0.0\\ cos(\phi) & 0.0 & 0.0 \end{pmatrix}*\begin{bmatrix} x'-t1\\ y'-t2\\ z'-t3 \end{bmatrix}=\begin{pmatrix} sin(\phi)*(y'-t2)-cos(\phi)*(z'-t3)\\ -sin(\phi)*(x'-t1)\\ cos(\phi)*(x'-t1) \end{pmatrix}$

k角微分稍有不同：

则

$\frac{\partial X'}{\partial \kappa}=R_{\phi}*R_{\omega}*R_{\kappa}*R_{\kappa}^T*\frac{\partial R_{\kappa}}{\kappa}*R_{\kappa}^T*R_{\omega}^T*R_{\phi}^T*\begin{pmatrix} x'-t1\\ y'-t2\\ z'-t3 \end{pmatrix}=R*R_{\kappa}^T*\frac{\partial R_{\kappa}}{\partial \kappa}* R^T*\begin{pmatrix} x'-t1\\ y'-t2\\ z'-t3 \end{pmatrix}$

而:

$R_{\kappa}^T*\frac{\partial R_{\kappa}}{\partial \kappa}=\begin{pmatrix} cos(\kappa)& sin(\kappa) & 0.0\\ -sin(\kappa)& cos(\kappa)& 0.0\\ 0.0 &0.0& 1.0 \end{pmatrix}*\begin{pmatrix} -sin(\kappa)& -cos(\kappa) & 0.0\\ cos(\kappa) & -sin(\kappa) & 0.0\\ 0.0 & 0.0 & 0.0 \end{pmatrix}=\begin{pmatrix} 0.0 & -1.0 & 0.0\\ 1.0 & 0.0 & 0.0\\ 0.0 & 0.0 & 0.0 \end{pmatrix}$

代入前式得:

$\frac{\partial R_{\kappa}}{\partial \kappa}=R*R_{\kappa}^T*\frac{\partial R_{\kappa}}{\partial \kappa}* R^T*\begin{pmatrix} x'-t1\\ y'-t2\\ z'-t3 \end{pmatrix}=\begin{pmatrix} a1 & b1 & c1\\ a2 & b2 & c2\\ a3 & b3 & c3 \end{pmatrix}*\begin{pmatrix} 0.0 & -1.0 & 0.0\\ 1.0 & 0.0 & 0.0\\ 0.0 & 0.0& 0.0 \end{pmatrix}*\begin{pmatrix} a1 & a2 & a3\\ b1 & b2 & b3\\ c1 & c2 & c3 \end{pmatrix}*\begin{pmatrix} x'-t1\\ y'-t2\\ z'-t3 \end{pmatrix}=\begin{pmatrix} 0.0 & a2b1-a1b2 & a3b1-a1b3\\ a1b2-a2b1 & 0.0 & a3b2-a2b3\\ a1b3-a3b1 & a2b3-a3b2 & 0.0 \end{pmatrix}$

这里有个性质：正交矩阵各元素等于其代数余子式，因此

$\frac{\partial R_{\kappa}}{\partial \kappa}=\begin{pmatrix} 0.0& -c3& c2\\ c3& 0.0& -c1\\ -c2& c1& 0.0 \end{pmatrix}*\begin{pmatrix} x'-t1\\ y'-t2\\ z'-t3 \end{pmatrix}$

将（式1）变形，同时将X‘’写成向量形式可得：

$\begin{pmatrix} \frac{\partial x'}{\partial t1} & \frac{\partial x'}{\partial t2}& \frac{\partial x'}{\partial t3}& \frac{\partial x'}{\partial \phi}& \frac{\partial x'}{\partial \omega}& \frac{\partial x'}{\partial \kappa}\\ \frac{\partial y'}{\partial t1}& \frac{\partial y'}{\partial t2}& \frac{\partial y'}{\partial t3} & \frac{\partial y'}{\partial \phi}& \frac{\partial y'}{\partial \omega}& \frac{\partial y'}{\partial \kappa}\\ \frac{\partial z'}{\partial t1}& \frac{\partial z'}{\partial t2}& \frac{\partial z'}{\partial t3}& \frac{\partial z'}{\partial \phi}& \frac{\partial z'}{\partial \omega} & \frac{\partial z'}{\partial t1} \end{pmatrix}*\begin{pmatrix} dt1\\ dt2\\ dt3\\ d\phi\\ d\omega\\ d\kappa \end{pmatrix}=\begin{pmatrix} x'-x\\ y'-y\\ z'-z \end{pmatrix}$

等式左边就是雅克比矩阵，右边是残差，可采用高斯牛顿算法，为了鲁棒性及效率，可采用阻尼法及M估计。本来这是6个自由度的问题，但是因为精度的原因，并不会优化roll，因为我认为它是不会变化的。这也是为什么采用欧拉角作为状态的原因，至于欧拉角固有的万向锁问题在这里是不存在的，因为万向锁只会在pitch=90度程序，显然这在现实世界中是不存在的：

bool utils::Translate3d_3d_LM(const std::vector<accordance_pair> &match_points, Eigen::Matrix3d &R, Eigen::Vector3d &T, double& error)
{
	if (match_points.empty())
	{
		LOG("match_points is empty, translate failed!") << std::endl;
		return false;
	}

	int N = match_points.size();
	std::vector<Eigen::Vector3d> q1(N), q2(N);
	for (int i = 0; i < N; i++)
	{
		q1[i] = match_points[i].source;
		q2[i] = match_points[i].target;
	}

	double omga = .0, kappa = .0, t1 = .0, t2 = .0, t3 = .0;
    double stopThresholdLM = 0.0;
    double currentEro = 0.0;
    double currentLambda = 0.0;
    double ni = 2.0;

	//Generate init lamda
	{
        Eigen::MatrixXd H = Eigen::MatrixXd::Zero(5, 5);
        Eigen::MatrixXd g = Eigen::MatrixXd::Zero(5, 1);
        for(int i = 0; i < N; i++)
        {
            Eigen::Vector3d trans = geometry::RotationAroundY(omga) * geometry::RotationAroundZ(kappa) * Eigen::Vector3d(q1[i].x(), q1[i].y(), q1[i].z()) + Eigen::Vector3d(t1, t2, t3);
			Eigen::Vector3d residual = trans - q2[i];
            currentEro += residual.transpose() * residual;

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

			Eigen::Matrix<double, 3, 5> J_aux;
			J_aux << (t3 - z), -std::cos(omga) * (y - t2), 1.0, 0.0, 0.0,
				0.0, (std::cos(omga) * (x - t1) + std::sin(omga) * (z - t3)), 0.0, 1.0, 0.0,
				(x - t1), -std::sin(omga) * (y - t2), 0.0, 0.0, 1.0;

			H += J_aux.transpose() * J_aux;
			g -= J_aux.transpose() * residual;
		}

        stopThresholdLM = 1e-6 * currentEro;
        double maxDiagonal = 0;
        for (int i = 0; i < 5; ++i)
        {
            maxDiagonal = std::max(std::fabs(H(i, i)), maxDiagonal);
        }
        double tau = 1e-5;
        currentLambda = tau * maxDiagonal;
    }

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

	int iter = 0;
	bool stop = false;
	while (!stop && iter < 30)
	{
		bool oneStepSuccess = false;
        int false_cnt = 0;

		while (!oneStepSuccess)
		{
			Eigen::MatrixXd H = Eigen::MatrixXd::Zero(5, 5);
			Eigen::MatrixXd g = Eigen::MatrixXd::Zero(5, 1);

			for (int i = 0; i < N; i++)
			{
				Eigen::Vector3d trans = geometry::RotationAroundY(omga) * geometry::RotationAroundZ(kappa) * Eigen::Vector3d(q1[i].x(), q1[i].y(), q1[i].z()) + Eigen::Vector3d(t1, t2, t3);

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

				double err_i_norm = (trans - q2[i]).norm();
				double weight = robustWeightCauchy(err_i_norm);

				Eigen::Matrix<double, 3, 5> J_aux;
				J_aux << (t3 - z), -std::cos(omga) * (y - t2), 1.0, 0.0, 0.0,
					0.0, (std::cos(omga) * (x - t1) + std::sin(omga) * (z - t3)), 0.0, 1.0, 0.0,
					(x - t1), -std::sin(omga) * (y - t2), 0.0, 0.0, 1.0;

				H += J_aux.transpose() * J_aux * weight;
				g -= J_aux.transpose() * (trans - q2[i]) * weight;
			}

			for (int i = 0; i < 5; i++)
            {
                H(i, i) += currentLambda;
            }

			Eigen::Matrix<double, 5, 1> delta = H.colPivHouseholderQr().solve(g);
            if ((delta.head(2).squaredNorm() < EPS && delta.tail(3).squaredNorm() < EPS) || false_cnt > 10)
            {
                stop = true;
                break;
            }

			omga += delta[0];
			kappa += delta[1];
			t1 += delta[2];
			t2 += delta[3];
			t3 += delta[4];

			double scale = 0;
            scale = delta.transpose() * (currentLambda * delta + g);
            scale += 1e-3;

			double temp_err = 0.0;
            for (int i = 0; i < N; i++)
            {
               	Eigen::Vector3d trans = geometry::RotationAroundY(omga) * geometry::RotationAroundZ(kappa) * Eigen::Vector3d(q1[i].x(), q1[i].y(), q1[i].z()) + Eigen::Vector3d(t1, t2, t3);
              	Eigen::Vector3d residual = trans - q2[i];
                temp_err += residual.transpose() * residual;
            }

			double rho = (currentEro - temp_err) / scale;
            if (rho > 0 && std::isfinite(temp_err)) // last step was good
            {
                double alpha = 1.0 - pow((2 * rho - 1), 3);
                alpha = std::min(alpha, 2. / 3.);
                double scaleFactor = (std::max)(1. / 3., alpha);
                currentLambda *= scaleFactor;
                ni = 2;
                currentEro = temp_err;
                oneStepSuccess = true;
            }
            else
            {
                currentLambda *= ni;
                ni *= 2;
                oneStepSuccess = false;
            }

			if(!oneStepSuccess)
            {
				omga -= delta[0];
				kappa -= delta[1];
				t1 -= delta[2];
				t2 -= delta[3];
				t3 -= delta[4];
				false_cnt++;
            }
            else
            {
                false_cnt = 0;
            }

		}
		iter++;
		if (sqrt(currentEro) <= stopThresholdLM)
			stop = true;
	}

	R = geometry::RotationAroundY(omga) * geometry::RotationAroundZ(kappa);
	T = Eigen::Vector3d(t1, t2, t3);
	error = std::sqrt(currentEro / N);
	return true;
}

至此，RT就可以求出，将其应用与gps坐标和航向，完成横向定位：

神气爱哥

关注

1
点赞
踩
12

收藏

觉得还不错? 一键收藏
打赏
8
评论
高精地图应用（四）横向定位

这里的横向定位是基于ICP算法的改进实现的3d-3d的配准方法，步骤：1.将单帧重建车道线的形值点，向高精度地图中的车道线做垂足，考虑到不受高程差异的影响，这里是在平面上完成的。求得的形值点与垂足就组成了一对同名点，我将形值点命名为source，垂足命名为target。会生成很多这样的同名点。2....
复制链接

扫一扫