Cartographer源码阅读2D&3D
文章平均质量分 79
Cartographer源码阅读,包括2D和3D代码
叶落寒蝉
北航QEL机器视觉&人工智能
展开
-
Cartographer源码阅读3D-后端优化-SPA
和2D基本一致,区别:(1)第一个submap只优化yaw角,不优化平移;(2)可选是否优化z轴,如果优化,则加入IMU相关的残差:线加速度和角速度;如果不优化,则和2D类似,加入轮速计残差和LocalSLAM的sequence边。struct NodeSpec3D { common::Time time; transform::Rigid3d local_pose; transform::Rigid3d global_pose;};struct SubmapSpec3D { t原创 2020-11-16 17:56:23 · 735 阅读 · 2 评论 -
Cartographer源码阅读3D-回环计算-分支定界
计算回环-ConstraintBuilder3D和ConstraintBuilder2D基本流程一致,通过2个接口:MaybeAddConstraint和MaybeAddGlobalConstraint,然后调用统一接口:ComputeConstraint函数,计算分支定界,然后利用最高得分的解作为初始解,进行CSM位姿解算。和2D不同的地方主要包括:(1)分支定界的方法不2D不同:多分辨率网格的形式不一致;离散方式不同:先通过直方图匹配的方法计算进行角度离散,再进行距离离散;分支操作:分支时,原创 2020-11-16 17:55:46 · 1331 阅读 · 2 评论 -
Cartographer源码阅读3D-后端PoseGraph3D
和PoseGraph2D的基本流程差不多,主要的差别是:(1)Constraint的形式不一样。(2)分支定界采用了直方图匹配(基于点云切片的思想),且分支定界的复杂度高于2D;(3)后端SPA优化的残差不一样。3D的Constraint:constraint = submap.global_pose * node.local_pose(包含重力对齐)而2D的Constraint:constraint = submap.global_pose_2d * node.local_pose_2d(原创 2020-11-16 17:55:15 · 353 阅读 · 0 评论 -
Cartographer源码阅读3D-Submap创建
Cartographer源码阅读3D-前端-Submap创建submap创建接口函数:std::unique_ptr<LocalTrajectoryBuilder3D::InsertionResult>LocalTrajectoryBuilder3D::InsertIntoSubmap( const common::Time time, const sensor::RangeData& filtered_range_data_in_local, const原创 2020-11-03 09:43:12 · 900 阅读 · 0 评论 -
Cartographer源码阅读3D-前端匹配
Cartographer源码阅读2D&3D-前端匹配暴力匹配:RealTimeCorrelativeScanMatcher3D暴力搜索计算匹配得分,取得最高分对应的位姿。class RealTimeCorrelativeScanMatcher3D { public: explicit RealTimeCorrelativeScanMatcher3D( const scan_matching::proto::RealTimeCorrelativeScanMatcherOptio原创 2020-10-09 19:10:33 · 820 阅读 · 0 评论 -
Cartographer源码阅读3D-前端数据处理-IMU&3D点云
Cartographer源码阅读2D&3D-3D前端数据处理-IMU&3D点云3D需要传入的数据包括:IMU(必须),点云数据(sensor_msgs::MultiEchoLaserScan或sensor_msgs::PointCloud2),轮速计数据(可选,nav_msgs::Odometry,和2D相同,不再分析)IMU数据的处理通过接收sensor_msgs::Imu数据,转换为sensor::ImuData数据:struct ImuData { // 时间 comm原创 2020-10-09 19:05:42 · 1023 阅读 · 0 评论 -
Cartographer源码阅读2D&3D-PGM生成及地图发布
Cartographer源码阅读2D&3D-PGM生成及地图发布pgm生成cartographer提供了pbstream转pgm的service,当然,也可以改写相关函数,直接从cartographer的后端取出数据生成pgm,但是需要修改相关代码。将pbstream转为pgm的节点在pbstream_to_ros_map_main.cc文件里实现。主要函数为:void Run(const std::string& pbstream_filename, const std::strin原创 2020-09-30 16:53:10 · 2170 阅读 · 2 评论 -
Cartographer源码阅读2D&3D-Pbstream读写
Cartographer源码阅读2D&3D-Pbstream读写写入proto查看protobuf官方文档:Protobuf文档需要自定义需要写入和读取的message即可。定义好.proto文件,在编译的阶段生成相应的.pb.h文件,通过调用这些文件,保存和读取proto文件。pbstream文件保存的内容保存pbstream文件是通过服务调用的,接口函数为node.cc文件中:bool Node::HandleWriteState( ::cartographer_ros_m原创 2020-09-30 16:47:10 · 2916 阅读 · 12 评论 -
Cartographer源码阅读2D&3D-删除Submap机制
Cartographer源码阅读-删除Submap机制-PureLocalizationTrimmertrim的删除接口在map_builder.cc文件中有所阐述。共包含两种trim的机制,第一种是OverlappingSubmapsTrimmer2D类型的,只用在2D SLAM中;第二种是PureLocalizationTrimmer,在2D和3D中都可以用。删除机制的入口int MapBuilder::AddTrajectoryBuilder( const std::set<Sen原创 2020-09-30 16:41:07 · 1459 阅读 · 0 评论 -
Cartographer源码阅读2D-后端优化-SPA
Cartographer源码阅读-2D后端优化-SPASPA优化关于SPA,类:// 存放Node的信息struct NodeSpec2D { common::Time time; // 该帧的时间 transform::Rigid2d local_pose_2d;// 不带重力对齐的LocalSLAM下的位姿 transform::Rigid2d global_pose_2d;// 不带重力对齐的GlobalSLAM下的位姿 Eigen::Quaterniond gravity_a原创 2020-09-30 16:27:31 · 721 阅读 · 0 评论 -
Cartographer源码阅读2D-后端回环计算
Cartographer源码阅读-2D后端回环计算ConstraintBuilder2D很重要的是:(1)2D的回环形式和3D的回环形式不一样,2D回环:submap的global pose(global pose * 重力方向)的逆 * node的global_pose_2d(global pose * 重力的逆)。(2)分支定界搜索的网格仍然是一个submap生成的,不是所有的submap。是拿着一个Node的点云数据,在一个submap里做分支定界搜索。class ConstraintBu原创 2020-09-30 16:15:18 · 683 阅读 · 0 评论 -
Cartographer源码阅读2D-后端-PoseGraph2D
Cartographer源码阅读-2D后端-PoseGraph2D后端PoseGraph2D后端管理着轨迹及其相关的Node及Submap的位姿,并对各个位姿进行优化,也是分支定界的接口。struct TrajectoryNodePose { // Node的数据 struct ConstantPoseData { // 时间 common::Time time; // 在Local SLAM坐标系下的位姿 transform::Rigid3d local_po原创 2020-09-30 15:23:08 · 543 阅读 · 0 评论 -
Cartographer源码阅读2D-前端Node稀疏化-MotionFilter
Cartographer源码阅读-2D前端Node稀疏化-MotionFilter在trajectory_builder_2d.cc的LocalTrajectoryBuilder2D::AddAccumulatedRangeData函数中,执行稀疏化任务,即,在AddAccumulatedRangeData函数中调用InsertIntoSubmap函数,在InsertIntoSubmap函数内调用(motion_filter_.IsSimilar(time, pose_estimate)函数,进行稀疏化,原创 2020-09-28 19:31:48 · 346 阅读 · 0 评论 -
Cartographer源码阅读2D-前端Submap生成及数据插入
Cartographer源码阅读-2D前端Submap生成及数据插入前端CSM位姿计算后,将激光点云转换到Local SLAM的坐标系下,插入submaps中,该submaps指的是前端LocalTrajectoryBuilder2D中维护的ActiveSubmap2d。ActiveSubmap2Dclass ActiveSubmaps2D { public: explicit ActiveSubmaps2D(const proto::SubmapsOptions2D& options)原创 2020-09-28 19:25:15 · 1270 阅读 · 0 评论 -
Cartographer源码阅读2D-前端CSM-CeresScanMatcher2D
Cartographer源码阅读-2D前端CSM-CeresScanMatcher2D基本原理:其实,在代码里实现的时候,又加上了旋转和平移的残差,防止点云匹配的结果和初值差太多。CeresScanMatcher2Dclass CeresScanMatcher2D { public: explicit CeresScanMatcher2D(const proto::CeresScanMatcherOptions2D& options); virtual ~CeresScanMat原创 2020-09-21 19:41:12 · 1213 阅读 · 1 评论 -
Cartographer源码阅读2D-前端暴力匹配-RealTimeCorrelativeScanMatcher2D
Cartographer源码阅读-2D前端暴力匹配-RealTimeCorrelativeScanMatcher2D使用暴力匹配的思想,解决当前激光帧和submap匹配的问题:假定在SLAM的过程中,车体的运动是连续有界的,即运动速度是不可能达到无穷大,根据车体的运动速度,可以预测当前帧和上一帧之间的最大相对运动大小是多少,假设我们已经知道上一帧经过CSM解算过的位姿,我们将所有可能的位姿和当前激光帧组合到一起,生成可能解。然后,遍历所有可能解,把所有的可能解投射到submap上:计算当前激光帧通过其位原创 2020-09-21 19:28:45 · 1313 阅读 · 1 评论 -
Cartographer源码阅读2D&3D-前端-位姿外推器-PoseExtrapolator
Cartographer源码阅读-2D前端-位姿外推器-PoseExtrapolatorPoseExtrapolator类:class PoseExtrapolator { public: explicit PoseExtrapolator(common::Duration pose_queue_duration, double imu_gravity_time_constant); PoseExtrapolator(const Pos原创 2020-09-19 00:11:31 · 1039 阅读 · 0 评论 -
Cartographer源码阅读2D&3D-前端自适应滤波-AdaptiveVoxelFilter
Cartographer源码阅读-2D前端自适应滤波-AdaptiveVoxelFilter基本原理:调用体素滤波,如果体素滤波后点数大于阈值,则返回,如果小于阈值,则接着使用二分法进行体素滤波。class AdaptiveVoxelFilter { public: // 构造函数读入max_length min_num_points max_range explicit AdaptiveVoxelFilter( const proto::AdaptiveVoxelFilterO原创 2020-09-18 23:54:44 · 1003 阅读 · 0 评论 -
Cartographer源码阅读2D&3D-前端根据时间戳滤波-RangeDataCollator
Cartographer源码阅读-2D前端-根据时间戳滤波-RangeDataCollator基本原理:根据当前激光帧的时间戳和上一帧激光帧的时间戳,找到当前激光帧和上一帧激光帧时间重叠的激光点,滤除时间重复的点。class RangeDataCollator { public: explicit RangeDataCollator( const std::vector<std::string>& expected_range_sensor_ids)原创 2020-09-18 23:46:53 · 791 阅读 · 0 评论 -
Cartographer源码阅读2D&3D-前端体素滤波-VoxelFilter
Cartographer源码阅读-2D前端体素滤波-VoxelFilter基本原理:将xyz分别除以分辨率,并取整,得到xyz所在cell的id(x,y,z),再将该三个整数分别放入3*32个字节的数中(很好的技巧,避免了大量的遍历和比较),插入voxelFileter::voxel_set_中,后面如果是第一次插入该数,则保留该点,如果之前存在该数,即意味着有其他点落在该网格内,不再保留该点。class VoxelFilter { public: // 'size' is the length原创 2020-09-18 23:39:47 · 1051 阅读 · 4 评论 -
Cartographer源码阅读2D-前端-LocalTrajectoryBuilder2D
Cartographer-2D前端-LocalTrajectoryBuilder2D前端的主要功能:(1)点云数据处理:滤波、畸变校正(2)位姿外推器推算位姿(3)暴力匹配(4)CSM匹配(5)生成Submap(6)生成Node传入后端class LocalTrajectoryBuilder2D { public: struct InsertionResult { // Node数据 std::shared_ptr<const TrajectoryNode::Da原创 2020-09-18 23:31:01 · 801 阅读 · 0 评论 -
Cartographer源码阅读2D&3D-数据流动-数据分发
Cartographer-2D数据流动-数据分发承接上一篇:轨迹添加Node// 接收轮速计数据void Node::HandleOdometryMessage(const int trajectory_id, const std::string& sensor_id, const nav_msgs::Odometry::ConstPtr& msg)原创 2020-09-18 22:26:26 · 746 阅读 · 0 评论 -
Cartographer源码阅读2D&3D-数据流动-轨迹添加
Cartographer-2D数据流动-轨迹添加以单线激光雷达数据和轮速计为例。入口在node_main.cc中:void Run(){ constexpr double kTfBufferCacheTimeInSeconds = 10.; // 构建tf_buffer tf2_ros::Buffer tf_buffer{::ros::Duration(kTfBufferCacheTimeInSeconds)}; tf2_ros::TransformList原创 2020-09-18 21:16:52 · 1319 阅读 · 0 评论