Q:Processing of ceres_scan_matcher_2d.cc

[yusuke-tsuburaya]

I would like to know what is being done in lines 75 to 82 of "ceres_scan_matcher_2d.cc".

[boscosiu]

We have a point_cloud and a grid. What we are trying to compute is the transform that best aligns the points in point_cloud with the occupied cells in grid.

In Cartographer, this is formulated as an optimization problem for Ceres. We provide functions that calculate the cost and Ceres finds the variables that minimize this cost.

Here the cost is the probability of a point being in empty space. So if point_cloud contains many points that are empty in grid, this is considered high cost. We want Ceres to calculate a pose that minimizes this cost, meaning more points matching non-empty (occupied) cells in the grid.

In CreateOccupiedSpaceCostFunction2D():

ceres::CostFunction* CreateOccupiedSpaceCostFunction2D(
    const double scaling_factor, const sensor::PointCloud& point_cloud,
    const Grid2D& grid) {
  return new ceres::AutoDiffCostFunction<OccupiedSpaceCostFunction2D,
                                         ceres::DYNAMIC /* residuals */,
                                         3 /* pose variables */>(
      new OccupiedSpaceCostFunction2D(scaling_factor, point_cloud, grid),
      point_cloud.size());
}

OccupiedSpaceCostFunction2D contains the logic for calculating the cost for Ceres. You can see in OccupiedSpaceCostFunction2D::operator() how we are taking the location of each point, indexing into the grid, and returning the value in the grid cell.