Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017 #ifndef CARTOGRAPHER_MAPPING_INTERNAL_3D_SCAN_MATCHING_OCCUPIED_SPACE_COST_FUNCTION_3D_H_
00018 #define CARTOGRAPHER_MAPPING_INTERNAL_3D_SCAN_MATCHING_OCCUPIED_SPACE_COST_FUNCTION_3D_H_
00019
00020 #include "Eigen/Core"
00021 #include "cartographer/mapping/3d/hybrid_grid.h"
00022 #include "cartographer/mapping/internal/3d/scan_matching/interpolated_grid.h"
00023 #include "cartographer/sensor/point_cloud.h"
00024 #include "cartographer/transform/rigid_transform.h"
00025 #include "cartographer/transform/transform.h"
00026
00027 namespace cartographer {
00028 namespace mapping {
00029 namespace scan_matching {
00030
00031
00032
00033
00034 class OccupiedSpaceCostFunction3D {
00035 public:
00036 static ceres::CostFunction* CreateAutoDiffCostFunction(
00037 const double scaling_factor, const sensor::PointCloud& point_cloud,
00038 const mapping::HybridGrid& hybrid_grid) {
00039 return new ceres::AutoDiffCostFunction<
00040 OccupiedSpaceCostFunction3D, ceres::DYNAMIC ,
00041 3 , 4 >(
00042 new OccupiedSpaceCostFunction3D(scaling_factor, point_cloud,
00043 hybrid_grid),
00044 point_cloud.size());
00045 }
00046
00047 template <typename T>
00048 bool operator()(const T* const translation, const T* const rotation,
00049 T* const residual) const {
00050 const transform::Rigid3<T> transform(
00051 Eigen::Map<const Eigen::Matrix<T, 3, 1>>(translation),
00052 Eigen::Quaternion<T>(rotation[0], rotation[1], rotation[2],
00053 rotation[3]));
00054 return Evaluate(transform, residual);
00055 }
00056
00057 private:
00058 OccupiedSpaceCostFunction3D(const double scaling_factor,
00059 const sensor::PointCloud& point_cloud,
00060 const mapping::HybridGrid& hybrid_grid)
00061 : scaling_factor_(scaling_factor),
00062 point_cloud_(point_cloud),
00063 interpolated_grid_(hybrid_grid) {}
00064
00065 OccupiedSpaceCostFunction3D(const OccupiedSpaceCostFunction3D&) = delete;
00066 OccupiedSpaceCostFunction3D& operator=(const OccupiedSpaceCostFunction3D&) =
00067 delete;
00068
00069 template <typename T>
00070 bool Evaluate(const transform::Rigid3<T>& transform,
00071 T* const residual) const {
00072 for (size_t i = 0; i < point_cloud_.size(); ++i) {
00073 const Eigen::Matrix<T, 3, 1> world =
00074 transform * point_cloud_[i].position.cast<T>();
00075 const T probability =
00076 interpolated_grid_.GetProbability(world[0], world[1], world[2]);
00077 residual[i] = scaling_factor_ * (1. - probability);
00078 }
00079 return true;
00080 }
00081
00082 const double scaling_factor_;
00083 const sensor::PointCloud& point_cloud_;
00084 const InterpolatedGrid interpolated_grid_;
00085 };
00086
00087 }
00088 }
00089 }
00090
00091 #endif // CARTOGRAPHER_MAPPING_INTERNAL_3D_SCAN_MATCHING_OCCUPIED_SPACE_COST_FUNCTION_3D_H_