cartographer 前端 相关性扫描匹配 和 ceres 优化两种
1.相关性扫描匹配:
double RealTimeCorrelativeScanMatcher2D::Match(
const transform::Rigid2d& initial_pose_estimate,
const sensor::PointCloud& point_cloud, const Grid2D& grid,
transform::Rigid2d* pose_estimate)
1.1 输入 :
- 预计位姿(推算位子)
- 点云数据
- 概率栅格地图
- 估计位姿(引用输出)
返回: 最佳得分
1.2 步骤:
// 初始位姿的角度
/将激光转到初始位姿的角度 (x,y)0 点云开始为:baselink坐标系
//确定查询窗口 角度+空间平面
//得到旋转的点云 查询角度范围 ×2 /角度分辨率 点云 对各个角度旋转
//将点云转到当前位姿下 平移
/生成平移的候选值, 只记录 x,y的偏移,未进行 实际点云平移
//求取得分 遍历候选值, 再遍历激光点,遍历激光点时将xy的偏移量传入
//取得分最大的候选值的 偏移量
//输入的初始位子 + 偏移量
2.ceres 优化:
void CeresScanMatcher2D::Match(const Eigen::Vector2d& target_translation,
const transform::Rigid2d& initial_pose_estimate,
const sensor::PointCloud& point_cloud,
const Grid2D& grid,
transform::Rigid2d* const pose_estimate,
ceres::Solver::Summary* const summary)
2.1 输入 :
- 预计位姿(推算位子)
- scanMatch匹配后位姿
- 点云数据
- 概率栅格地图
- 优化后位姿(输出引用)
- ceres 优化器
2.2 步骤:
- 构建观测残差 栅格不连续,三次卷积插值 使其连续可导
- 构建平移变化量残差
- 构建旋转变化量残差
- ceres 求解,得pose+cost
3.输入数据的坐标关系
返回: 最佳得分
预计位姿 点云数据 在重力坐标系下
评论(0)
您还未登录,请登录后发表或查看评论