车道线论文:Maptr v1/LaneGAP/Maptr v2/Map QR

自动驾驶实时地图生成经典论文介绍

作者 | 罗清雨 编辑 | 自动驾驶之心

 原文链接:https://zhuanlan.zhihu.com/p/947364175

点击下方卡片,关注“自动驾驶之心”公众号

戳我-> 领取自动驾驶近15个方向学习路线

>>点击进入→自动驾驶之心『高精地图』技术交流群

本文只做学术分享,如有侵权,联系删文

高清地图(HD Map)对于自动驾驶非常重要,它提供了关于驾驶环境的详细信息,如车道标记、人行横道和道路边界等。传统的高清地图通常通过基于SLAM(即时定位与地图构建)的方法离线构建,这些方法主要的缺点如下

  • 成本高,有相关资质的公司少

  • 更新不及时,路况可能随时变化

  • 容易与自车位置信息不一致,产生误差。

所以实时的地图生成成为了研究的重点,下面主要介绍几篇这个方向上的经典论文。

MapTR v1

过去传统车道线检测算法一般是基于分割,得到一系列散点,再利用模型进行后处理,将散点组成不同的车道线,通常需要聚类,匹配等算法,这个过程后处理一般比较重,往往需要2-3个全职的工程师维护这个过程,还伴随着很多的超参数,通常需要很大的维护量。

MapTR最大的贡献就是由模型直接出分割散点,变成了模型直接出车道线,减少了中间的后处理过程。

创新点

  • 等价排列建模->解决GT歧义问题;

  • 分层查询嵌入机制->更好的学习GT,先匹配车道线,再匹配车道线里面的点,降低学习难度;

等价排列建模

MapTR的核心在于将车道线离散化成点集(论文中取的20个点),如点(p1,p2,...p20),用来表征一条完整的车道线,这样产生了两个问题

  • 车道线(Polyline):它是开放形状的元素,可以从任意一端开始定义,这样会产生两种等价的排列方式。对于没有方向的车道线,p1->p20是正确的GT,p20->p1也应该是正确的GT,解不唯一;

  • 行人过道(Polygon):它是闭合形状的元素,每个点都可以作为起点,并且多边形可以沿顺时针或逆时针方向排列,这会导致多个等价的排列组合。

等价排序建模的核心思想是:对于每个地图元素,不强制使用唯一的排列顺序,而是允许所有等价排列。通过这种方式,模型可以在学习过程中处理不同排列方式的点集,而不会引入不必要的歧义。

等价排序建模
等价排序建模

作用与效果

  • 稳定的学习过程:由于不再强制模型使用唯一的排列,模型在训练过程中对点集排列方式的选择更加灵活,能够适应不同形状的地图元素,避免了不必要的学习难度。

  • 显著的性能提升:通过消除排列歧义,MapTR 在实验中表现出比使用固定排列的模型更高的性能。例如,文章提到使用等价排序建模后,模型的 mAP 提高了 5.9%,对行人过道的 AP 提升了 11.9%。

分层查询嵌入机制

在自动驾驶场景中,构建矢量化高清地图需要同时关注地图元素的整体结构(如行人过道的轮廓、车道线的走向)以及组成这些结构的精确点集(例如每个地图元素的关键点)。传统方法往往处理不够高效,或是只能逐步预测点集(如使用自回归解码器的逐点生成方法),导致推理速度较慢。

为了解决这一问题,MapTR 提出了一个分层查询嵌入机制,能够通过分层的方式同时处理实例级别和点级别的信息,提升了效率和准确性。

分层查询嵌入机制将地图元素的表示分为两层:

  • 实例级别查询(Instance-level Queries):用于表示每个地图元素的整体结构。

  • 点级别查询(Point-level Queries):用于表示组成地图元素的各个点的信息。

这两类查询结合起来,能够有效编码一个地图元素的全局信息和局部细节。具体的公式化如下:

分层查询嵌入机制
分层查询嵌入机制

LaneGAP

Lane Graph as Path:https://arxiv.org/pdf/2303.08815

主要贡献:

  • 提出了一种基于路径的车道图建模方式:与传统的像素或片段级别的车道建模不同,作者提出了一种基于路径(Path-wise)的建模方式。该方法通过保持车道的连续性,更好地捕捉了道路拓扑结构,为自动驾驶中的轨迹规划提供了更有效的信息。

  • LaneGAP框架的设计:作者提出了一个名为LaneGAP的在线车道图构建方法。LaneGAP框架是端到端的,通过车载传感器(如摄像头和激光雷达)输入数据来学习路径,然后通过Path2Graph算法将路径恢复成完整的车道图。该方法保持了车道的连续性,并编码了交通信息,以便为后续的规划任务提供指导。

  • 设计了新的图形评估指标:为了公平地比较不同建模方法,作者提出了一个新的图形中心评估指标,称为TOPO metric,专门评估车道图的连接性、方向性及其在交叉口等复杂区域的处理情况。该指标与不同的建模方法(像素级、片段级和路径级)兼容。

  • 实验验证了路径级建模的优越性:作者通过在多个数据集(包括nuScenes和Argoverse2)上的定量和定性实验,证明了LaneGAP方法在精度和推理速度上优于现有的基于像素和片段的车道图建模方法。此外,LaneGAP还在OpenLane-V2数据集上击败了最新的片段建模方法TopoNet,在mIoU指标上提升了1.6,进一步证明了路径级建模的有效性。

基于路径的车道图建模方式

传统的车道图构建方法大致可以分为像素级(Pixel-wise)和片段级(Piece-wise)两类:

  • 像素级方法(Pixel-wise Modeling):

    • 流程:首先通过鸟瞰图视角(BEV)的像素级特征图来预测车道的分割图和方向图。然后,通过一些启发式的后处理步骤(如细化算法)从分割图中提取车道的骨架,最后将这些细化后的像素骨架连接成车道图。

    • 优点:这种方法可以直接处理图像的每一个像素,适用于简单的车道环境,在某些场景下可以得到较高分辨率的结果。

    • 缺点:由于像素级方法依赖于复杂的后处理步骤,这些步骤非常耗时,且在处理复杂路口或拓扑结构时容易失效(如连接点丢失、骨架不完整)。此外,生成的车道容易出现破碎和不连续的现象。

  • 片段级方法(Piece-wise Modeling):

    • 流程:车道首先被划分为多个小段(例如在车道分叉点或汇合点处进行分割),然后预测这些车道片段之间的连接关系。最后,基于这些片段和它们之间的连接关系,通过后处理步骤(Piece2Graph算法)将它们拼接成完整的车道图。

    • 优点:这种方法试图捕捉车道的拓扑结构,特别是在处理交叉路口时,它可以通过片段连接的方式构建较为复杂的车道图。

    • 缺点:该方法的核心问题是车道片段之间的连接预测困难,特别是在复杂的道路环境中,片段容易丢失,导致片段间的连接性不好。由此生成的车道图通常是不完整或碎片化的,车道连续性难以保证。

  • 基于路径的建模方式(Path-wise Modeling)

    • 高效的图构建:由于不需要复杂的后处理步骤(如像素细化或片段连接),路径建模方法可以更快地生成车道图,同时减少了不连续性或连接失败的问题。

    • 适用于复杂场景:路径级方法特别适合处理复杂的路口和车道拓扑,因为它通过完整路径的方式捕捉了交通流动的连续性。

    • 流程:首先,车道图通过一个新的算法Graph2Path被分解为一系列连续的路径,而不是分割成小的片段。然后,通过路径检测模型来检测完整的路径,并通过Path2Graph算法将这些路径还原为完整的车道图。

    • 优点:保持车道的连续性:这种方法能够保持车道的全局连续性,不像像素级和片段级方法那样容易破碎或丢失片段。

    • 缺点:路径级建模的挑战在于精确检测完整路径的难度,特别是在复杂的交通场景或环境不确定时,可能会对路径的检测提出更高要求。

三种车道线建模方式对比
三种车道线建模方式对比
总结
总结

MapTR v2

算法结构图
算法结构图

主要贡献:

  • 解耦自注意力机制:大大降低了内存消耗并带来了收益,为了降低计算和内存成本,MapTRv2引入了分离的自注意力机制,在实例维度和点维度上分别进行注意力计算,显著减少了内存消耗,同时提高了性能。

  • 一对多匹配:在训练过程中,除了基本的单一匹配外,MapTRv2还引入了一对多匹配分支,增加正样本比例,进一步提高了模型的学习效率。

  • 辅助loss:对透视图和鸟瞰图都采用了辅助稠密监督,显著提高了性能。

  • 拓展到中心线学习:MapTRv2将MapTR扩展到中心线建模和学习,这对于下游运动规划非常重要。

  • 支持3D车道线检测:将框架扩展到3D地图构建(会议版本学习2D地图),并在Argoverse2数据集上提供额外的实验。

1.解耦自注意力机制

在标准自注意力机制中,计算复杂度是  ,如下图1所示:

其中

  •  是实例查询(instance queries)的数量;

  •  是每个实例中点查询(point queries)的数量。

通过解耦自注意力机制,计算复杂度降低为  ,如下图3所示。

解耦自注意力机制
解耦自注意力机制
2.一对多匹配(One-to-Many Matching)

是为了加速训练收敛而引入的一种技术。在这种机制下,通过为每个真实的地图元素分配多个预测元素,可以提高正样本的比例,从而提高模型训练的效率。

在标准的一对一匹配One-to-One Matching)中,每个真实地图元素(Ground Truth, GT)被分配给一个预测元素。这种方法虽然有效,但在实际训练过程中,由于数据稀疏,正样本的数量较少,导致模型收敛较慢。为了增加正样本数量,加速模型的收敛过程,作者引入了一对多匹配机制。

一对多匹配的核心思想是为每个真实的地图元素生成多个预测元素,并通过与多个预测结果进行匹配,增加正样本的比例。这种方式在Transformer架构中非常适合,因为模型可以并行地处理大量查询(queries)。

具体实现
  • 第一步: 使用一对一匹配组进行标准的匈牙利匹配,生成一对一的匹配结果。

  • 第二步: 将真实地图元素重复 K 次,形成一对多匹配组,并对这些副本与预测实例进行匹配。

  • 第三步: 使用一对一和一对多匹配的结果计算总损失,并优化模型。

一对多匹配通过为每个真实地图元素生成多个预测副本,增加了正样本的比例,从而加快了模型的训练收敛。这种技术特别适合于在线地图构建任务中,因为它能够显著提高模型的训练效率,同时保持甚至提升模型的性能。

辅助loss

本文提出了三种辅助损失,分别是:

  1. 深度预测损失(Depth Prediction Loss)

  2. 鸟瞰图分割损失(BEV Segmentation Loss)

  3. 透视视图分割损失(PV Segmentation Loss)

辅助loss
辅助loss
剥离分析结果

Map QR

《Leveraging Enhanced Queries of Point Sets for Vectorized Map Construction》2024 ECCV

https://arxiv.org/pdf/2402.17430

主要贡献:

  • 创新的查询设计:提出了散射-聚合查询(Scatter-and-Gather Query),将查询显式地分为内容部分和位置部分。不同于传统方法对每个点分别进行位置预测,MapQR通过实例查询一次性预测多个点位置,利用位置信息加强对地图实例的构建。降低了运算复杂度。

  • 性能提升:在多个在线地图构建基准数据集(如nuScenes和Argoverse 2)上,MapQR在保持高效的同时,达到了最佳的平均精度(mAP),超越了现有的最新方法。

散射-聚集查询机制(Scatter-and-Gather Query)

查询类型的区别

  • MapTR:每个点查询只负责预测一个位置,它们独立于其他点进行信息探测和预测。虽然所有点最后被组合成一个完整的地图元素,但点查询之间没有共享信息,导致每个点的内容信息较为分散,缺乏全局性。

  • MapQR(SGQ):散射-聚合查询通过实例查询将整个地图元素的点信息统一管理,所有点共享相同的内容信息但具有不同的位置信息。这种设计确保了地图元素的全局一致性,使得预测的每个点可以更好地捕捉到整个地图元素的特性。

计算复杂度的区别

  • MapTR:由于MapTR使用独立的点查询,计算自注意力(Self-Attention)的复杂度是 ,其中 N 是地图元素的数量,n 是每个元素中的点数量。随着查询数量的增加,计算复杂度会大幅上升。

  • MapQR(SGQ):散射-聚合查询的设计减少了点查询的数量,仅需对实例查询进行自注意力计算,计算复杂度为 。因此,相比MapTR,SGQ在处理大规模查询时更加高效,计算成本和内存消耗更低。

位置信息建模的区别

  • MapTR:位置信息是通过每个点的查询独立学习的。这种方式容易导致同一个地图元素内的不同点预测出现不一致的情况,尤其在复杂几何结构中,点预测可能会缺乏整体性。

  • MapQR(SGQ):SGQ通过参考点(Reference Points)显式建模位置信息,所有子查询的位置信息由这些参考点生成并嵌入。这种位置信息的显式建模不仅提升了每个点的准确性,还确保了整个地图元素的几何形状更加完整和准确。

1.实例查询的定义

每个地图元素(如车道线、道路边界)被一个实例查询(Instance Query, 表示。这个查询包含:

  • 内容信息(Content Information):用于描述地图元素的几何和语义属性。

  • 位置信息(Positional Information):用于定位地图元素的具体点位。

2. 散射操作(Scatter Operation)

在散射阶段,实例查询 被复制为多个子查询 qscai, , 这些子查询共享相同的内容信息,但具有不同的位置信息:

其中:

  • i表示第 i个地图元素。

  • j表示第 j 个点位。

  • n是地图元素中的点的数量。

3. 位置嵌入(Positional Embedding)

每个子查询  通过参考点(Reference Points, 生成位置信息。位置信息是通过参考点的坐标( , )生成并嵌入查询中:

其中:

  • PE(x)是位置信息x的正弦嵌入。

  • LP是线性变换,用于对位置嵌入进一步处理。

4. 信息交互(Cross-Attention)

散射的子查询  和输入特征图(BEV特征图, )进行交互,提取位置信息相关的特征:

5. 聚合操作(Gather Operation)

在聚合阶段,散射的子查询 被重新聚合成一个完整的实例查询 。通过将所有子查询的结果拼接,并通过MLP进行处理:

其中:

  • concat 是将所有子查询的结果拼接。

  • MLP 是多层感知机,用于聚合子查询。

6. 完整计算流程

完整的散射-聚合查询过程可以表示为:

与其他方法比较

与其他方案比较
与其他方案比较

自动驾驶之心

论文辅导来啦

知识星球交流社区

近4000人的交流社区,近300+自动驾驶公司与科研结构加入!涉及30+自动驾驶技术栈学习路线,从0到一带你入门自动驾驶感知(大模型、端到端自动驾驶、世界模型、仿真闭环、3D检测、车道线、BEV感知、Occupancy、多传感器融合、多传感器标定、目标跟踪)、自动驾驶定位建图(SLAM、高精地图、局部在线地图)、自动驾驶规划控制/轨迹预测等领域技术方案、大模型,更有行业动态和岗位发布!欢迎加入。

独家专业课程

端到端自动驾驶大模型、VLA、仿真测试、自动驾驶C++、BEV感知、BEV模型部署、BEV目标跟踪、毫米波雷达视觉融合、多传感器标定、多传感器融合、多模态3D目标检测、车道线检测、轨迹预测、在线高精地图、世界模型、点云3D目标检测、目标跟踪、Occupancy、CUDA与TensorRT模型部署、大模型与自动驾驶、NeRF、语义分割、自动驾驶仿真、传感器部署、决策规划、轨迹预测等多个方向学习视频

学习官网:www.zdjszx.com

报错:guo@guo-Dell-G15-5520:~/g2o仿真/src/build$ make Consolidate compiler generated dependencies of target g2o_demo [ 50%] Building CXX object CMakeFiles/g2o_demo.dir/optimize.cc.o /home/guo/g2o仿真/src/optimize.cc: In function ‘std::vector<g2o::SE3Quat> addNoise(const std::vector<g2o::SE3Quat>&, double, double, bool)’: /home/guo/g2o仿真/src/optimize.cc:90:61: error: no match for ‘operator*’ (operand types are ‘g2o::SE3Quat’ and ‘const Quaternion’ {aka ‘const Eigen::Quaternion<double>’}) 90 | noisy[i].setRotation(g2o::SE3Quat::exp(rotNoiseVec) * noisy[i].rotation()); | ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ ^ ~~~~~~~~~~~~~~~~~~~ | | | | g2o::SE3Quat const Quaternion {aka const Eigen::Quaternion<double>} In file included from /home/guo/g2o仿真/src/optimize.cc:11: /opt/ros/noetic/include/g2o/types/slam3d/se3quat.h:99:22: note: candidate: ‘g2o::SE3Quat g2o::SE3Quat::operatonst Eigen::MatrixBase<U>&)’ 325 | operator*(const TranspositionsBase<TranspositionsDerived> &transpositions, | ^~~~~~~~ /usr/local/include/eigen3/Eigen/src/Core/Transpositions.h:325:1: note: template argument deduction/substitution failed: /home/guo/g2o仿真/src/optimize.cc:90:81: note: ‘g2o::SE3Quat’ is not derived from ‘const Eigen::TranspositionsBase<TranspositionsDerived>’ 90 | noisy[i].setRotation(g2o::SE3Quat::exp(rotNoiseVec) * noisy[i].rotation()); | ^ In file included from /usr/local/include/eigen3/Eigen/Householder:24, from /usr/local/include/eigen3/Eigen/QR:15, from /usr/local/include/eigen3/Eigen/SVD:11, from /usr/local/include/eigen3/Eigen/Geometry:13, from /home/guo/g2o仿真/src/optimize.cc:5: /usr/local/include/eigen3/Eigen/src/Householder/HouseholderSequence.h:513:99: note: candidate: ‘template<class OtherDerived, class VectorsType, class CoeffsType, int Side> typename Eigen::internal::matrix_type_times_scalar_type<typename VectorsType::Scalar, OtherDerived>::Type Eigen::operator*(const Eigen::MatrixBase<Derived>&, const Eigen::HouseholderSequence<VectorsType, CoeffsType, Side>&)’ 513 | typename internal::matrix_type_times_scalar_type<typename VectorsType::Scalar,OtherDerived>::Type operator*(const MatrixBase<OtherDerived>& other, const HouseholderSequence<VectorsType,CoeffsType,Side>& h) | ^~~~~~~~ /usr/local/include/eigen3/Eigen/src/Householder/HouseholderSequence.h:513:99: note: template argument deduction/substitution failed: /home/guo/g2o仿真/src/optimize.cc:90:81: note: ‘g2o::SE3Quat’ is not derived from ‘const Eigen::MatrixBase<Derived>’ 90 | noisy[i].setRotation(g2o::SE3Quat::exp(rotNoiseVec) * noisy[i].rotation()); | ^ In file included from /usr/local/include/eigen3/Eigen/Geometry:46, from /home/guo/g2o仿真/src/optimize.cc:5: /usr/local/include/eigen3/Eigen/src/Geometry/Scaling.h:135:1: note: candidate: ‘template<class Derived, class Scalar> Eigen::CwiseBinaryOp<Eigen::internal::scalar_product_op<typename Eigen::internal::traits<T>::Scalar, Scalar>, const Derived, const typename Eigen::internal::plain_constant_type<Expr, Scalar>::type> Eigen::operator*(const Eigen::MatrixBase<Derived>&, const Eigen::UniformScaling<Scalar>&)’ 135 | operator*(const MatrixBase<Derived>& matrix, const UniformScaling<Scalar>& s) | ^~~~~~~~ /usr/local/include/eigen3/Eigen/src/Geometry/Scaling.h:135:1: note: template argument deduction/substitution failed: /home/guo/g2o仿真/src/optimize.cc:90:81: note: ‘g2o::SE3Quat’ is not derived from ‘const Eigen::MatrixBase<Derived>’ 90 | noisy[i].setRotation(g2o::SE3Quat::exp(rotNoiseVec) * noisy[i].rotation()); | ^ In file included from /usr/local/include/eigen3/Eigen/SparseCore:62, from /usr/local/include/eigen3/Eigen/Sparse:26, from /opt/ros/noetic/include/g2o/solvers/eigen/linear_solver_eigen.h:30, from /home/guo/g2o仿真/src/optimize.cc:10: /usr/local/include/eigen3/Eigen/src/SparseCore/SparsePermutation.h:147:1: note: candidate: ‘template<class SparseDerived, class PermDerived> const Eigen::Product<MatrixDerived, PermutationDerived, 2> Eigen::operator*(const Eigen::SparseMatrixBase<OtherDerived>&, const Eigen::PermutationBase<PermutationDerived>&)’ 147 | operator*(const SparseMatrixBase<SparseDerived>& matrix, const PermutationBase<PermDerived>& perm) | ^~~~~~~~ /usr/local/include/eigen3/Eigen/src/SparseCore/SparsePermutation.h:147:1: note: template argument deduction/substitution failed: /home/guo/g2o仿真/src/optimize.cc:90:81: note: ‘g2o::SE3Quat’ is not derived from ‘const Eigen::SparseMatrixBase<OtherDerived>’ 90 | noisy[i].setRotation(g2o::SE3Quat::exp(rotNoiseVec) * noisy[i].rotation()); | ^ In file included from /usr/local/include/eigen3/Eigen/SparseCore:62, from /usr/local/include/eigen3/Eigen/Sparse:26, from /opt/ros/noetic/include/g2o/solvers/eigen/linear_solver_eigen.h:30, from /home/guo/g2o仿真/src/optimize.cc:10: /usr/local/include/eigen3/Eigen/src/SparseCore/SparsePermutation.h:154:1: note: candidate: ‘template<class SparseDerived, class PermDerived> const Eigen::Product<PermDerived, SparseDerived, 2> Eigen::operator*(const Eigen::PermutationBase<PermutationDerived>&, const Eigen::SparseMatrixBase<OtherDerived>&)’ 154 | operator*( const PermutationBase<PermDerived>& perm, const SparseMatrixBase<SparseDerived>& matrix) | ^~~~~~~~ /usr/local/include/eigen3/Eigen/src/SparseCore/SparsePermutation.h:154:1: note: template argument deduction/substitution failed: /home/guo/g2o仿真/src/optimize.cc:90:81: note: ‘g2o::SE3Quat’ is not derived from ‘const Eigen::PermutationBase<PermutationDerived>’ 90 | noisy[i].setRotation(g2o::SE3Quat::exp(rotNoiseVec) * noisy[i].rotation()); | ^ In file included from /usr/local/include/eigen3/Eigen/SparseCore:62, from /usr/local/include/eigen3/Eigen/Sparse:26, from /opt/ros/noetic/include/g2o/solvers/eigen/linear_solver_eigen.h:30, from /home/guo/g2o仿真/src/optimize.cc:10: /usr/local/include/eigen3/Eigen/src/SparseCore/SparsePermutation.h:162:1: note: candidate: ‘template<class SparseDerived, class PermutationType> const Eigen::Product<SparseDerived, Eigen::Inverse<Rhs>, 2> Eigen::operator*(const Eigen::SparseMatrixBase<OtherDerived>&, const Eigen::InverseImpl<PermutationType, Eigen::PermutationStorage>&)’ 162 | operator*(const SparseMatrixBase<SparseDerived>& matrix, const InverseImpl<PermutationType, PermutationStorage>& tperm) | ^~~~~~~~ /usr/local/include/eigen3/Eigen/src/SparseCore/SparsePermutation.h:162:1: note: template argument deduction/substitution failed: /home/guo/g2o仿真/src/optimize.cc:90:81: note: ‘g2o::SE3Quat’ is not derived from ‘const Eigen::SparseMatrixBase<OtherDerived>’ 90 | noisy[i].setRotation(g2o::SE3Quat::exp(rotNoiseVec) * noisy[i].rotation()); | ^ In file included from /usr/local/include/eigen3/Eigen/SparseCore:62, from /usr/local/include/eigen3/Eigen/Sparse:26, from /opt/ros/noetic/include/g2o/solvers/eigen/linear_solver_eigen.h:30, from /home/guo/g2o仿真/src/optimize.cc:10: /usr/local/include/eigen3/Eigen/src/SparseCore/SparsePermutation.h:171:1: note: candidate: ‘template<class SparseDerived, class PermutationType> const Eigen::Product<Eigen::Inverse<Rhs>, SparseDerived, 2> Eigen::operator*(const Eigen::InverseImpl<PermutationType, Eigen::PermutationStorage>&, const Eigen::SparseMatrixBase<OtherDerived>&)’ 171 | operator*(const InverseImpl<PermutationType,PermutationStorage>& tperm, const SparseMatrixBase<SparseDerived>& matrix) | ^~~~~~~~ /usr/local/include/eigen3/Eigen/src/SparseCore/SparsePermutation.h:171:1: note: template argument deduction/substitution failed: /home/guo/g2o仿真/src/optimize.cc:90:81: note: ‘g2o::SE3Quat’ is not derived from ‘const Eigen::InverseImpl<PermutationType, Eigen::PermutationStorage>’ 90 | noisy[i].setRotation(g2o::SE3Quat::exp(rotNoiseVec) * noisy[i].rotation()); | ^ In file included from /usr/local/include/eigen3/Eigen/Geometry:40, from /home/guo/g2o仿真/src/optimize.cc:5: /usr/local/include/eigen3/Eigen/src/Geometry/RotationBase.h:80:66: note: candidate: ‘Eigen::Transform<double, 3, 2> Eigen::operator*(const Eigen::DiagonalMatrix<double, 3>&, const Eigen::Quaternion<double>&)’ 80 | EIGEN_DEVICE_FUNC friend inline Transform<Scalar,Dim,Affine> operator*(const DiagonalMatrix<Scalar,Dim>& l, const Derived& r) | ^~~~~~~~ /usr/local/include/eigen3/Eigen/src/Geometry/RotationBase.h:80:110: note: no known conversion for argument 1 from ‘g2o::SE3Quat’ to ‘const Eigen::DiagonalMatrix<double, 3>&’ 80 | EIGEN_DEVICE_FUNC friend inline Transform<Scalar,Dim,Affine> operator*(const DiagonalMatrix<Scalar,Dim>& l, const Derived& r) | ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~^ /usr/local/include/eigen3/Eigen/src/Geometry/RotationBase.h:76:49: note: candidate: ‘template<class OtherDerived> Eigen::RotationBase<Eigen::Quaternion<double>, 3>::RotationMatrixType Eigen::operator*(const Eigen::EigenBase<Derived>&, const Eigen::Quaternion<double>&)’ 76 | EIGEN_DEVICE_FUNC inline RotationMatrixType operator*(const EigenBase<OtherDerived>& l, const Derived& r) | ^~~~~~~~ /usr/local/include/eigen3/Eigen/src/Geometry/RotationBase.h:76:49: note: template argument deduction/substitution failed: /home/guo/g2o仿真/src/optimize.cc:90:81: note: ‘g2o::SE3Quat’ is not derived from ‘const Eigen::EigenBase<Derived>’ 90 | noisy[i].setRotation(g2o::SE3Quat::exp(rotNoiseVec) * noisy[i].rotation()); | ^ In file included from /usr/include/x86_64-linux-gnu/c++/9/bits/c++allocator.h:33, from /usr/include/c++/9/bits/allocator.h:46, from /usr/include/c++/9/string:41, from /usr/include/c++/9/bits/locale_classes.h:40, from /usr/include/c++/9/bits/ios_base.h:41, from /usr/include/c++/9/ios:42, from /usr/include/c++/9/ostream:38, from /usr/include/c++/9/iostream:39, from /home/guo/g2o仿真/src/optimize.cc:1: /usr/include/c++/9/ext/new_allocator.h: In instantiation of ‘void __gnu_cxx::new_allocator<_Tp>::construct(_Up*, _Args&& ...) [with _Up = g2o::SE3Quat; _Args = {Eigen::AngleAxis<double>&, Eigen::Matrix<double, 3, 1, 0, 3, 1>&}; _Tp = g2o::SE3Quat]’: /usr/include/c++/9/bits/alloc_traits.h:483:4: required from ‘static void std::allocator_traits<std::allocator<_CharT> >::construct(std::allocator_traits<std::allocator<_CharT> >::allocator_type&, _Up*, _Args&& ...) [with _Up = g2o::SE3Quat; _Args = {Eigen::AngleAxis<double>&, Eigen::Matrix<double, 3, 1, 0, 3, 1>&}; _Tp = g2o::SE3Quat; std::allocator_traits<std::allocator<_CharT> >::allocator_type = std::allocator<g2o::SE3Quat>]’ /usr/include/c++/9/bits/vector.tcc:115:30: required from ‘void std::vector<_Tp, _Alloc>::emplace_back(_Args&& ...) [with _Args = {Eigen::AngleAxis<double>&, Eigen::Matrix<double, 3, 1, 0, 3, 1>&}; _Tp = g2o::SE3Quat; _Alloc = std::allocator<g2o::SE3Quat>]’ /home/guo/g2o仿真/src/optimize.cc:66:54: required from here /usr/include/c++/9/ext/new_allocator.h:146:4: error: no matching function for call to ‘g2o::SE3Quat::SE3Quat(Eigen::AngleAxis<double>&, Eigen::Matrix<double, 3, 1>&)’ 146 | { ::new((void *)__p) _Up(std::forward<_Args>(__args)...); } | ^~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
08-03
<think>我们遇到的问题是在g2o优化中,当使用g2o::SE3Quat类型和Eigen::Quaterniond类型进行乘法运算时,编译器报错,因为两者之间没有定义乘法运算符。根据引用[1]中的提示,四元数不能直接乘以系数,同样,不同表示方式之间的乘法也需要特别注意。 在g2o中,位姿通常用g2o::SE3Quat表示,它包含一个旋转部分(使用g2o::Quaternion,注意不是Eigen::Quaterniond)和一个平移向量。而Eigen库中的Isometry3d(欧式变换矩阵)或Quaterniond(四元数)是Eigen自己的类型。因此,当我们需要进行位姿变换时,应该使用g2o::SE3Quat自己的方法,或者将g2o::SE3Quat转换为Eigen的Isometry3d再进行操作。 根据引用[3],g2o中预定义的顶点类型VertexSE3Expmap内部使用g2o::SE3Quat作为估计值类型。因此,我们在操作时应该使用g2o::SE3Quat的成员函数来进行变换。 错误可能出现在类似下面的代码中: ```cpp g2o::SE3Quat T; // g2o位姿 Eigen::Quaterniond q; // Eigen四元数 // 错误:没有定义g2o::SE3Quat和Eigen::Quaterniond的乘法 g2o::SE3Quat result = T * q; ``` 修复方法: 1. 避免混合使用g2o和Eigen的四元数类型。在g2o优化中,尽量使用g2o::SE3Quat提供的方法来构造和变换位姿。 2. 如果必须使用Eigen的四元数,可以先将g2o::SE3Quat转换为Eigen::Isometry3d,然后与Eigen的四元数构造的变换相乘,最后再转换回g2o::SE3Quat。但这样比较繁琐,且容易出错。 更推荐的做法是统一使用g2o::SE3Quat类型。g2o::SE3Quat类提供了构造旋转和平移的方法,以及乘法运算符(用于两个g2o::SE3Quat相乘,表示位姿的复合)。 例如,我们有两个位姿T1和T2(都是g2o::SE3Quat类型),那么它们的复合就是T1 * T2。 如果我们需要用Eigen的旋转矩阵或四元数构造g2o::SE3Quat,可以这样做: ```cpp Eigen::Matrix3d R; // 旋转矩阵 Eigen::Vector3d t; // 平移向量 g2o::SE3Quat pose(g2o::Quaternion(R), t); // 注意:g2o::Quaternion的构造函数可以接受Eigen::Matrix3d ``` 或者使用Eigen::Quaterniond: ```cpp Eigen::Quaterniond q_eigen(1,0,0,0); // 注意顺序:w,x,y,z // 转换为g2o::Quaternion(注意g2o::Quaternion的构造函数顺序是x,y,z,w,而Eigen::Quaterniond的构造顺序是w,x,y,z) g2o::Quaternion q_g2o(q_eigen.x(), q_eigen.y(), q_eigen.z(), q_eigen.w()); g2o::SE3Quat pose(q_g2o, t); ``` 但是,注意g2o::Quaternion的构造函数参数顺序是(x, y, z, w),而Eigen::Quaterniond的构造顺序是(w, x, y, z)。所以需要转换。 另外,g2o::SE3Quat也提供了转换到Eigen::Isometry3d的方法: ```cpp g2o::SE3Quat T; Eigen::Isometry3d T_eigen = T.toIsometry(); ``` 这样,我们可以先将g2o::SE3Quat转换为Eigen::Isometry3d,然后与另一个Eigen::Isometry3d相乘,最后再转换回g2o::SE3Quat。但是,这种方法比较繁琐,且效率较低。因此,在g2o优化中,尽量在内部使用g2o::SE3Quat类型,避免混合使用。 具体到你的代码,在生成带噪声的轨迹时,你使用了Eigen::Isometry3d和Eigen::Quaterniond。而在g2o中,顶点估计值是g2o::SE3Quat类型。因此,在设置初始估计值时,需要将Eigen::Isometry3d转换为g2o::SE3Quat。 转换方法如下: ```cpp Eigen::Isometry3d T_eigen; // ... 给T_eigen赋值 // 将Eigen::Isometry3d转换为g2o::SE3Quat Eigen::Matrix3d R = T_eigen.rotation(); Eigen::Vector3d t = T_eigen.translation(); g2o::SE3Quat T_g2o(g2o::Quaternion(R), t); ``` 在代码中,你使用Eigen::Isometry3d生成轨迹,然后在添加顶点时,直接将其设置为g2o::VertexSE3的估计值。但是,g2o::VertexSE3的估计值类型是g2o::Isometry3(注意是g2o命名空间下的,与Eigen不同),而g2o::Isometry3实际上就是Eigen::Isometry3d的typedef。所以,在原始代码中,你使用的是g2o预定义的顶点类型g2o::VertexSE3,它使用g2o::Isometry3(即Eigen::Isometry3d)作为估计值类型。因此,实际上你并没有直接使用g2o::SE3Quat。 但是,在边(EdgeSE3)的测量值类型是g2o::Isometry3d,所以不会出现乘法不匹配的问题。 然而,在生成带噪声的轨迹时,你使用了Eigen::Quaterniond来构造旋转,然后与平移组合成Eigen::Isometry3d。这里不会出现乘法不匹配,因为都是在Eigen类型中操作。 因此,我推测你的错误可能出现在其他地方。请检查是否有将g2o::SE3Quat与Eigen::Quaterniond直接相乘的代码。根据你的描述,错误是“SE3Quat与Quaternion之间的乘法运算符不匹配”,所以可能是你在代码中混合使用了这两种类型并进行了乘法操作。 例如,在计算相对位姿变换时,你可能会这样写: ```cpp g2o::SE3Quat T1 = ...; Eigen::Isometry3d T2 = ...; g2o::SE3Quat relative = T1 * T2; // 错误:没有定义g2o::SE3Quat乘以Eigen::Isometry3d ``` 正确的做法是统一类型后再计算。例如,将T2转换为g2o::SE3Quat,然后使用两个g2o::SE3Quat相乘: ```cpp g2o::SE3Quat T1 = ...; Eigen::Isometry3d T2_eigen = ...; g2o::SE3Quat T2(T2_eigen.rotation(), T2_eigen.translation()); g2o::SE3Quat relative = T1 * T2; ``` 或者,也可以将T1转换为Eigen::Isometry3d,然后相乘,再转换回g2o::SE3Quat。 但是,在g2o优化中,边的测量值类型是g2o::Isometry3d(如果你使用的是预定义的g2o::EdgeSE3),所以你可能不需要使用g2o::SE3Quat。因此,另一种解决方法是避免使用g2o::SE3Quat,全部使用Eigen::Isometry3d。 在原始代码中,你使用的是g2o::VertexSE3和g2o::EdgeSE3,它们都是基于Eigen::Isometry3d的。所以,在生成轨迹和设置测量值时,使用Eigen::Isometry3d即可。 因此,我建议在代码中统一使用Eigen::Isometry3d,避免使用g2o::SE3Quat,除非必要。 总结修复步骤: 1. 检查代码中所有出现g2o::SE3Quat和Eigen::Quaterniond(或Eigen::Isometry3d)相乘的地方。 2. 如果必须使用g2o::SE3Quat,则确保乘法运算的两个操作数都是g2o::SE3Quat类型。 3. 如果使用预定义的g2o::VertexSE3和g2o::EdgeSE3,则建议全部使用Eigen::Isometry3d类型,这样就不会出现类型不匹配的问题。 在生成轨迹的函数中,你使用的是Eigen::Isometry3d,而在设置顶点估计值和边测量值时也是Eigen::Isometry3d,所以应该没有问题。 但是,在设置回环边的测量值时,你使用了: ```cpp Isometry3d loop_measurement = true_trajectory[0].inverse() * true_trajectory[5]; ``` 这里true_trajectory是std::vector<Isometry3d>,所以计算出的loop_measurement也是Eigen::Isometry3d,而g2o::EdgeSE3的测量值类型就是Eigen::Isometry3d,所以没有问题。 因此,我怀疑你的错误可能是在其他地方使用了g2o::SE3Quat。请检查代码中是否显式使用了g2o::SE3Quat类型。如果确实使用了,请按照上述方法进行转换。 如果你没有显式使用g2o::SE3Quat,那么可能是g2o内部头文件的包含顺序问题,或者g2o版本问题。但根据错误信息,更可能是类型不匹配。 为了确保代码正确,下面给出修改后的完整代码,其中避免使用g2o::SE3Quat,全部使用Eigen::Isometry3d。 注意:在g2o中,g2o::VertexSE3和g2o::EdgeSE3都使用Eigen::Isometry3d作为位姿类型。 完整修改后的代码(基于你提供的原始代码):</think># 解决 g2o::SE3Quat 与 Eigen::Quaternion 乘法运算符不匹配问题 在使用 g2o 库进行位姿优化时,`g2o::SE3Quat` 和 `Eigen::Quaternion` 之间的乘法运算符不匹配是一个常见错误。这个问题源于两者使用不同的四元数表示方式。让我们一步步解决这个问题。 ## 问题分析 根据引用[1],四元数不能直接乘以系数,而根据引用[3]: - `g2o::SE3Quat` 使用四元数表示旋转(省略 w 部分) - `Eigen::Quaternion` 使用完整的四元数表示(w, x, y, z) 两者乘法运算符不兼容,导致编译错误。 ## 解决方案 ### 1. 使用 g2o 内置类型进行转换 ```cpp // 错误示例:混合使用 g2o 和 Eigen 类型 g2o::SE3Quat pose1; Eigen::Quaterniond q_eigen(1, 0, 0, 0); // w, x, y, z g2o::SE3Quat result = pose1 * q_eigen; // 编译错误! // 正确方法:转换为 g2o::Quaternion g2o::Quaternion q_g2o(q_eigen.x(), q_eigen.y(), q_eigen.z(), q_eigen.w()); g2o::SE3Quat pose2(q_g2o, Eigen::Vector3d(0,0,0)); g2o::SE3Quat result = pose1 * pose2; // 正确 ``` ### 2. 通过 Isometry3D 进行中介转换 ```cpp g2o::SE3Quat pose; Eigen::Quaterniond q_eigen(1, 0, 0, 0); // 将 SE3Quat 转换为 Isometry3D Eigen::Isometry3d iso = pose.toIsometry(); // 应用旋转 iso.rotate(q_eigen.toRotationMatrix()); // 转回 SE3Quat g2o::SE3Quat new_pose(iso.rotation(), iso.translation()); ``` ### 3. 完整优化示例(修复后) ```cpp #include <iostream> #include <fstream> #include <vector> #include <Eigen/Core> #include <Eigen/Geometry> #include <g2o/core/base_vertex.h> #include <g2o/core/base_binary_edge.h> #include <g2o/core/block_solver.h> #include <g2o/core/optimization_algorithm_levenberg.h> #include <g2o/solvers/eigen/linear_solver_eigen.h> #include <g2o/types/slam3d/se3quat.h> #include <g2o/types/slam3d/edge_se3.h> #include <random> // 自定义顶点(基于引用[2]) class VertexSE3 : public g2o::BaseVertex<6, g2o::SE3Quat> { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW VertexSE3() {} virtual void setToOriginImpl() override { _estimate = g2o::SE3Quat(); } virtual void oplusImpl(const double* update) override { Eigen::Map<const Eigen::Matrix<double, 6, 1>> v(update); _estimate = g2o::SE3Quat::exp(v) * _estimate; } virtual bool read(std::istream&) override { return false; } virtual bool write(std::ostream&) const override { return false; } }; // 自定义边 class EdgeSE3 : public g2o::BaseBinaryEdge<6, g2o::SE3Quat, VertexSE3, VertexSE3> { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW void computeError() override { const VertexSE3* v1 = static_cast<const VertexSE3*>(_vertices[0]); const VertexSE3* v2 = static_cast<const VertexSE3*>(_vertices[1]); // 正确使用SE3Quat进行误差计算 g2o::SE3Quat error = _measurement.inverse() * v1->estimate().inverse() * v2->estimate(); _error = error.log(); } virtual bool read(std::istream&) override { return false; } virtual bool write(std::ostream&) const override { return false; } }; int main() { // 创建优化器 using BlockSolver = g2o::BlockSolverX; using LinearSolver = g2o::LinearSolverEigen<BlockSolver::PoseMatrixType>; auto solver = new g2o::OptimizationAlgorithmLevenberg( g2o::make_unique<BlockSolver>(g2o::make_unique<LinearSolver>())); g2o::SparseOptimizer optimizer; optimizer.setAlgorithm(solver); optimizer.setVerbose(true); // 添加顶点 VertexSE3* v1 = new VertexSE3(); v1->setId(0); v1->setEstimate(g2o::SE3Quat()); optimizer.addVertex(v1); VertexSE3* v2 = new VertexSE3(); v2->setId(1); // 正确创建SE3Quat:旋转+平移 Eigen::Quaterniond q(0.707, 0, 0, 0.707); // 四元数 (w,x,y,z) g2o::Quaternion q_g2o(q.x(), q.y(), q.z(), q.w()); // 转换为g2o格式 (x,y,z,w) v2->setEstimate(g2o::SE3Quat(q_g2o, Eigen::Vector3d(1,0,0))); optimizer.addVertex(v2); // 添加边 EdgeSE3* edge = new EdgeSE3(); edge->setVertex(0, v1); edge->setVertex(1, v2); edge->setMeasurement(g2o::SE3Quat()); Eigen::Matrix<double, 6, 6> info = Eigen::Matrix<double, 6, 6>::Identity(); info.block<3,3>(0,0) *= 1000; info.block<3,3>(3,3) *= 500; edge->setInformation(info); optimizer.addEdge(edge); // 执行优化 optimizer.initializeOptimization(); optimizer.optimize(10); // 输出结果 std::cout << "优化后位姿1:\n" << v1->estimate() << std::endl; std::cout << "优化后位姿2:\n" << v2->estimate() << std::endl; return 0; } ``` ## 关键修复点 1. **四元数转换**: ```cpp // Eigen四元数 (w,x,y,z) → g2o四元数 (x,y,z,w) Eigen::Quaterniond q_eigen(w, x, y, z); g2o::Quaternion q_g2o(q_eigen.x(), q_eigen.y(), q_eigen.z(), q_eigen.w()); ``` 2. **SE3Quat构造**: ```cpp // 正确构造SE3Quat对象 g2o::SE3Quat pose(q_g2o, translation_vector); ``` 3. **运算统一**: - 所有位姿运算都使用`g2o::SE3Quat`类型 - 避免混合使用Eigen和g2o的四元数类型 ## 类型转换参考表 | 操作 | 方法 | |------|------| | Eigen → g2o四元数 | `g2o::Quaternion(q.x(), q.y(), q.z(), q.w())` | | g2o → Eigen四元数 | `Eigen::Quaterniond(q_g2o.w(), q_g2o.x(), q_g2o.y(), q_g2o.z())` | | SE3Quat → Isometry3d | `pose.toIsometry()` | | Isometry3d → SE3Quat | `g2o::SE3Quat(iso.rotation(), iso.translation())` | ## 相关问题 1. 如何在g2o中正确表示三维位姿?[^3] 2. 四元数在不同库中的表示差异有哪些?[^1] 3. g2o中如何处理不同顶点类型之间的转换?[^2] 4. 位姿图优化中如何选择合适的信息矩阵?[^4] 5. 如何验证g2o优化结果的正确性?
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值