mapping_2d/scan_matching/occupied_space_cost_functor.h
Go to the documentation of this file.
1 /*
2  * Copyright 2016 The Cartographer Authors
3  *
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
9  *
10  * Unless required by applicable law or agreed to in writing, software
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  */
16
17 #ifndef CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_OCCUPIED_SPACE_COST_FUNCTOR_H_
18 #define CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_OCCUPIED_SPACE_COST_FUNCTOR_H_
19
20 #include "Eigen/Core"
21 #include "Eigen/Geometry"
24 #include "ceres/ceres.h"
25 #include "ceres/cubic_interpolation.h"
26
27 namespace cartographer {
28 namespace mapping_2d {
29 namespace scan_matching {
30
31 // Computes the cost of inserting occupied space described by the point cloud
32 // into the map. The cost increases with the amount of free space that would be
33 // replaced by occupied space.
35  public:
36  // Creates an OccupiedSpaceCostFunctor using the specified map, resolution
37  // level, and point cloud.
38  OccupiedSpaceCostFunctor(const double scaling_factor,
39  const sensor::PointCloud& point_cloud,
40  const ProbabilityGrid& probability_grid)
41  : scaling_factor_(scaling_factor),
42  point_cloud_(point_cloud),
43  probability_grid_(probability_grid) {}
44
47
48  template <typename T>
49  bool operator()(const T* const pose, T* residual) const {
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.);
55
58  const MapLimits& limits = probability_grid_.limits();
59
60  for (size_t i = 0; i < point_cloud_.size(); ++i) {
61  // Note that this is a 2D point. The third component is a scaling factor.
62  const Eigen::Matrix<T, 3, 1> point((T(point_cloud_[i].x())),
63  (T(point_cloud_[i].y())), T(1.));
64  const Eigen::Matrix<T, 3, 1> world = transform * point;
65  interpolator.Evaluate(
66  (limits.max().x() - world[0]) / limits.resolution() - 0.5 +
68  (limits.max().y() - world[1]) / limits.resolution() - 0.5 +
70  &residual[i]);
71  residual[i] = scaling_factor_ * (1. - residual[i]);
72  }
73  return true;
74  }
75
76  private:
77  static constexpr int kPadding = INT_MAX / 4;
79  public:
80  enum { DATA_DIMENSION = 1 };
81
83  : probability_grid_(probability_grid) {}
84
85  void GetValue(const int row, const int column, double* const value) const {
87  column >= NumCols() - kPadding) {
88  *value = mapping::kMinProbability;
89  } else {
90  *value = static_cast<double>(probability_grid_.GetProbability(
92  }
93  }
94
95  int NumRows() const {
98  }
99
100  int NumCols() const {
103  }
104
105  private:
107  };
108
109  const double scaling_factor_;
112 };
113
114 } // namespace scan_matching
115 } // namespace mapping_2d
116 } // namespace cartographer
117
118 #endif // CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_OCCUPIED_SPACE_COST_FUNCTOR_H_
const Eigen::Vector2d & max() const
Definition: map_limits.h:61
OccupiedSpaceCostFunctor(const double scaling_factor, const sensor::PointCloud &point_cloud, const ProbabilityGrid &probability_grid)
constexpr float kMinProbability
transform::Rigid3d pose
float GetProbability(const Eigen::Array2i &xy_index) const
const CellLimits & cell_limits() const
Definition: map_limits.h:64
bool operator()(const T *const pose, T *residual) const
std::vector< Eigen::Vector3f > PointCloud
Definition: point_cloud.h:30
void GetValue(const int row, const int column, double *const value) const
float value
OccupiedSpaceCostFunctor & operator=(const OccupiedSpaceCostFunctor &)=delete

cartographer
Author(s):
autogenerated on Mon Jun 10 2019 12:51:39