做完单帧匹配后,能得到局部的单帧重建车道线,这时以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'表示X经过一次平移旋转后的结果,,,
表示绕X轴旋转角的旋转矩阵:
表示绕Y轴旋转角的旋转矩阵:
表示绕Z轴旋转角的旋转矩阵:
假设将R设为矩阵模式,如下:
则有:
写成完整形式为:
根据泰勒公式展开:
(式1)
而:
旋转矩阵稍复杂一些,推导过程如下:
而:
代入前式得:
同理
而:
代入前式得:
k角微分稍有不同:
则
而:
代入前式得:
这里有个性质:正交矩阵各元素等于其代数余子式,因此
将(式1)变形,同时将X‘’写成向量形式可得:
等式左边就是雅克比矩阵,右边是残差,可采用高斯牛顿算法,为了鲁棒性及效率,可采用阻尼法及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坐标和航向,完成横向定位:
文章浏览阅读2w次,点赞7次,收藏51次。四个步骤1.创建C++ Win32项目动态库dll 2.在Win32项目动态库中添加 外部依赖项 lib头文件和lib库3.导出C接口4.c#调用c++动态库开始你的表演...①创建一个空白的解决方案,在解决方案中添加 Visual C++ , Win32 项目空白解决方案的创建:添加Visual C++ , Win32 项目这......_c#调用lib
文章浏览阅读4.6k次。苹方字体是苹果系统上的黑体,挺好看的。注重颜值的网站都会使用,例如知乎:font-family: -apple-system, BlinkMacSystemFont, Helvetica Neue, PingFang SC, Microsoft YaHei, Source Han Sans SC, Noto Sans CJK SC, W..._ubuntu pingfang
文章浏览阅读159次。表单表单概述表单标签表单域按钮控件demo表单标签表单标签基本语法结构<form action="处理数据程序的url地址“ method=”get|post“ name="表单名称”></form><!--action,当提交表单时,向何处发送表单中的数据,地址可以是相对地址也可以是绝对地址--><!--method将表单中的数据传送给服务器处理,get方式直接显示在url地址中,数据可以被缓存,且长度有限制;而post方式数据隐藏传输,_html表单的处理程序有那些
文章浏览阅读1.2k次。使用说明:开启Google的登陆二步验证(即Google Authenticator服务)后用户登陆时需要输入额外由手机客户端生成的一次性密码。实现Google Authenticator功能需要服务器端和客户端的支持。服务器端负责密钥的生成、验证一次性密码是否正确。客户端记录密钥后生成一次性密码。下载谷歌验证类库文件放到项目合适位置(我这边放在项目Vender下面)https://github.com/PHPGangsta/GoogleAuthenticatorPHP代码示例://引入谷_php otp 验证器
文章浏览阅读4.3k次,点赞5次,收藏11次。matplotlib.plot画图横坐标混乱及间隔处理_matplotlib更改横轴间距
文章浏览阅读2.2k次。①Storage driver 处理各镜像层及容器层的处理细节,实现了多层数据的堆叠,为用户 提供了多层数据合并后的统一视图②所有 Storage driver 都使用可堆叠图像层和写时复制(CoW)策略③docker info 命令可查看当系统上的 storage driver主要用于测试目的,不建议用于生成环境。_docker 保存容器
文章浏览阅读834次,点赞27次,收藏13次。网络拓扑结构是指计算机网络中各组件(如计算机、服务器、打印机、路由器、交换机等设备)及其连接线路在物理布局或逻辑构型上的排列形式。这种布局不仅描述了设备间的实际物理连接方式,也决定了数据在网络中流动的路径和方式。不同的网络拓扑结构影响着网络的性能、可靠性、可扩展性及管理维护的难易程度。_网络拓扑csdn
文章浏览阅读1.8k次,点赞5次,收藏8次。IOS系统Date的坑要创建一个指定时间的new Date对象时,通常的做法是:new Date("2020-09-21 11:11:00")这行代码在 PC 端和安卓端都是正常的,而在 iOS 端则会提示 Invalid Date 无效日期。在IOS年月日中间的横岗许换成斜杠,也就是new Date("2020/09/21 11:11:00")通常为了兼容IOS的这个坑,需要做一些额外的特殊处理,笔者在开发的时候经常会忘了兼容IOS系统。所以就想试着重写Date函数,一劳永逸,避免每次ne_date.prototype 将所有 ios
文章浏览阅读5.3k次。方法一:用PLSQL Developer工具。 1 在PLSQL Developer的sql window里输入select * from test for update; 2 按F8执行 3 打开锁, 再按一下加号. 鼠标点到第一列的列头,使全列成选中状态,然后粘贴,最后commit提交即可。(前提..._excel导入pl/sql
文章浏览阅读83次。Git常用命令速查手册1、初始化仓库git init2、将文件添加到仓库git add 文件名 # 将工作区的某个文件添加到暂存区 git add -u # 添加所有被tracked文件中被修改或删除的文件信息到暂存区,不处理untracked的文件git add -A # 添加所有被tracked文件中被修改或删除的文件信息到暂存区,包括untracked的文件...
文章浏览阅读202次。分享119个ASP.NET源码总有一个是你想要的_千博二手车源码v2023 build 1120
文章浏览阅读1.8k次。版权声明:转载请注明出处 http://blog.csdn.net/irean_lau。目录(?)[+]1、缺省构造函数。2、缺省拷贝构造函数。3、 缺省析构函数。4、缺省赋值运算符。5、缺省取址运算符。6、 缺省取址运算符 const。[cpp] view plain copy_空类默认产生哪些类成员函数