CN114170300A - 高精地图点云位姿优化方法、装置、设备及介质 - Google Patents
高精地图点云位姿优化方法、装置、设备及介质 Download PDFInfo
- Publication number
- CN114170300A CN114170300A CN202111505030.4A CN202111505030A CN114170300A CN 114170300 A CN114170300 A CN 114170300A CN 202111505030 A CN202111505030 A CN 202111505030A CN 114170300 A CN114170300 A CN 114170300A
- Authority
- CN
- China
- Prior art keywords
- pose
- point cloud
- frame
- frame point
- residual error
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
Images
Classifications
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/70—Determining position or orientation of objects or cameras
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C22/00—Measuring distance traversed on the ground by vehicles, persons, animals or other moving solid bodies, e.g. using odometers, using pedometers
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/30—Determination of transform parameters for the alignment of images, i.e. image registration
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T2207/00—Indexing scheme for image analysis or image enhancement
- G06T2207/10—Image acquisition modality
- G06T2207/10028—Range image; Depth image; 3D point clouds
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Theoretical Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Processing Or Creating Images (AREA)
Abstract
本公开提供了高精地图点云位姿优化方法、装置、设备及介质,涉及计算机技术领域,尤其涉及高精地图技术领域。具体技术方案为:获取目标环境对应的至少一个点云对中每帧点云的第一位姿;点云对包括目标环境对应的多帧点云中相邻的两帧点云;获取每个点云对中两帧点云之间的相对位姿;相对位姿是里程计模块估计出的;根据第一位姿和相对位姿,对至少一个点云对的位姿轨迹进行优化,得到每帧点云的第二位姿。本公开的技术方案可使位姿轨迹更加平滑,克服GNSS信号较弱或缺失对位姿的影响。
Description
技术领域
本公开涉及计算机技术领域,尤其涉及高精地图技术领域,具体涉及一种高精地图点云位姿优化方法、装置、设备及介质。
背景技术
高精地图也称高精度地图,是自动驾驶汽车使用。高精地图,拥有精确的车辆位置信息和丰富的道路元素数据信息,可以帮助汽车预知路面复杂信息,如坡度、曲率、航向等,更好地规避潜在的风险。大规模的室外场景建图,一般需要依赖于GNSS(GlobalNavigation Satellite System,全球导航卫星系统)获得初始的全局位姿,然后进行回环检测、点云配准融合等。但在立交桥、林荫路等场景GNSS信号容易丢失或者信号较弱,导致初始的位姿轨迹漂移、不平滑,导致后续的点云融合失败率较高。
发明内容
本公开提供了一种高精地图点云位姿优化方法、装置、设备及介质。
根据本公开的第一方面,提供了一种点云位姿优化方法,包括:
获取目标环境对应的至少一个点云对中每帧点云的第一位姿;所述点云对包括目标环境对应的多帧点云中相邻的两帧点云;
获取每个点云对中两帧点云之间的相对位姿;相对位姿是里程计模块估计出的;
根据上述第一位姿和相对位姿,对至少一个点云对的位姿轨迹进行优化,得到每帧点云的第二位姿。
根据本公开的第二方面,提供了一种点云位姿优化装置,包括:
位姿获取模块,用于获取目标环境对应的至少一个点云对中每帧点云的第一位姿;点云对包括目标环境对应的多帧点云中相邻的两帧点云;
相对位姿获取模块,用于获取每个点云对中两帧点云之间的相对位姿;相对位姿是里程计模块估计出的;
第一优化模块,用于根据上述第一位姿和相对位姿,对至少一个点云对的位姿轨迹进行优化,得到每帧点云的第二位姿。
根据本公开的第三方面,提供了一种电子设备,包括:至少一个处理器;以及与至少一个处理器通信连接的存储器;其中,存储器存储有可被至少一个处理器执行的指令,该指令被至少一个处理器执行,以使至少一个处理器能够执行本公开任一实施例提供的点云位姿优化方法。
根据本公开的第四方面,提供了一种存储有计算机指令的非瞬时计算机可读存储介质,其中,计算机指令用于使计算机执行本公开任一实施例提供的点云位姿优化方法。
根据本公开的第五方面,提供了一种计算机程序产品,包括计算机程序,该计算机程序在被处理器执行时实现本公开任一实施例提供的点云位姿优化方法。
本公开的技术方案可至少实现如下有益效果:
本公开的技术方案基于里程计估计的相对位姿对点云的位姿轨迹进行优化,可减少对GNSS信号的依赖,使位姿轨迹更加平滑,以克服GNSS信号较弱或缺失对位姿的影响,进而提高点云配准融合的成功率,有利于精确的构建大规模室外场景的高精度地图。
应当理解,本部分所描述的内容并非旨在标识本公开的实施例的关键或重要特征,也不用于限制本公开的范围。本公开的其它特征将通过以下的说明书而变得容易理解。
附图说明
附图用于更好地理解本方案,不构成对本公开的限定。其中:
图1是本公开实施例提供的一种点云位姿优化方法的流程示意图;
图2是本公开实施例涉及的位姿图的示意图;
图3是本公开实施例提供的另一种点云位姿优化方法的流程示意图;
图4是本公开实施例提供的点云位姿优化方法的一个示例的原理示意图;
图5是本公开实施例提供的一种点云位姿优化装置的结构框架示意图;
图6是本公开实施例提供的另一种点云位姿优化装置的结构框架示意图;
图7是本公开实施例提供的一种电子设备的结构框架示意图。
具体实施方式
以下结合附图对本公开的示范性实施例做出说明,其中包括本公开实施例的各种细节以助于理解,应当将它们认为仅仅是示范性的。因此,本领域普通技术人员应当认识到,可以对这里描述的实施例做出各种改变和修改,而不会背离本公开的范围和精神。同样,为了清楚和简明,以下的描述中省略了对公知功能和结构的描述。
在本公开实施例的描述中,需要理解的是,术语“第一”、“第二”仅用于描述目的,而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此,限定有“第一”、“第二”的特征可以明示或者隐含地包括一个或者更多个该特征。在本公开实施例的描述中,除非另有说明,“多个”的含义是两个或两个以上。
应该进一步理解的是,本公开实施例中使用的措辞“和/或”包括一个或更多个相关联的列出项的全部或任一单元和全部组合。
本技术领域技术人员可以理解,除非另外定义,本公开实施例使用的所有术语(包括技术术语和科学术语),具有与本公开所属领域中的普通技术人员的一般理解相同的意义。
首先对本公开实施例涉及的几个术语作进行介绍:
高精地图:也称高精度地图、高分辨率地图或高度自动驾驶地图,是用于自动驾驶的专用地图,由含有语义信息的车道模型、道路部件、道路属性等矢量信息,以及用于多传感器定位的特征图层构成,在整个自动驾驶领域扮演着核心角色,可辅助自动驾驶车辆判断自身位置、可行驶区域、目标类型、行驶方向、前车相对位置、感知红绿灯状态及行驶车道等信息。
点云:指扫描资料以点的形式进行记录,每个点包含有三维坐标,还可能含有颜色信息,反射强度信息等。其中颜色信息通常是通过采集设备获取图像并将图像中对应位置的像素点的颜色信息赋予点云中的对应的点确定的。反射强度信息的获取是激光雷达接收装置采集到的回波强度,此强度信息与目标的表面材质,粗糙度,入射角方向以及仪器的发射能量,激光波长有关。
位姿:指点云在指定坐标系中的位置和姿态。
本公开的发明人在研究中发现在大规模的室外场景建图时,在立交桥、林荫路等路段的GNSS信号容易丢失或者信号较弱,导致初始的位姿轨迹漂移、不平滑,导致后续的点云融合失败率较高。目前对于GNSS信号不好的路段,通常需要人工介入处理,导致高精地图的生产效率较低。
下面以具体实施例对本公开的技术方案以及本公开的技术方案如何解决上述技术问题进行详细说明。
根据本公开的实施例,本公开提供了一种点云位姿优化方法,如图1所示,该方法包括:
S101,获取目标环境对应的至少一个点云对中每帧点云的第一位姿。
点云对包括目标环境对应的多帧点云中相邻的两帧点云。在目标环境对应的多帧点云中,任意相邻的两帧点云均可形成一个点云对。其中,目标环境对应的多帧点云可以是由采集设备在目标环境中采集得到的,采集设备可以是车辆、无人机、移动机器人等移动设备。
本公开实施例可以在采集设备采集到各点云对的点云后进行点云位姿的优化,也可以是采集设备的采集过程中对已采集到的点云的位姿进行实时优化。
对于上述每帧点云,第一位姿可以是由该帧点云对应的GNSS位姿对该帧点云的位姿进行初始化得到的,即将GNSS位姿作为该帧点云的第一位姿。
在一种可选的实施方式中,对于第i(i为任意整数)帧点云,其时间戳ti与GNSS位姿的时间戳可能存在不同步的情况,无法获取同一时间戳的GNSS位姿,可首先获取与时间戳ti相邻的两个时间戳的GNSS位姿,对两个GNSS位姿进行线性差值,得到一个新的GNSS位姿,该新的GNSS位姿即作为第i帧点云对应的GNSS位姿。例如,对于时间戳为第10秒的点云,可能没有同样为第10秒的GNSS位姿,若与其相邻的第9秒和第11秒具有GNSS位姿,则可对第9秒和第11秒的GNSS位姿进行线性插值,将线性插值得到数据作为第10秒的GNSS位姿。
S102,获取每个点云对中两帧点云之间的相对位姿。
相对位姿是里程计模块估计出的。
本公开实施例中的时里程计模块可以包括LIO(LiDAR Inertial Odometry,激光雷达惯性里程计)和视觉里程计中的至少一种。里程计模块可以基于激光雷达测量数据和/或图像对相邻两帧点云之间的相对位姿进行估计,基于里程计模块估计出的相对位姿对位姿轨迹进行优化,克服GNSS信号较弱或缺失对位姿轨迹的影响,进而提高点云配准融合的成功率,有利于精确的构建大规模室外场景的高精度地图,同时还可减少人工干预的频次,提高高精度地图的自动化程度和生产效率。
S103,根据各点云对中每帧点云的第一位姿和各点云对中两帧点云之间的相对位姿,对至少一个点云对的位姿轨迹进行优化,得到至少一个点云对中每帧点云的第二位姿。
本公开实施例中的位姿轨迹可以位姿图(Pose graph)的方式呈现,参照图2所示的位姿图示例,位姿图涉及多个节点(如图2中的圆圈所示)和节点之间的边(如图2中的长方形及其所在的线条所示),其中,节点为一帧点云的位姿,节点之间的边为两个节点的相对位姿,进行位姿图优化(Pose graph optimization)即可实现对位姿轨迹的优化。图2中的三角形表示用于初始化各节点的GNSS位姿,也即初始化后的第一位姿。
在一种可选的实施方式中,对每个点云对中的第一帧点云和第二帧点云执行以下操作:根据第一帧点云和第二帧点云之间的相对位姿、以及第二帧点云的第一位姿,构建当前点云对的第一位姿残差;对第一位姿残差进行优化,得到优化后的第一帧点云和第二帧点云的第二位姿。
在一示例中,第一帧点云和第二帧点云均可以是历史帧点云,可适用于先采集后优化的应用需求,例如高精度地图的制图需求;在另一个示例中,第一帧点云可以为历史帧点,第二帧点云可以是当前帧点云,可适用于采集过程中实时优化的应用需求。
基于该实施方式,本公开实施例可对各点云对进行残差构建,通过对残差进行优化,实现对位姿轨迹较为精确的优化。
在一种可选的实施方式中,根据第一帧点云和第二帧点云之间的相对位姿、以及第二帧点云的第一位姿,构建当前点云对的第一位姿残差,包括:根据第一帧点云和第二帧点云之间的相对位姿,构建当前点云对的第一位姿残差中的第一残差项;根据第二帧点云的第一位姿,构建当前点云对的第一位姿残差中的第二残差项。
在一种可选的实施方式中,第一位姿残差中的第一残差项的构建方式可参照如下表达式:
在表达式(1)中,Ti表示第i帧点云(作为一个点云对中的第一帧点云)的位姿,Ti+1表示第i+1帧点云(作为一个点云对中的第二帧点云)的位姿,表示基于Ti和Ti+1计算出第i帧点云和第i+1帧点云之间的相对位姿,Todom_ii+1表示里程计估计出的第i帧点云和第i+1帧点云之间的相对位姿;rodom_ii+1表示基于Ti、Ti+1和相对位姿Todom_ii+1构建的第一残差项,其具体用于表征实际测量或计算出的相对位姿和里程计估计出的相对位姿Todom_ii+1的差异。
在一种可选的实施方式中,第二残差项的构建方式可参照如下表达式:
在表达式(2)中,Ti+1的含义同表达式(1);Tgnss_i+1表示用于初始化第i+1帧点云的GNSS位姿,也即初始化后第i+1帧点云的第一位姿;rgnss_i+1表示基于第i+1帧点云的位姿和GNSS位姿构建的第二残差项,其具体用于表征实际测量出的第i+1帧点云的位姿和用于初始化第i+1帧点云位姿的GNSS位姿的差异。
在另一种可选的实施方式中,表达式(2)中的Ti+1还可以由Ti来替代。
在一种可选的实施方式中,对第一位姿残差进行优化的具体方式可以是最小化第一位姿残差,具体地,可最小化第一位姿残差中的第一残差项和第二残差项,最小化可由如下表达式表达:
在表达式(3)中,Ωodom表示第一残差项rodom_ii+1的权重,Ωgnss表示第二残差项rgnss_i+1的权重,两个残差项的权重可根据实际需求设置;T表示转置算法;通过表达式(3)的计算可得到第i帧点的位姿Ti和第i+1帧的位姿Ti+1的优化后的数值。
本公开实施例在构建第一位姿残差时,分别基于点云相对位姿的第一残差项和基于点云位姿的第二残差项,较为全面地考虑了点云位姿信息的误差情况,可提升优化过程的精度。
本公开对图1所示的各步骤的先后顺序不作限定,可以根据实际需求调整各步骤的顺序,例如步骤S101和S102可以如图1所示先后执行,也可以同时执行。
根据本公开的实施例,本公开还提供了一种点云位姿优化方法,如图3所示,该方法包括:
S301,获取目标环境对应的至少一个点云对中每帧点云的第一位姿。
S302,获取每个点云对中两帧点云之间的相对位姿。
S303,根据各点云对中每帧点云的第一位姿和各点云对中两帧点云之间的相对位姿,对至少一个点云对的位姿轨迹进行优化,得到至少一个点云对中每帧点云的第二位姿。
步骤S301至S303的具体实施方式可参照前面的步骤S101至S103,此处不再赘述。
S304,根据各点云对中每帧点云的第二位姿和各点云对中两帧点云之间的相对位姿,对至少一个点云对的位姿轨迹进行优化,得到至少一个点云对中每帧点云的第三位姿。
根据第一次优化后得到的点云的第二位姿和里程计模块估计出的相对位姿,对位姿图中的轨迹进行第二次优化,可使轨迹更加平滑,进一步减少对GNSS信号依赖,提高点云配准融合的成功率,有利于构建更加精确的高精度地图。
在一种可选的实施方式中,对于各点云对的至少部分点云,若第一优化后的第二位姿和第一次优化前的第一位姿的距离差的绝对值大于预设距离阈值,即第一次优化前后的位姿误差过大,则认为第一位姿(也即GNSS位姿)的可靠性较差,将其剔除后,以第二位姿来代替第一位姿再次进行优化,再次优化后可位姿轨迹的平滑度得到较大程度的改善。距离阈值可根据实际需求设置。
在一种可选的实施方式中,对每个点云对中的第一帧点云和第二帧点云执行以下操作:在第二帧点云的第二位姿和第一位姿的距离差的绝对值大于预设距离阈值的情况下,根据第一帧点云和第二帧点云的第二位姿、第一帧点云和第二帧点云之间的相对位姿、以及第二帧点云的第一位姿,构建当前点云对的第二位姿残差;对第二位姿残差进行优化,得到优化后的第一帧点云和第二帧点云的第三位姿。
基于该实施方式可更新各点云对的位姿参数,基于新的位姿参数进行残差,通过残差进行优化,实现较为精确的第二次优化。
在一种可选的实施方式中,第一帧点云和第二帧点云的第二位姿、第一帧点云和第二帧点云之间的相对位姿、以及第二帧点云的第一位姿,构建当前点云对的第二位姿残差,包括:根据第一帧点云和第二帧点云的第二位姿、以及第一帧点云和第二帧点云之间的相对位姿,构建当前点云对的第二位姿残差中的第一残差项;根据第二帧点云的第一位姿和第二位姿,构建当前点云对的第二位姿残差中的第二残差项。
在一种可选的实施方式中,第二位姿残差中的第一残差项的构建方式可参照前面的表达式(1),表达式(1)中的Ti和Ti+1分别更新为第i帧点云的第二位姿和第i+1帧点云的第二位姿,其余参数的含义同前。
在一种可选的实施方式中,第二位姿残差中的第二残差项的构建方式可参照前面的表达式(2),表达式(2)中的Ti+1更新为第i+1帧点云的第二位姿,其余参数的含义同前。
在一种可选的实施方式中,对第二位姿残差进行优化的具体方法可以是最小化第二位姿残差,具体地,可最小化第二位姿残差中的第一残差项和第二残差项,最小化的方式可参照前面的表达式(3)。
本公开实施例在构建第二位姿残差时,分别构建基于点云相对位姿的第一残差项和基于点云位姿的第二残差项,较为全面地考虑了点云位姿信息的误差情况,可提升第二次优化过程的精度。
在一种可选的实施方式中,本公开提供的点云位姿优化方法,还包括:确定第三位姿形成的位姿轨迹的平滑度是否满足预设的平滑度条件;在平滑度不满足平滑度条件的情况下,对第三位姿形成的位姿轨迹继续进行至少一次优化,直至优化后的位姿轨迹的平滑度满足上述平滑度条件。
在前两次优化后的位姿轨迹不够平滑的情况下,本公开实施例可对位姿轨迹继续进行优化,从而可不断提高位姿轨迹的平滑度,以满足实际需要。
平滑度条件可根据实际需求设置,对第三位姿形成的位姿轨迹进行每一次优化,均可参照前面第二次优化的方式,对表达式(1)至表达式(2)的相应参数进行迭代更新即可。
本公开对图3所示的各步骤的先后顺序不作限定,可以根据实际需求调整各步骤的顺序,例如步骤S301和S302可以如图3所示先后执行,也可以同时执行。
图4示出了对位姿图进行优化的原理示意图,参照图4,本公开实施例提供的点云位姿优化方法的一个具体示例如下:
图4中的轨迹a为期望得到的位姿轨迹,由于实际场景中存在遮挡路段,GNSS信号较差甚至缺失(如图4中所示的信号缺失区域),导致位姿轨迹发生了变化,产生轨迹b,轨迹b中线条下凹的部分即对应信号缺失区域,通过本公开实施例的第一次优化可得到轨迹c,轨迹c的平滑度可得到有效的改善。轨迹c和轨迹b在信号缺失区域距离较远,轨迹b在信号缺失区域的GNSS位姿约束的可靠性明显较低,此时可将轨迹b中信号缺失区域的GNSS位姿的约束剔除,以第一次优化后的位姿也即轨迹c在信号缺失区域的位姿代替被剔除的GNSS位姿对位姿轨迹进行再次优化,可得到更加平滑的轨迹d。
根据本公开的实施例,本公开还提供了一种点云位姿优化装置,如图5所示,该装置包括:初始位姿获取模块501、相对位姿获取模块502和第一优化模块503。
初始位姿获取模块501,用于获取目标环境对应的至少一个点云对中每帧点云的第一位姿;点云对包括目标环境对应的多帧点云中相邻的两帧点云。
相对位姿获取模块502,用于获取每个点云对中两帧点云之间的相对位姿;相对位姿是里程计模块估计出的。
第一优化模块503,用于根据各点云对中每帧点云的第一位姿和各点云对中两帧点云之间的相对位姿,对至少一个点云对的位姿轨迹进行优化,得到每帧点云的第二位姿。
在一种可选的实施方式中,第一优化模块503具体用于对每个点云对中的第一帧点云和第二帧点云执行以下操作:
根据第一帧点云和第二帧点云之间的相对位姿、以及第二帧点云的第一位姿,构建当前点云对的第一位姿残差;对第一位姿残差进行优化,得到优化后的第一帧点云和第二帧点云的第二位姿。
在一种可选的实施方式中,第一优化模块503在构建当前点云对的第一位姿残差时,具体用于:根据第一帧点云和第二帧点云之间的相对位姿,构建当前点云对的第一位姿残差中的第一残差项;根据第二帧点云的第一位姿,构建当前点云对的第一位姿残差中的第二残差项。
在一种可选的实施方式中,如图6所示,本公开提供的点云位姿优化装置还包括:初始位姿获取模块601、相对位姿获取模块602和第一优化模块603和第二优化模块604。
初始位姿获取模块601、相对位姿获取模块602和第一优化模块603的功能可参照前面的初始位姿获取模块501、相对位姿获取模块502和第一优化模块503的功能;第二优化模块604用于根据各点云对中每帧点云第二位姿和各点云对中两帧点云之间的相对位姿,对至少一个点云对的位姿轨迹进行优化,得到至少一个点云对中每帧点云的第三位姿。
在一种可选的实施方式中,第二优化模块604具体用于对每个点云对中的第一帧点云和第二帧点云执行以下操作:
在第二帧点云的第二位姿和第一位姿的距离差的绝对值大于预设距离阈值的情况下,根据第一帧点云和第二帧点云的第二位姿、第一帧点云和第二帧点云之间的相对位姿、以及第二帧点云的第一位姿,构建当前点云对的第二位姿残差;对第二位姿残差进行优化,得到优化后的第一帧点云和第二帧点云的第三位姿。
在一种可选的实施方式中,第二优化模块604在构建第二位姿鳌头时,具体用于:根据第一帧点云和第二帧点云的第二位姿、以及第一帧点云和第二帧点云之间的相对位姿,构建当前点云对的第二位姿残差中的第一残差项;根据第二帧点云的第一位姿和第二位姿,构建当前点云对的第二位姿残差中的第二残差项。
在一种可选的实施方式中,本公开提供的点云位姿优化装置还包括:第三优化模块。该第三优化模块用于确定第三位姿形成的位姿轨迹的平滑度是满足预设的平滑度条件;在平滑度不满足平滑度条件的情况下,在第三位姿形成的位姿轨迹继续进行至少一次优化,直至优化后的位姿轨迹的平滑度满足平滑度条件。
本公开实施例各装置中的模块的功能可以参照上述方法实施例中的对应描述,在此不再赘述。
本公开的技术方案中,所涉及的用户个人信息的获取,存储和应用等,均符合相关法律法规的规定,且不违背公序良俗。
根据本公开的实施例,本公开还提供了一种电子设备、一种非瞬时计算机可读存储介质和一种计算机程序产品。
本公开提供的电子设备,包括:至少一个处理器;以及与至少一个处理器通信连接的存储器;其中,存储器存储有可被至少一个处理器执行的指令,该指令被至少一个处理器执行,以使至少一个处理器能够执行本公开任一实施例提供的点云位姿优化方法。
本公开提供的非瞬时计算机可读存储介质,存储有计算机指令,该计算机指令用于使计算机执行本公开任一实施例提供的点云位姿优化方法。
本公开提供的计算机程序产品,包括计算机程序,该计算机程序在被处理器执行时实现本公开任一实施例提供的点云位姿优化方法。
图7示出了可以用来实施本公开的实施例的示例电子设备700的示意性框图。电子设备旨在表示各种形式的数字计算机,诸如,膝上型计算机、台式计算机、工作台、个人数字助理、服务器、刀片式服务器、大型计算机、和其它适合的计算机。电子设备还可以表示各种形式的移动装置,诸如,个人数字处理、蜂窝电话、智能电话、可穿戴设备和其它类似的计算装置。本文所示的部件、它们的连接和关系、以及它们的功能仅仅作为示例,并且不意在限制本文中描述的和/或者要求的本公开的实现。
如图7所示,设备700包括计算单元701,其可以根据存储在只读存储器(ROM)702中的计算机程序或者从存储单元708加载到随机访问存储器(RAM)703中的计算机程序,来执行各种适当的动作和处理。在RAM703中,还可存储设备700操作所需的各种程序和数据。计算单元701、ROM 702以及RAM 703通过总线704彼此相连。输入/输出(I/O)接口705也连接至总线704。
设备700中的多个部件连接至I/O接口705,包括:输入单元706,例如键盘、鼠标等;输出单元707,例如各种类型的显示器、扬声器等;存储单元708,例如磁盘、光盘等;以及通信单元709,例如网卡、调制解调器、无线通信收发机等。通信单元709允许设备700通过诸如因特网的计算机网络和/或各种电信网络与其他设备交换信息/数据。
计算单元701可以是各种具有处理和计算能力的通用和/或专用处理组件。计算单元701的一些示例包括但不限于中央处理单元(CPU)、图形处理单元(GPU)、各种专用的人工智能(AI)计算芯片、各种运行机器学习模型算法的计算单元、数字信号处理器(DSP)、以及任何适当的处理器、控制器、微控制器等。计算单元701执行上文所描述的各个方法和处理。例如,在一些实施例中,上述方法可被实现为计算机软件程序,其被有形地包含于机器可读介质,例如存储单元708。在一些实施例中,计算机程序的部分或者全部可以经由ROM 702和/或通信单元709而被载入和/或安装到设备700上。当计算机程序加载到RAM 703并由计算单元701执行时,可以执行上文描述的方法的一个或多个步骤。备选地,在其他实施例中,计算单元701可以通过其他任何适当的方式(例如,借助于固件)而被配置为执行上述方法。
本文中以上描述的系统和技术的各种实施方式可以在数字电子电路系统、集成电路系统、场可编程门阵列(FPGA)、专用集成电路(ASIC)、专用标准产品(ASSP)、芯片上系统的系统(SOC)、负载可编程逻辑设备(CPLD)、计算机硬件、固件、软件、和/或它们的组合中实现。这些各种实施方式可以包括:实施在一个或者多个计算机程序中,该一个或者多个计算机程序可在包括至少一个可编程处理器的可编程系统上执行和/或解释,该可编程处理器可以是专用或者通用可编程处理器,可以从存储系统、至少一个输入装置、和至少一个输出装置接收数据和指令,并且将数据和指令传输至该存储系统、该至少一个输入装置、和该至少一个输出装置。
用于实施本公开的方法的程序代码可以采用一个或多个编程语言的任何组合来编写。这些程序代码可以提供给通用计算机、专用计算机或其他可编程数据处理装置的处理器或控制器,使得程序代码当由处理器或控制器执行时使流程图和/或框图中所规定的功能/操作被实施。程序代码可以完全在机器上执行、部分地在机器上执行,作为独立软件包部分地在机器上执行且部分地在远程机器上执行或完全在远程机器或服务器上执行。
在本公开的上下文中,机器可读介质可以是有形的介质,其可以包含或存储以供指令执行系统、装置或设备使用或与指令执行系统、装置或设备结合地使用的程序。机器可读介质可以是机器可读信号介质或机器可读储存介质。机器可读介质可以包括但不限于电子的、磁性的、光学的、电磁的、红外的、或半导体系统、装置或设备,或者上述内容的任何合适组合。机器可读存储介质的更具体示例会包括基于一个或多个线的电气连接、便携式计算机盘、硬盘、随机存取存储器(RAM)、只读存储器(ROM)、可擦除可编程只读存储器(EPROM或快闪存储器)、光纤、便捷式紧凑盘只读存储器(CD-ROM)、光学储存设备、磁储存设备、或上述内容的任何合适组合。
为了提供与用户的交互,可以在计算机上实施此处描述的系统和技术,该计算机具有:用于向用户显示信息的显示装置(例如,CRT(阴极射线管)或者LCD(液晶显示器)监视器);以及键盘和指向装置(例如,鼠标或者轨迹球),用户可以通过该键盘和该指向装置来将输入提供给计算机。其它种类的装置还可以用于提供与用户的交互;例如,提供给用户的反馈可以是任何形式的传感反馈(例如,视觉反馈、听觉反馈、或者触觉反馈);并且可以用任何形式(包括声输入、语音输入或者、触觉输入)来接收来自用户的输入。
可以将此处描述的系统和技术实施在包括后台部件的计算系统(例如,作为数据服务器)、或者包括中间件部件的计算系统(例如,应用服务器)、或者包括前端部件的计算系统(例如,具有图形用户界面或者网络浏览器的用户计算机,用户可以通过该图形用户界面或者该网络浏览器来与此处描述的系统和技术的实施方式交互)、或者包括这种后台部件、中间件部件、或者前端部件的任何组合的计算系统中。可以通过任何形式或者介质的数字数据通信(例如,通信网络)来将系统的部件相互连接。通信网络的示例包括:局域网(LAN)、广域网(WAN)和互联网。
计算机系统可以包括客户端和服务器。客户端和服务器一般远离彼此并且通常通过通信网络进行交互。通过在相应的计算机上运行并且彼此具有客户端-服务器关系的计算机程序来产生客户端和服务器的关系。服务器可以是云服务器,也可以为分布式系统的服务器,或者是结合了区块链的服务器。
应该理解,可以使用上面所示的各种形式的流程,重新排序、增加或删除步骤。例如,本发公开中记载的各步骤可以并行地执行也可以顺序地执行也可以不同的次序执行,只要能够实现本公开公开的技术方案所期望的结果,本文在此不进行限制。
上述具体实施方式,并不构成对本公开保护范围的限制。本领域技术人员应该明白的是,根据设计要求和其他因素,可以进行各种修改、组合、子组合和替代。任何在本公开的精神和原则之内所作的修改、等同替换和改进等,均应包含在本公开保护范围之内。
Claims (17)
1.一种点云位姿优化方法,包括:
获取目标环境的至少一个点云对中每帧点云的第一位姿;所述点云对包括所述目标环境对应的多帧点云中相邻的两帧点云;
获取每个点云对中两帧点云之间的相对位姿;所述相对位姿是里程计模块估计出的;
根据所述第一位姿和所述相对位姿,对所述至少一个点云对的位姿轨迹进行优化,得到所述每帧点云的第二位姿。
2.根据权利要求1所述的点云位姿优化方法,其中,所述根据所述第一位姿和所述相对位姿,对所述至少一个点云对的位姿轨迹进行优化,得到所述每帧点云的第二位姿,包括:
对每个点云对中的第一帧点云和第二帧点云执行以下操作:
根据所述第一帧点云和所述第二帧点云之间的相对位姿、以及所述第二帧点云的第一位姿,构建当前点云对的第一位姿残差;
对所述第一位姿残差进行优化,得到优化后的所述第一帧点云和所述第二帧点云的第二位姿。
3.根据权利要求2所述的点云位姿优化方法,其中,所述根据所述第一帧点云和所述第二帧点云之间的相对位姿、以及所述第二帧点云的第一位姿,构建当前点云对的第一位姿残差,包括:
根据所述第一帧点云和所述第二帧点云之间的相对位姿,构建当前点云对的第一位姿残差中的第一残差项;
根据所述第二帧点云的第一位姿,构建当前点云对的第一位姿残差中的第二残差项。
4.根据权利要求1-3中任一项所述的点云位姿优化方法,还包括:
根据所述第二位姿和所述相对位姿,对所述位姿轨迹进行优化,得到所述每帧点云的第三位姿。
5.根据权利要求4所述的点云位姿优化方法,其中,所述根据所述第二位姿和所述相对位姿,对所述位姿轨迹进行优化,得到所述每帧点云的第三位姿,包括:
对每个点云对中的第一帧点云和第二帧点云执行以下操作:
在所述第二帧点云的第二位姿和第一位姿的距离差的绝对值大于预设距离阈值的情况下,根据所述第一帧点云和所述第二帧点云的第二位姿、所述第一帧点云和所述第二帧点云之间的相对位姿、以及所述第二帧点云的第一位姿,构建当前点云对的第二位姿残差;
对所述第二位姿残差进行优化,得到优化后的所述第一帧点云和所述第二帧点云的第三位姿。
6.根据权利要求5所述的点云位姿优化方法,其中,所述第一帧点云和所述第二帧点云的第二位姿、所述第一帧点云和所述第二帧点云之间的相对位姿、以及所述第二帧点云的第一位姿,构建当前点云对的第二位姿残差,包括:
根据所述第一帧点云和所述第二帧点云的第二位姿、以及所述第一帧点云和所述第二帧点云之间的相对位姿,构建当前点云对的第二位姿残差中的第一残差项;
根据所述第二帧点云的第一位姿和第二位姿,构建当前点云对的第二位姿残差中的第二残差项。
7.根据权利要求4所述的点云位姿优化方法,还包括:
确定所述第三位姿形成的位姿轨迹的平滑度是否满足预设的平滑度条件;
在所述平滑度不满足所述平滑度条件的情况下,对所述第三位姿形成的位姿轨迹继续进行至少一次优化,直至优化后的位姿轨迹的平滑度满足所述平滑度条件。
8.一种点云位姿优化装置,包括:
初始位姿获取模块,用于获取目标环境对应的至少一个点云对中每帧点云的第一位姿;所述点云对包括所述目标环境对应的多帧点云中相邻的两帧点云;
相对位姿获取模块,用于获取每个点云对中两帧点云之间的相对位姿;所述相对位姿是里程计模块估计出的;
第一优化模块,用于根据所述第一位姿和所述相对位姿,对所述至少一个点云对的位姿轨迹进行优化,得到所述每帧点云的第二位姿。
9.根据权利要求8所述的点云位姿优化装置,其中,所述第一优化模块具体用于对每个点云对中的第一帧点云和第二帧点云执行以下操作:
根据所述第一帧点云和所述第二帧点云之间的相对位姿、以及所述第二帧点云的第一位姿,构建当前点云对的第一位姿残差;
对所述第一位姿残差进行优化,得到优化后的所述第一帧点云和所述第二帧点云的第二位姿。
10.根据权利要求9所述的点云位姿优化装置,所述第一优化模块具体用于:
根据所述第一帧点云和所述第二帧点云之间的相对位姿,构建当前点云对的第一位姿残差中的第一残差项;
根据所述第二帧点云的第一位姿,构建当前点云对的第一位姿残差中的第二残差项。
11.根据权利要求8-10中任一项所述的点云位姿优化装置,还包括:
第二优化模块,用于根据所述第二位姿和所述相对位姿,对所述位姿轨迹进行优化,得到所述每帧点云的第三位姿。
12.根据权利要求11所述的点云位姿优化装置,其中,所述第二优化模块具体用于对每个点云对中的第一帧点云和第二帧点云执行以下操作:
在所述第二帧点云的第二位姿和第一位姿的距离差的绝对值大于预设距离阈值的情况下,根据所述第一帧点云和所述第二帧点云的第二位姿、所述第一帧点云和所述第二帧点云之间的相对位姿、以及所述第二帧点云的第一位姿,构建当前点云对的第二位姿残差;
对所述第二位姿残差进行优化,得到优化后的所述第一帧点云和所述第二帧点云的第三位姿。
13.根据权利要求12所述的点云位姿优化装置,其中,所述第二优化模块具体用于:
根据所述第一帧点云和所述第二帧点云的第二位姿、以及所述第一帧点云和所述第二帧点云之间的相对位姿,构建当前点云对的第二位姿残差中的第一残差项;
根据所述第二帧点云的第一位姿和第二位姿,构建当前点云对的第二位姿残差中的第二残差项。
14.根据权利要求11所述的点云位姿优化装置,还包括:
第三优化模块,用于确定所述第三位姿形成的位姿轨迹的平滑度是否满足预设的平滑度条件;在所述平滑度不满足所述平滑度条件的情况下,在所述第三位姿形成的位姿轨迹继续进行至少一次优化,直至优化后的位姿轨迹的平滑度满足所述平滑度条件。
15.一种电子设备,包括:
至少一个处理器;以及
与所述至少一个处理器通信连接的存储器;其中,
所述存储器存储有可被所述至少一个处理器执行的指令,所述指令被所述至少一个处理器执行,以使所述至少一个处理器能够执行权利要求1-7中任一项所述的点云位姿优化方法。
16.一种存储有计算机指令的非瞬时计算机可读存储介质,其中,所述计算机指令用于使所述计算机执行根据权利要求1-7中任一项所述的点云位姿优化方法。
17.一种计算机程序产品,包括计算机程序,所述计算机程序在被处理器执行时实现根据权利要求1-7中任一项所述的点云位姿优化方法。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202111505030.4A CN114170300A (zh) | 2021-12-10 | 2021-12-10 | 高精地图点云位姿优化方法、装置、设备及介质 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202111505030.4A CN114170300A (zh) | 2021-12-10 | 2021-12-10 | 高精地图点云位姿优化方法、装置、设备及介质 |
Publications (1)
Publication Number | Publication Date |
---|---|
CN114170300A true CN114170300A (zh) | 2022-03-11 |
Family
ID=80485314
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202111505030.4A Pending CN114170300A (zh) | 2021-12-10 | 2021-12-10 | 高精地图点云位姿优化方法、装置、设备及介质 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN114170300A (zh) |
Cited By (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN115239899A (zh) * | 2022-06-29 | 2022-10-25 | 北京百度网讯科技有限公司 | 位姿图生成方法、高精地图生成方法和装置 |
-
2021
- 2021-12-10 CN CN202111505030.4A patent/CN114170300A/zh active Pending
Cited By (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN115239899A (zh) * | 2022-06-29 | 2022-10-25 | 北京百度网讯科技有限公司 | 位姿图生成方法、高精地图生成方法和装置 |
CN115239899B (zh) * | 2022-06-29 | 2023-09-05 | 北京百度网讯科技有限公司 | 位姿图生成方法、高精地图生成方法和装置 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN112966587B (zh) | 目标检测模型的训练方法、目标检测方法及相关设备 | |
CN115265523B (zh) | 机器人同时定位与建图方法、装置及可读介质 | |
CN113899363B (zh) | 车辆的定位方法、装置及自动驾驶车辆 | |
CN114323033B (zh) | 基于车道线和特征点的定位方法、设备及自动驾驶车辆 | |
CN112763995A (zh) | 雷达标定方法、装置、电子设备和路侧设备 | |
CN113674287A (zh) | 高精地图的绘制方法、装置、设备以及存储介质 | |
CN114140759A (zh) | 高精地图车道线位置确定方法、装置及自动驾驶车辆 | |
CN113688935A (zh) | 高精地图的检测方法、装置、设备以及存储介质 | |
CN114034295A (zh) | 高精地图生成方法、装置、电子设备、介质及程序产品 | |
CN113920217A (zh) | 用于生成高精地图车道线的方法、装置、设备和产品 | |
CN114506343A (zh) | 轨迹规划方法、装置、设备、存储介质及自动驾驶车辆 | |
CN113298910A (zh) | 生成交通标志线地图的方法、设备和存储介质 | |
CN117232499A (zh) | 多传感器融合的点云地图构建方法、装置、设备及介质 | |
CN113706704A (zh) | 基于高精地图规划路线的方法、设备以及自动驾驶车辆 | |
CN114170300A (zh) | 高精地图点云位姿优化方法、装置、设备及介质 | |
CN113932796A (zh) | 高精地图车道线生成方法、装置和电子设备 | |
CN114299242A (zh) | 高精地图中图像处理方法、装置、设备以及存储介质 | |
CN115900697B (zh) | 对象运动轨迹信息处理方法、电子设备以及自动驾驶车辆 | |
CN115937449A (zh) | 高精地图生成方法、装置、电子设备和存储介质 | |
CN115792985A (zh) | 一种车辆定位方法、装置、电子设备、存储介质及车辆 | |
CN113984072B (zh) | 车辆定位方法、装置、设备、存储介质及自动驾驶车辆 | |
CN115790621A (zh) | 高精地图更新方法、装置及电子设备 | |
CN115147561A (zh) | 位姿图生成方法、高精地图生成方法和装置 | |
CN114581869A (zh) | 确定目标对象位置的方法、装置、电子设备和存储介质 | |
CN114842305A (zh) | 深度预测模型训练方法、深度预测方法及相关装置 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination |