Base ceres scan matcher on correspondence cost function (#1085)

- Base ceres scan matcher on correspondence cost function instead of probabilities
 - Step towards [RFC 0019](https://github.com/googlecartographer/rfcs/blob/master/text/0019-probability-grid-and-submap2d-restructuring.md)
master
Kevin Daun 2018-04-19 10:24:16 +02:00 committed by Wally B. Feed
parent 9b3606b7c0
commit f4937b5cc6
4 changed files with 26 additions and 30 deletions

View File

@ -60,8 +60,9 @@ void Grid2D::SetCorrespondenceCost(const Eigen::Array2i& cell_index,
// Returns the probability of the cell with 'cell_index'. // Returns the probability of the cell with 'cell_index'.
float Grid2D::GetCorrespondenceCost(const Eigen::Array2i& cell_index) const { float Grid2D::GetCorrespondenceCost(const Eigen::Array2i& cell_index) const {
LOG(FATAL) << "Not implemented"; if (!limits().Contains(cell_index)) return kMaxCorrespondenceCost;
return kUnknownCorrespondenceValue; return ValueToCorrespondenceCost(
correspondence_cost_cells()[ToFlatIndex(cell_index)]);
} }
// Returns true if the probability at the specified index is known. // Returns true if the probability at the specified index is known.

View File

@ -22,7 +22,7 @@
#include "Eigen/Core" #include "Eigen/Core"
#include "cartographer/common/ceres_solver_options.h" #include "cartographer/common/ceres_solver_options.h"
#include "cartographer/common/lua_parameter_dictionary.h" #include "cartographer/common/lua_parameter_dictionary.h"
#include "cartographer/mapping/2d/probability_grid.h" #include "cartographer/mapping/2d/grid_2d.h"
#include "cartographer/mapping/internal/2d/scan_matching/occupied_space_cost_function_2d.h" #include "cartographer/mapping/internal/2d/scan_matching/occupied_space_cost_function_2d.h"
#include "cartographer/mapping/internal/2d/scan_matching/rotation_delta_cost_functor_2d.h" #include "cartographer/mapping/internal/2d/scan_matching/rotation_delta_cost_functor_2d.h"
#include "cartographer/mapping/internal/2d/scan_matching/translation_delta_cost_functor_2d.h" #include "cartographer/mapping/internal/2d/scan_matching/translation_delta_cost_functor_2d.h"
@ -62,7 +62,7 @@ CeresScanMatcher2D::~CeresScanMatcher2D() {}
void CeresScanMatcher2D::Match(const Eigen::Vector2d& target_translation, void CeresScanMatcher2D::Match(const Eigen::Vector2d& target_translation,
const transform::Rigid2d& initial_pose_estimate, const transform::Rigid2d& initial_pose_estimate,
const sensor::PointCloud& point_cloud, const sensor::PointCloud& point_cloud,
const ProbabilityGrid& probability_grid, const Grid2D& grid,
transform::Rigid2d* const pose_estimate, transform::Rigid2d* const pose_estimate,
ceres::Solver::Summary* const summary) const { ceres::Solver::Summary* const summary) const {
double ceres_pose_estimate[3] = {initial_pose_estimate.translation().x(), double ceres_pose_estimate[3] = {initial_pose_estimate.translation().x(),
@ -74,7 +74,7 @@ void CeresScanMatcher2D::Match(const Eigen::Vector2d& target_translation,
OccupiedSpaceCostFunction2D::CreateAutoDiffCostFunction( OccupiedSpaceCostFunction2D::CreateAutoDiffCostFunction(
options_.occupied_space_weight() / options_.occupied_space_weight() /
std::sqrt(static_cast<double>(point_cloud.size())), std::sqrt(static_cast<double>(point_cloud.size())),
point_cloud, probability_grid), point_cloud, grid),
nullptr /* loss function */, ceres_pose_estimate); nullptr /* loss function */, ceres_pose_estimate);
CHECK_GT(options_.translation_weight(), 0.); CHECK_GT(options_.translation_weight(), 0.);
problem.AddResidualBlock( problem.AddResidualBlock(

View File

@ -22,7 +22,7 @@
#include "Eigen/Core" #include "Eigen/Core"
#include "cartographer/common/lua_parameter_dictionary.h" #include "cartographer/common/lua_parameter_dictionary.h"
#include "cartographer/mapping/2d/probability_grid.h" #include "cartographer/mapping/2d/grid_2d.h"
#include "cartographer/mapping/2d/scan_matching/proto/ceres_scan_matcher_options_2d.pb.h" #include "cartographer/mapping/2d/scan_matching/proto/ceres_scan_matcher_options_2d.pb.h"
#include "cartographer/sensor/point_cloud.h" #include "cartographer/sensor/point_cloud.h"
#include "ceres/ceres.h" #include "ceres/ceres.h"
@ -48,8 +48,7 @@ class CeresScanMatcher2D {
// 'summary'. // 'summary'.
void Match(const Eigen::Vector2d& target_translation, void Match(const Eigen::Vector2d& target_translation,
const transform::Rigid2d& initial_pose_estimate, const transform::Rigid2d& initial_pose_estimate,
const sensor::PointCloud& point_cloud, const sensor::PointCloud& point_cloud, const Grid2D& grid,
const ProbabilityGrid& probability_grid,
transform::Rigid2d* pose_estimate, transform::Rigid2d* pose_estimate,
ceres::Solver::Summary* summary) const; ceres::Solver::Summary* summary) const;

View File

@ -19,7 +19,7 @@
#include "Eigen/Core" #include "Eigen/Core"
#include "Eigen/Geometry" #include "Eigen/Geometry"
#include "cartographer/mapping/2d/probability_grid.h" #include "cartographer/mapping/2d/grid_2d.h"
#include "cartographer/mapping/probability_values.h" #include "cartographer/mapping/probability_values.h"
#include "cartographer/sensor/point_cloud.h" #include "cartographer/sensor/point_cloud.h"
#include "ceres/ceres.h" #include "ceres/ceres.h"
@ -29,19 +29,18 @@ namespace cartographer {
namespace mapping { namespace mapping {
namespace scan_matching { namespace scan_matching {
// Computes a cost for matching the 'point_cloud' to the 'probability_grid' with // Computes a cost for matching the 'point_cloud' to the 'grid' with
// a 'pose'. The cost increases when points fall into less occupied space, i.e. // a 'pose'. The cost increases with poorer correspondence of the grid and the
// at pixels with lower values. // point observation (e.g. points falling into less occupied space).
class OccupiedSpaceCostFunction2D { class OccupiedSpaceCostFunction2D {
public: public:
static ceres::CostFunction* CreateAutoDiffCostFunction( static ceres::CostFunction* CreateAutoDiffCostFunction(
const double scaling_factor, const sensor::PointCloud& point_cloud, const double scaling_factor, const sensor::PointCloud& point_cloud,
const ProbabilityGrid& probability_grid) { const Grid2D& grid) {
return new ceres::AutoDiffCostFunction<OccupiedSpaceCostFunction2D, return new ceres::AutoDiffCostFunction<OccupiedSpaceCostFunction2D,
ceres::DYNAMIC /* residuals */, ceres::DYNAMIC /* residuals */,
3 /* pose variables */>( 3 /* pose variables */>(
new OccupiedSpaceCostFunction2D(scaling_factor, point_cloud, new OccupiedSpaceCostFunction2D(scaling_factor, point_cloud, grid),
probability_grid),
point_cloud.size()); point_cloud.size());
} }
@ -53,9 +52,9 @@ class OccupiedSpaceCostFunction2D {
Eigen::Matrix<T, 3, 3> transform; Eigen::Matrix<T, 3, 3> transform;
transform << rotation_matrix, translation, T(0.), T(0.), T(1.); transform << rotation_matrix, translation, T(0.), T(0.), T(1.);
const GridArrayAdapter adapter(probability_grid_); const GridArrayAdapter adapter(grid_);
ceres::BiCubicInterpolator<GridArrayAdapter> interpolator(adapter); ceres::BiCubicInterpolator<GridArrayAdapter> interpolator(adapter);
const MapLimits& limits = probability_grid_.limits(); const MapLimits& limits = grid_.limits();
for (size_t i = 0; i < point_cloud_.size(); ++i) { for (size_t i = 0; i < point_cloud_.size(); ++i) {
// Note that this is a 2D point. The third component is a scaling factor. // Note that this is a 2D point. The third component is a scaling factor.
@ -68,7 +67,7 @@ class OccupiedSpaceCostFunction2D {
(limits.max().y() - world[1]) / limits.resolution() - 0.5 + (limits.max().y() - world[1]) / limits.resolution() - 0.5 +
static_cast<double>(kPadding), static_cast<double>(kPadding),
&residual[i]); &residual[i]);
residual[i] = scaling_factor_ * (1. - residual[i]); residual[i] = scaling_factor_ * residual[i];
} }
return true; return true;
} }
@ -79,39 +78,36 @@ class OccupiedSpaceCostFunction2D {
public: public:
enum { DATA_DIMENSION = 1 }; enum { DATA_DIMENSION = 1 };
explicit GridArrayAdapter(const ProbabilityGrid& probability_grid) explicit GridArrayAdapter(const Grid2D& grid) : grid_(grid) {}
: probability_grid_(probability_grid) {}
void GetValue(const int row, const int column, double* const value) const { void GetValue(const int row, const int column, double* const value) const {
if (row < kPadding || column < kPadding || row >= NumRows() - kPadding || if (row < kPadding || column < kPadding || row >= NumRows() - kPadding ||
column >= NumCols() - kPadding) { column >= NumCols() - kPadding) {
*value = kMinProbability; *value = kMaxCorrespondenceCost;
} else { } else {
*value = static_cast<double>(probability_grid_.GetProbability( *value = static_cast<double>(grid_.GetCorrespondenceCost(
Eigen::Array2i(column - kPadding, row - kPadding))); Eigen::Array2i(column - kPadding, row - kPadding)));
} }
} }
int NumRows() const { int NumRows() const {
return probability_grid_.limits().cell_limits().num_y_cells + return grid_.limits().cell_limits().num_y_cells + 2 * kPadding;
2 * kPadding;
} }
int NumCols() const { int NumCols() const {
return probability_grid_.limits().cell_limits().num_x_cells + return grid_.limits().cell_limits().num_x_cells + 2 * kPadding;
2 * kPadding;
} }
private: private:
const ProbabilityGrid& probability_grid_; const Grid2D& grid_;
}; };
OccupiedSpaceCostFunction2D(const double scaling_factor, OccupiedSpaceCostFunction2D(const double scaling_factor,
const sensor::PointCloud& point_cloud, const sensor::PointCloud& point_cloud,
const ProbabilityGrid& probability_grid) const Grid2D& grid)
: scaling_factor_(scaling_factor), : scaling_factor_(scaling_factor),
point_cloud_(point_cloud), point_cloud_(point_cloud),
probability_grid_(probability_grid) {} grid_(grid) {}
OccupiedSpaceCostFunction2D(const OccupiedSpaceCostFunction2D&) = delete; OccupiedSpaceCostFunction2D(const OccupiedSpaceCostFunction2D&) = delete;
OccupiedSpaceCostFunction2D& operator=(const OccupiedSpaceCostFunction2D&) = OccupiedSpaceCostFunction2D& operator=(const OccupiedSpaceCostFunction2D&) =
@ -119,7 +115,7 @@ class OccupiedSpaceCostFunction2D {
const double scaling_factor_; const double scaling_factor_;
const sensor::PointCloud& point_cloud_; const sensor::PointCloud& point_cloud_;
const ProbabilityGrid& probability_grid_; const Grid2D& grid_;
}; };
} // namespace scan_matching } // namespace scan_matching