interpolated_grid.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_3D_SCAN_MATCHING_INTERPOLATED_GRID_H_
18 #define CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_INTERPOLATED_GRID_H_
19
20 #include <cmath>
21
23
24 namespace cartographer {
25 namespace mapping_3d {
26 namespace scan_matching {
27
28 // Interpolates between HybridGrid probability voxels. We use the tricubic
29 // interpolation which interpolates the values and has vanishing derivative at
30 // these points.
31 //
32 // This class is templated to work with the autodiff that Ceres provides.
33 // For this reason, it is also important that the interpolation scheme be
34 // continuously differentiable.
36  public:
37  explicit InterpolatedGrid(const HybridGrid& hybrid_grid)
38  : hybrid_grid_(hybrid_grid) {}
39
40  InterpolatedGrid(const InterpolatedGrid&) = delete;
42
43  // Returns the interpolated probability at (x, y, z) of the HybridGrid
44  // used to perform the interpolation.
45  //
46  // This is a piecewise, continuously differentiable function. We use the
47  // scalar part of Jet parameters to select our interval below. It is the
48  // tensor product volume of piecewise cubic polynomials that interpolate
49  // the values, and have vanishing derivative at the interval boundaries.
50  template <typename T>
51  T GetProbability(const T& x, const T& y, const T& z) const {
52  double x1, y1, z1, x2, y2, z2;
53  ComputeInterpolationDataPoints(x, y, z, &x1, &y1, &z1, &x2, &y2, &z2);
54
55  const Eigen::Array3i index1 =
56  hybrid_grid_.GetCellIndex(Eigen::Vector3f(x1, y1, z1));
57  const double q111 = hybrid_grid_.GetProbability(index1);
58  const double q112 =
59  hybrid_grid_.GetProbability(index1 + Eigen::Array3i(0, 0, 1));
60  const double q121 =
61  hybrid_grid_.GetProbability(index1 + Eigen::Array3i(0, 1, 0));
62  const double q122 =
63  hybrid_grid_.GetProbability(index1 + Eigen::Array3i(0, 1, 1));
64  const double q211 =
65  hybrid_grid_.GetProbability(index1 + Eigen::Array3i(1, 0, 0));
66  const double q212 =
67  hybrid_grid_.GetProbability(index1 + Eigen::Array3i(1, 0, 1));
68  const double q221 =
69  hybrid_grid_.GetProbability(index1 + Eigen::Array3i(1, 1, 0));
70  const double q222 =
71  hybrid_grid_.GetProbability(index1 + Eigen::Array3i(1, 1, 1));
72
73  const T normalized_x = (x - x1) / (x2 - x1);
74  const T normalized_y = (y - y1) / (y2 - y1);
75  const T normalized_z = (z - z1) / (z2 - z1);
76
77  // Compute pow(..., 2) and pow(..., 3). Using pow() here is very expensive.
78  const T normalized_xx = normalized_x * normalized_x;
79  const T normalized_xxx = normalized_x * normalized_xx;
80  const T normalized_yy = normalized_y * normalized_y;
81  const T normalized_yyy = normalized_y * normalized_yy;
82  const T normalized_zz = normalized_z * normalized_z;
83  const T normalized_zzz = normalized_z * normalized_zz;
84
85  // We first interpolate in z, then y, then x. All 7 times this uses the same
86  // scheme: A * (2t^3 - 3t^2 + 1) + B * (-2t^3 + 3t^2).
87  // The first polynomial is 1 at t=0, 0 at t=1, the second polynomial is 0
88  // at t=0, 1 at t=1. Both polynomials have derivative zero at t=0 and t=1.
89  const T q11 = (q111 - q112) * normalized_zzz * 2. +
90  (q112 - q111) * normalized_zz * 3. + q111;
91  const T q12 = (q121 - q122) * normalized_zzz * 2. +
92  (q122 - q121) * normalized_zz * 3. + q121;
93  const T q21 = (q211 - q212) * normalized_zzz * 2. +
94  (q212 - q211) * normalized_zz * 3. + q211;
95  const T q22 = (q221 - q222) * normalized_zzz * 2. +
96  (q222 - q221) * normalized_zz * 3. + q221;
97  const T q1 = (q11 - q12) * normalized_yyy * 2. +
98  (q12 - q11) * normalized_yy * 3. + q11;
99  const T q2 = (q21 - q22) * normalized_yyy * 2. +
100  (q22 - q21) * normalized_yy * 3. + q21;
101  return (q1 - q2) * normalized_xxx * 2. + (q2 - q1) * normalized_xx * 3. +
102  q1;
103  }
104
105  private:
106  template <typename T>
107  void ComputeInterpolationDataPoints(const T& x, const T& y, const T& z,
108  double* x1, double* y1, double* z1,
109  double* x2, double* y2,
110  double* z2) const {
111  const Eigen::Vector3f lower = CenterOfLowerVoxel(x, y, z);
112  *x1 = lower.x();
113  *y1 = lower.y();
114  *z1 = lower.z();
115  *x2 = lower.x() + hybrid_grid_.resolution();
116  *y2 = lower.y() + hybrid_grid_.resolution();
117  *z2 = lower.z() + hybrid_grid_.resolution();
118  }
119
120  // Center of the next lower voxel, i.e., not necessarily the voxel containing
121  // (x, y, z). For each dimension, the largest voxel index so that the
122  // corresponding center is at most the given coordinate.
123  Eigen::Vector3f CenterOfLowerVoxel(const double x, const double y,
124  const double z) const {
125  // Center of the cell containing (x, y, z).
126  Eigen::Vector3f center = hybrid_grid_.GetCenterOfCell(
127  hybrid_grid_.GetCellIndex(Eigen::Vector3f(x, y, z)));
128  // Move to the next lower voxel center.
129  if (center.x() > x) {
130  center.x() -= hybrid_grid_.resolution();
131  }
132  if (center.y() > y) {
133  center.y() -= hybrid_grid_.resolution();
134  }
135  if (center.z() > z) {
136  center.z() -= hybrid_grid_.resolution();
137  }
138  return center;
139  }
140
141  // Uses the scalar part of a Ceres Jet.
142  template <typename T>
143  Eigen::Vector3f CenterOfLowerVoxel(const T& jet_x, const T& jet_y,
144  const T& jet_z) const {
145  return CenterOfLowerVoxel(jet_x.a, jet_y.a, jet_z.a);
146  }
147
149 };
150
151 } // namespace scan_matching
152 } // namespace mapping_3d
153 } // namespace cartographer
154
155 #endif // CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_INTERPOLATED_GRID_H_
Eigen::Vector3f GetCenterOfCell(const Eigen::Array3i &index) const
Definition: hybrid_grid.h:444
void ComputeInterpolationDataPoints(const T &x, const T &y, const T &z, double *x1, double *y1, double *z1, double *x2, double *y2, double *z2) const
Eigen::Array3i GetCellIndex(const Eigen::Vector3f &point) const
Definition: hybrid_grid.h:428
float GetProbability(const Eigen::Array3i &index) const
Definition: hybrid_grid.h:518
Eigen::Vector3f CenterOfLowerVoxel(const T &jet_x, const T &jet_y, const T &jet_z) const
Eigen::Vector3f CenterOfLowerVoxel(const double x, const double y, const double z) const
T GetProbability(const T &x, const T &y, const T &z) const
InterpolatedGrid & operator=(const InterpolatedGrid &)=delete

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