17 #ifndef CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_OCCUPIED_SPACE_COST_FUNCTOR_H_ 18 #define CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_OCCUPIED_SPACE_COST_FUNCTOR_H_ 21 #include "Eigen/Geometry" 24 #include "ceres/ceres.h" 25 #include "ceres/cubic_interpolation.h" 28 namespace mapping_2d {
29 namespace scan_matching {
50 Eigen::Matrix<T, 2, 1> translation(pose[0], pose[1]);
51 Eigen::Rotation2D<T> rotation(pose[2]);
52 Eigen::Matrix<T, 2, 2> rotation_matrix = rotation.toRotationMatrix();
53 Eigen::Matrix<T, 3, 3> transform;
54 transform << rotation_matrix, translation, T(0.), T(0.), T(1.);
57 ceres::BiCubicInterpolator<GridArrayAdapter> interpolator(adapter);
62 const Eigen::Matrix<T, 3, 1> point((T(
point_cloud_[i].x())),
64 const Eigen::Matrix<T, 3, 1> world = transform * point;
65 interpolator.Evaluate(
85 void GetValue(
const int row,
const int column,
double*
const value)
const {
86 if (row < kPadding || column < kPadding || row >=
NumRows() - kPadding ||
87 column >=
NumCols() - kPadding) {
91 Eigen::Array2i(column - kPadding, row - kPadding)));
118 #endif // CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_OCCUPIED_SPACE_COST_FUNCTOR_H_ const ProbabilityGrid & probability_grid_
const Eigen::Vector2d & max() const
OccupiedSpaceCostFunctor(const double scaling_factor, const sensor::PointCloud &point_cloud, const ProbabilityGrid &probability_grid)
constexpr float kMinProbability
static constexpr int kPadding
double resolution() const
const ProbabilityGrid & probability_grid_
float GetProbability(const Eigen::Array2i &xy_index) const
const CellLimits & cell_limits() const
bool operator()(const T *const pose, T *residual) const
const double scaling_factor_
std::vector< Eigen::Vector3f > PointCloud
void GetValue(const int row, const int column, double *const value) const
const MapLimits & limits() const
GridArrayAdapter(const ProbabilityGrid &probability_grid)
const sensor::PointCloud & point_cloud_
OccupiedSpaceCostFunctor & operator=(const OccupiedSpaceCostFunctor &)=delete