diff --git a/cartographer/mapping_2d/scan_matching/occupied_space_cost_functor.h b/cartographer/internal/mapping_2d/scan_matching/occupied_space_cost_function.h similarity index 79% rename from cartographer/mapping_2d/scan_matching/occupied_space_cost_functor.h rename to cartographer/internal/mapping_2d/scan_matching/occupied_space_cost_function.h index 295ac5c..ac62dc4 100644 --- a/cartographer/mapping_2d/scan_matching/occupied_space_cost_functor.h +++ b/cartographer/internal/mapping_2d/scan_matching/occupied_space_cost_function.h @@ -14,8 +14,8 @@ * limitations under the License. */ -#ifndef CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_OCCUPIED_SPACE_COST_FUNCTOR_H_ -#define CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_OCCUPIED_SPACE_COST_FUNCTOR_H_ +#ifndef CARTOGRAPHER_INTERNAL_MAPPING_2D_SCAN_MATCHING_OCCUPIED_SPACE_COST_FUNCTION_H_ +#define CARTOGRAPHER_INTERNAL_MAPPING_2D_SCAN_MATCHING_OCCUPIED_SPACE_COST_FUNCTION_H_ #include "Eigen/Core" #include "Eigen/Geometry" @@ -28,22 +28,21 @@ namespace cartographer { namespace mapping_2d { namespace scan_matching { -// Computes the cost of inserting occupied space described by the point cloud -// into the map. The cost increases with the amount of free space that would be -// replaced by occupied space. -class OccupiedSpaceCostFunctor { +// Computes a cost for matching the 'point_cloud' to the 'probability_grid' with +// a 'pose'. The cost increases when points fall into less occupied space, i.e. +// at pixels with lower values. +class OccupiedSpaceCostFunction { public: - // Creates an OccupiedSpaceCostFunctor using the specified map, resolution - // level, and point cloud. - OccupiedSpaceCostFunctor(const double scaling_factor, - const sensor::PointCloud& point_cloud, - const ProbabilityGrid& probability_grid) + OccupiedSpaceCostFunction(const double scaling_factor, + const sensor::PointCloud& point_cloud, + const ProbabilityGrid& probability_grid) : scaling_factor_(scaling_factor), point_cloud_(point_cloud), probability_grid_(probability_grid) {} - OccupiedSpaceCostFunctor(const OccupiedSpaceCostFunctor&) = delete; - OccupiedSpaceCostFunctor& operator=(const OccupiedSpaceCostFunctor&) = delete; + OccupiedSpaceCostFunction(const OccupiedSpaceCostFunction&) = delete; + OccupiedSpaceCostFunction& operator=(const OccupiedSpaceCostFunction&) = + delete; template bool operator()(const T* const pose, T* residual) const { @@ -115,4 +114,4 @@ class OccupiedSpaceCostFunctor { } // namespace mapping_2d } // namespace cartographer -#endif // CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_OCCUPIED_SPACE_COST_FUNCTOR_H_ +#endif // CARTOGRAPHER_INTERNAL_MAPPING_2D_SCAN_MATCHING_OCCUPIED_SPACE_COST_FUNCTION_H_ diff --git a/cartographer/mapping_3d/scan_matching/occupied_space_cost_functor.h b/cartographer/internal/mapping_3d/scan_matching/occupied_space_cost_function.h similarity index 70% rename from cartographer/mapping_3d/scan_matching/occupied_space_cost_functor.h rename to cartographer/internal/mapping_3d/scan_matching/occupied_space_cost_function.h index 2f3435a..0afed38 100644 --- a/cartographer/mapping_3d/scan_matching/occupied_space_cost_functor.h +++ b/cartographer/internal/mapping_3d/scan_matching/occupied_space_cost_function.h @@ -14,8 +14,8 @@ * limitations under the License. */ -#ifndef CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_OCCUPIED_SPACE_COST_FUNCTOR_H_ -#define CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_OCCUPIED_SPACE_COST_FUNCTOR_H_ +#ifndef CARTOGRAPHER_INTERNAL_MAPPING_3D_SCAN_MATCHING_OCCUPIED_SPACE_COST_FUNCTION_H_ +#define CARTOGRAPHER_INTERNAL_MAPPING_3D_SCAN_MATCHING_OCCUPIED_SPACE_COST_FUNCTION_H_ #include "Eigen/Core" #include "cartographer/mapping_3d/hybrid_grid.h" @@ -28,22 +28,21 @@ namespace cartographer { namespace mapping_3d { namespace scan_matching { -// Computes the cost of inserting occupied space described by the point cloud -// into the map. The cost increases with the amount of free space that would be -// replaced by occupied space. -class OccupiedSpaceCostFunctor { +// Computes a cost for matching the 'point_cloud' to the 'hybrid_grid' with a +// 'translation' and 'rotation'. The cost increases when points fall into less +// occupied space, i.e. at voxels with lower values. +class OccupiedSpaceCostFunction { public: - // Creates an OccupiedSpaceCostFunctor using the specified grid, 'rotation' to - // add to all poses, and point cloud. - OccupiedSpaceCostFunctor(const double scaling_factor, - const sensor::PointCloud& point_cloud, - const HybridGrid& hybrid_grid) + OccupiedSpaceCostFunction(const double scaling_factor, + const sensor::PointCloud& point_cloud, + const HybridGrid& hybrid_grid) : scaling_factor_(scaling_factor), point_cloud_(point_cloud), interpolated_grid_(hybrid_grid) {} - OccupiedSpaceCostFunctor(const OccupiedSpaceCostFunctor&) = delete; - OccupiedSpaceCostFunctor& operator=(const OccupiedSpaceCostFunctor&) = delete; + OccupiedSpaceCostFunction(const OccupiedSpaceCostFunction&) = delete; + OccupiedSpaceCostFunction& operator=(const OccupiedSpaceCostFunction&) = + delete; template bool operator()(const T* const translation, const T* const rotation, @@ -78,4 +77,4 @@ class OccupiedSpaceCostFunctor { } // namespace mapping_3d } // namespace cartographer -#endif // CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_OCCUPIED_SPACE_COST_FUNCTOR_H_ +#endif // CARTOGRAPHER_INTERNAL_MAPPING_3D_SCAN_MATCHING_OCCUPIED_SPACE_COST_FUNCTION_H_ diff --git a/cartographer/mapping_2d/scan_matching/ceres_scan_matcher.cc b/cartographer/mapping_2d/scan_matching/ceres_scan_matcher.cc index b079366..b209db7 100644 --- a/cartographer/mapping_2d/scan_matching/ceres_scan_matcher.cc +++ b/cartographer/mapping_2d/scan_matching/ceres_scan_matcher.cc @@ -22,8 +22,8 @@ #include "Eigen/Core" #include "cartographer/common/ceres_solver_options.h" #include "cartographer/common/lua_parameter_dictionary.h" +#include "cartographer/internal/mapping_2d/scan_matching/occupied_space_cost_function.h" #include "cartographer/mapping_2d/probability_grid.h" -#include "cartographer/mapping_2d/scan_matching/occupied_space_cost_functor.h" #include "cartographer/mapping_2d/scan_matching/rotation_delta_cost_functor.h" #include "cartographer/mapping_2d/scan_matching/translation_delta_cost_functor.h" #include "cartographer/transform/transform.h" @@ -71,9 +71,9 @@ void CeresScanMatcher::Match(const transform::Rigid2d& previous_pose, ceres::Problem problem; CHECK_GT(options_.occupied_space_weight(), 0.); problem.AddResidualBlock( - new ceres::AutoDiffCostFunction( - new OccupiedSpaceCostFunctor( + new OccupiedSpaceCostFunction( options_.occupied_space_weight() / std::sqrt(static_cast(point_cloud.size())), point_cloud, probability_grid), diff --git a/cartographer/mapping_3d/scan_matching/ceres_scan_matcher.cc b/cartographer/mapping_3d/scan_matching/ceres_scan_matcher.cc index 362f5c6..5d1bb1f 100644 --- a/cartographer/mapping_3d/scan_matching/ceres_scan_matcher.cc +++ b/cartographer/mapping_3d/scan_matching/ceres_scan_matcher.cc @@ -22,9 +22,9 @@ #include "cartographer/common/ceres_solver_options.h" #include "cartographer/common/make_unique.h" +#include "cartographer/internal/mapping_3d/scan_matching/occupied_space_cost_function.h" #include "cartographer/mapping_3d/ceres_pose.h" #include "cartographer/mapping_3d/rotation_parameterization.h" -#include "cartographer/mapping_3d/scan_matching/occupied_space_cost_functor.h" #include "cartographer/mapping_3d/scan_matching/rotation_delta_cost_functor.h" #include "cartographer/mapping_3d/scan_matching/translation_delta_cost_functor.h" #include "cartographer/transform/rigid_transform.h" @@ -93,9 +93,9 @@ void CeresScanMatcher::Match(const transform::Rigid3d& previous_pose, *point_clouds_and_hybrid_grids[i].first; const HybridGrid& hybrid_grid = *point_clouds_and_hybrid_grids[i].second; problem.AddResidualBlock( - new ceres::AutoDiffCostFunction( - new OccupiedSpaceCostFunctor( + new OccupiedSpaceCostFunction( options_.occupied_space_weight(i) / std::sqrt(static_cast(point_cloud.size())), point_cloud, hybrid_grid),