00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017 #ifndef CARTOGRAPHER_MAPPING_INTERNAL_3D_SCAN_MATCHING_INTERPOLATED_GRID_H_
00018 #define CARTOGRAPHER_MAPPING_INTERNAL_3D_SCAN_MATCHING_INTERPOLATED_GRID_H_
00019
00020 #include <cmath>
00021
00022 #include "cartographer/mapping/3d/hybrid_grid.h"
00023
00024 namespace cartographer {
00025 namespace mapping {
00026 namespace scan_matching {
00027
00028
00029
00030
00031
00032
00033
00034
00035 class InterpolatedGrid {
00036 public:
00037 explicit InterpolatedGrid(const HybridGrid& hybrid_grid)
00038 : hybrid_grid_(hybrid_grid) {}
00039
00040 InterpolatedGrid(const InterpolatedGrid&) = delete;
00041 InterpolatedGrid& operator=(const InterpolatedGrid&) = delete;
00042
00043
00044
00045
00046
00047
00048
00049
00050 template <typename T>
00051 T GetProbability(const T& x, const T& y, const T& z) const {
00052 double x1, y1, z1, x2, y2, z2;
00053 ComputeInterpolationDataPoints(x, y, z, &x1, &y1, &z1, &x2, &y2, &z2);
00054
00055 const Eigen::Array3i index1 =
00056 hybrid_grid_.GetCellIndex(Eigen::Vector3f(x1, y1, z1));
00057 const double q111 = hybrid_grid_.GetProbability(index1);
00058 const double q112 =
00059 hybrid_grid_.GetProbability(index1 + Eigen::Array3i(0, 0, 1));
00060 const double q121 =
00061 hybrid_grid_.GetProbability(index1 + Eigen::Array3i(0, 1, 0));
00062 const double q122 =
00063 hybrid_grid_.GetProbability(index1 + Eigen::Array3i(0, 1, 1));
00064 const double q211 =
00065 hybrid_grid_.GetProbability(index1 + Eigen::Array3i(1, 0, 0));
00066 const double q212 =
00067 hybrid_grid_.GetProbability(index1 + Eigen::Array3i(1, 0, 1));
00068 const double q221 =
00069 hybrid_grid_.GetProbability(index1 + Eigen::Array3i(1, 1, 0));
00070 const double q222 =
00071 hybrid_grid_.GetProbability(index1 + Eigen::Array3i(1, 1, 1));
00072
00073 const T normalized_x = (x - x1) / (x2 - x1);
00074 const T normalized_y = (y - y1) / (y2 - y1);
00075 const T normalized_z = (z - z1) / (z2 - z1);
00076
00077
00078 const T normalized_xx = normalized_x * normalized_x;
00079 const T normalized_xxx = normalized_x * normalized_xx;
00080 const T normalized_yy = normalized_y * normalized_y;
00081 const T normalized_yyy = normalized_y * normalized_yy;
00082 const T normalized_zz = normalized_z * normalized_z;
00083 const T normalized_zzz = normalized_z * normalized_zz;
00084
00085
00086
00087
00088
00089 const T q11 = (q111 - q112) * normalized_zzz * 2. +
00090 (q112 - q111) * normalized_zz * 3. + q111;
00091 const T q12 = (q121 - q122) * normalized_zzz * 2. +
00092 (q122 - q121) * normalized_zz * 3. + q121;
00093 const T q21 = (q211 - q212) * normalized_zzz * 2. +
00094 (q212 - q211) * normalized_zz * 3. + q211;
00095 const T q22 = (q221 - q222) * normalized_zzz * 2. +
00096 (q222 - q221) * normalized_zz * 3. + q221;
00097 const T q1 = (q11 - q12) * normalized_yyy * 2. +
00098 (q12 - q11) * normalized_yy * 3. + q11;
00099 const T q2 = (q21 - q22) * normalized_yyy * 2. +
00100 (q22 - q21) * normalized_yy * 3. + q21;
00101 return (q1 - q2) * normalized_xxx * 2. + (q2 - q1) * normalized_xx * 3. +
00102 q1;
00103 }
00104
00105 private:
00106 template <typename T>
00107 void ComputeInterpolationDataPoints(const T& x, const T& y, const T& z,
00108 double* x1, double* y1, double* z1,
00109 double* x2, double* y2,
00110 double* z2) const {
00111 const Eigen::Vector3f lower = CenterOfLowerVoxel(x, y, z);
00112 *x1 = lower.x();
00113 *y1 = lower.y();
00114 *z1 = lower.z();
00115 *x2 = lower.x() + hybrid_grid_.resolution();
00116 *y2 = lower.y() + hybrid_grid_.resolution();
00117 *z2 = lower.z() + hybrid_grid_.resolution();
00118 }
00119
00120
00121
00122
00123 Eigen::Vector3f CenterOfLowerVoxel(const double x, const double y,
00124 const double z) const {
00125
00126 Eigen::Vector3f center = hybrid_grid_.GetCenterOfCell(
00127 hybrid_grid_.GetCellIndex(Eigen::Vector3f(x, y, z)));
00128
00129 if (center.x() > x) {
00130 center.x() -= hybrid_grid_.resolution();
00131 }
00132 if (center.y() > y) {
00133 center.y() -= hybrid_grid_.resolution();
00134 }
00135 if (center.z() > z) {
00136 center.z() -= hybrid_grid_.resolution();
00137 }
00138 return center;
00139 }
00140
00141
00142 template <typename T>
00143 Eigen::Vector3f CenterOfLowerVoxel(const T& jet_x, const T& jet_y,
00144 const T& jet_z) const {
00145 return CenterOfLowerVoxel(jet_x.a, jet_y.a, jet_z.a);
00146 }
00147
00148 const HybridGrid& hybrid_grid_;
00149 };
00150
00151 }
00152 }
00153 }
00154
00155 #endif // CARTOGRAPHER_MAPPING_INTERNAL_3D_SCAN_MATCHING_INTERPOLATED_GRID_H_