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. 点云数据 
  3. 概率栅格地图   
  4. 估计位姿(引用输出)

返回:   最佳得分

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 输入 :   

  1. 预计位姿(推算位子)   
  2. scanMatch匹配后位姿
  3. 点云数据 
  4. 概率栅格地图
  5. 优化后位姿(输出引用)
  6. ceres 优化器

2.2 步骤:

  1. 构建观测残差     栅格不连续,三次卷积插值 使其连续可导
  2. 构建平移变化量残差
  3. 构建旋转变化量残差
  4. ceres 求解,得pose+cost

3.输入数据的坐标关系

                                       

返回:   最佳得分

预计位姿    点云数据  在重力坐标系下