diff --git a/cartographer/mapping/2d/grid_2d.cc b/cartographer/mapping/2d/grid_2d.cc index 1729e5d..e17260f 100644 --- a/cartographer/mapping/2d/grid_2d.cc +++ b/cartographer/mapping/2d/grid_2d.cc @@ -60,8 +60,9 @@ void Grid2D::SetCorrespondenceCost(const Eigen::Array2i& cell_index, // Returns the probability of the cell with 'cell_index'. float Grid2D::GetCorrespondenceCost(const Eigen::Array2i& cell_index) const { - LOG(FATAL) << "Not implemented"; - return kUnknownCorrespondenceValue; + if (!limits().Contains(cell_index)) return kMaxCorrespondenceCost; + return ValueToCorrespondenceCost( + correspondence_cost_cells()[ToFlatIndex(cell_index)]); } // Returns true if the probability at the specified index is known. diff --git a/cartographer/mapping/internal/2d/scan_matching/ceres_scan_matcher_2d.cc b/cartographer/mapping/internal/2d/scan_matching/ceres_scan_matcher_2d.cc index b9fd6e1..0de8168 100644 --- a/cartographer/mapping/internal/2d/scan_matching/ceres_scan_matcher_2d.cc +++ b/cartographer/mapping/internal/2d/scan_matching/ceres_scan_matcher_2d.cc @@ -22,7 +22,7 @@ #include "Eigen/Core" #include "cartographer/common/ceres_solver_options.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/rotation_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, const transform::Rigid2d& initial_pose_estimate, const sensor::PointCloud& point_cloud, - const ProbabilityGrid& probability_grid, + const Grid2D& grid, transform::Rigid2d* const pose_estimate, ceres::Solver::Summary* const summary) const { double ceres_pose_estimate[3] = {initial_pose_estimate.translation().x(), @@ -74,7 +74,7 @@ void CeresScanMatcher2D::Match(const Eigen::Vector2d& target_translation, OccupiedSpaceCostFunction2D::CreateAutoDiffCostFunction( options_.occupied_space_weight() / std::sqrt(static_cast(point_cloud.size())), - point_cloud, probability_grid), + point_cloud, grid), nullptr /* loss function */, ceres_pose_estimate); CHECK_GT(options_.translation_weight(), 0.); problem.AddResidualBlock( diff --git a/cartographer/mapping/internal/2d/scan_matching/ceres_scan_matcher_2d.h b/cartographer/mapping/internal/2d/scan_matching/ceres_scan_matcher_2d.h index a88b855..d3f5e35 100644 --- a/cartographer/mapping/internal/2d/scan_matching/ceres_scan_matcher_2d.h +++ b/cartographer/mapping/internal/2d/scan_matching/ceres_scan_matcher_2d.h @@ -22,7 +22,7 @@ #include "Eigen/Core" #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/sensor/point_cloud.h" #include "ceres/ceres.h" @@ -48,8 +48,7 @@ class CeresScanMatcher2D { // 'summary'. void Match(const Eigen::Vector2d& target_translation, const transform::Rigid2d& initial_pose_estimate, - const sensor::PointCloud& point_cloud, - const ProbabilityGrid& probability_grid, + const sensor::PointCloud& point_cloud, const Grid2D& grid, transform::Rigid2d* pose_estimate, ceres::Solver::Summary* summary) const; diff --git a/cartographer/mapping/internal/2d/scan_matching/occupied_space_cost_function_2d.h b/cartographer/mapping/internal/2d/scan_matching/occupied_space_cost_function_2d.h index a8f91d4..d1ebfbf 100644 --- a/cartographer/mapping/internal/2d/scan_matching/occupied_space_cost_function_2d.h +++ b/cartographer/mapping/internal/2d/scan_matching/occupied_space_cost_function_2d.h @@ -19,7 +19,7 @@ #include "Eigen/Core" #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/sensor/point_cloud.h" #include "ceres/ceres.h" @@ -29,19 +29,18 @@ namespace cartographer { namespace mapping { namespace scan_matching { -// Computes a cost for matching the 'point_cloud' to the 'probability_grid' with -// a 'pose'. The cost increases when points fall into less occupied space, i.e. -// at pixels with lower values. +// Computes a cost for matching the 'point_cloud' to the 'grid' with +// a 'pose'. The cost increases with poorer correspondence of the grid and the +// point observation (e.g. points falling into less occupied space). class OccupiedSpaceCostFunction2D { public: static ceres::CostFunction* CreateAutoDiffCostFunction( const double scaling_factor, const sensor::PointCloud& point_cloud, - const ProbabilityGrid& probability_grid) { + const Grid2D& grid) { return new ceres::AutoDiffCostFunction( - new OccupiedSpaceCostFunction2D(scaling_factor, point_cloud, - probability_grid), + new OccupiedSpaceCostFunction2D(scaling_factor, point_cloud, grid), point_cloud.size()); } @@ -53,9 +52,9 @@ class OccupiedSpaceCostFunction2D { Eigen::Matrix transform; transform << rotation_matrix, translation, T(0.), T(0.), T(1.); - const GridArrayAdapter adapter(probability_grid_); + const GridArrayAdapter adapter(grid_); ceres::BiCubicInterpolator interpolator(adapter); - const MapLimits& limits = probability_grid_.limits(); + const MapLimits& limits = grid_.limits(); for (size_t i = 0; i < point_cloud_.size(); ++i) { // 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 + static_cast(kPadding), &residual[i]); - residual[i] = scaling_factor_ * (1. - residual[i]); + residual[i] = scaling_factor_ * residual[i]; } return true; } @@ -79,39 +78,36 @@ class OccupiedSpaceCostFunction2D { public: enum { DATA_DIMENSION = 1 }; - explicit GridArrayAdapter(const ProbabilityGrid& probability_grid) - : probability_grid_(probability_grid) {} + explicit GridArrayAdapter(const Grid2D& grid) : grid_(grid) {} void GetValue(const int row, const int column, double* const value) const { if (row < kPadding || column < kPadding || row >= NumRows() - kPadding || column >= NumCols() - kPadding) { - *value = kMinProbability; + *value = kMaxCorrespondenceCost; } else { - *value = static_cast(probability_grid_.GetProbability( + *value = static_cast(grid_.GetCorrespondenceCost( Eigen::Array2i(column - kPadding, row - kPadding))); } } int NumRows() const { - return probability_grid_.limits().cell_limits().num_y_cells + - 2 * kPadding; + return grid_.limits().cell_limits().num_y_cells + 2 * kPadding; } int NumCols() const { - return probability_grid_.limits().cell_limits().num_x_cells + - 2 * kPadding; + return grid_.limits().cell_limits().num_x_cells + 2 * kPadding; } private: - const ProbabilityGrid& probability_grid_; + const Grid2D& grid_; }; OccupiedSpaceCostFunction2D(const double scaling_factor, const sensor::PointCloud& point_cloud, - const ProbabilityGrid& probability_grid) + const Grid2D& grid) : scaling_factor_(scaling_factor), point_cloud_(point_cloud), - probability_grid_(probability_grid) {} + grid_(grid) {} OccupiedSpaceCostFunction2D(const OccupiedSpaceCostFunction2D&) = delete; OccupiedSpaceCostFunction2D& operator=(const OccupiedSpaceCostFunction2D&) = @@ -119,7 +115,7 @@ class OccupiedSpaceCostFunction2D { const double scaling_factor_; const sensor::PointCloud& point_cloud_; - const ProbabilityGrid& probability_grid_; + const Grid2D& grid_; }; } // namespace scan_matching