12345qiupeng
/
cartographer
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Make the OccupiedSpaceCostFunction internal. (#703) 

[RFC=0003](https://github.com/googlecartographer/rfcs/blob/master/text/0003-internal-headers.md)
master
Wolfgang Hess 2017-11-23 18:04:36 +01:00 committed by Wally B. Feed
parent 70a0f41364
commit 8e6101de5c
4 changed files with 32 additions and 34 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
cartographer/mapping_2d/scan_matching/occupied_space_cost_functor.h → cartographer/internal/mapping_2d/scan_matching/occupied_space_cost_function.h
View File

@ -14,8 +14,8 @@
* limitations under the License. * limitations under the License.
*/ */
#ifndef CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_OCCUPIED_SPACE_COST_FUNCTOR_H_ #ifndef CARTOGRAPHER_INTERNAL_MAPPING_2D_SCAN_MATCHING_OCCUPIED_SPACE_COST_FUNCTION_H_
#define CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_OCCUPIED_SPACE_COST_FUNCTOR_H_ #define CARTOGRAPHER_INTERNAL_MAPPING_2D_SCAN_MATCHING_OCCUPIED_SPACE_COST_FUNCTION_H_
#include "Eigen/Core" #include "Eigen/Core"
#include "Eigen/Geometry" #include "Eigen/Geometry"
@ -28,22 +28,21 @@ namespace cartographer {
namespace mapping_2d { namespace mapping_2d {
namespace scan_matching { namespace scan_matching {
// Computes the cost of inserting occupied space described by the point cloud // Computes a cost for matching the 'point_cloud' to the 'probability_grid' with
// into the map. The cost increases with the amount of free space that would be // a 'pose'. The cost increases when points fall into less occupied space, i.e.
// replaced by occupied space. // at pixels with lower values.
class OccupiedSpaceCostFunctor { class OccupiedSpaceCostFunction {
public: public:
// Creates an OccupiedSpaceCostFunctor using the specified map, resolution OccupiedSpaceCostFunction(const double scaling_factor,
// level, and point cloud. const sensor::PointCloud& point_cloud,
OccupiedSpaceCostFunctor(const double scaling_factor, const ProbabilityGrid& probability_grid)
const sensor::PointCloud& point_cloud,
const ProbabilityGrid& probability_grid)
: scaling_factor_(scaling_factor), : scaling_factor_(scaling_factor),
point_cloud_(point_cloud), point_cloud_(point_cloud),
probability_grid_(probability_grid) {} probability_grid_(probability_grid) {}
OccupiedSpaceCostFunctor(const OccupiedSpaceCostFunctor&) = delete; OccupiedSpaceCostFunction(const OccupiedSpaceCostFunction&) = delete;
OccupiedSpaceCostFunctor& operator=(const OccupiedSpaceCostFunctor&) = delete; OccupiedSpaceCostFunction& operator=(const OccupiedSpaceCostFunction&) =
delete;
template <typename T> template <typename T>
bool operator()(const T* const pose, T* residual) const { bool operator()(const T* const pose, T* residual) const {
@ -115,4 +114,4 @@ class OccupiedSpaceCostFunctor {
} // namespace mapping_2d } // namespace mapping_2d
} // namespace cartographer } // 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_

27
cartographer/mapping_3d/scan_matching/occupied_space_cost_functor.h → cartographer/internal/mapping_3d/scan_matching/occupied_space_cost_function.h
View File

@ -14,8 +14,8 @@
* limitations under the License. * limitations under the License.
*/ */
#ifndef CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_OCCUPIED_SPACE_COST_FUNCTOR_H_ #ifndef CARTOGRAPHER_INTERNAL_MAPPING_3D_SCAN_MATCHING_OCCUPIED_SPACE_COST_FUNCTION_H_
#define CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_OCCUPIED_SPACE_COST_FUNCTOR_H_ #define CARTOGRAPHER_INTERNAL_MAPPING_3D_SCAN_MATCHING_OCCUPIED_SPACE_COST_FUNCTION_H_
#include "Eigen/Core" #include "Eigen/Core"
#include "cartographer/mapping_3d/hybrid_grid.h" #include "cartographer/mapping_3d/hybrid_grid.h"
@ -28,22 +28,21 @@ namespace cartographer {
namespace mapping_3d { namespace mapping_3d {
namespace scan_matching { namespace scan_matching {
// Computes the cost of inserting occupied space described by the point cloud // Computes a cost for matching the 'point_cloud' to the 'hybrid_grid' with a
// into the map. The cost increases with the amount of free space that would be // 'translation' and 'rotation'. The cost increases when points fall into less
// replaced by occupied space. // occupied space, i.e. at voxels with lower values.
class OccupiedSpaceCostFunctor { class OccupiedSpaceCostFunction {
public: public:
// Creates an OccupiedSpaceCostFunctor using the specified grid, 'rotation' to OccupiedSpaceCostFunction(const double scaling_factor,
// add to all poses, and point cloud. const sensor::PointCloud& point_cloud,
OccupiedSpaceCostFunctor(const double scaling_factor, const HybridGrid& hybrid_grid)
const sensor::PointCloud& point_cloud,
const HybridGrid& hybrid_grid)
: scaling_factor_(scaling_factor), : scaling_factor_(scaling_factor),
point_cloud_(point_cloud), point_cloud_(point_cloud),
interpolated_grid_(hybrid_grid) {} interpolated_grid_(hybrid_grid) {}
OccupiedSpaceCostFunctor(const OccupiedSpaceCostFunctor&) = delete; OccupiedSpaceCostFunction(const OccupiedSpaceCostFunction&) = delete;
OccupiedSpaceCostFunctor& operator=(const OccupiedSpaceCostFunctor&) = delete; OccupiedSpaceCostFunction& operator=(const OccupiedSpaceCostFunction&) =
delete;
template <typename T> template <typename T>
bool operator()(const T* const translation, const T* const rotation, bool operator()(const T* const translation, const T* const rotation,
@ -78,4 +77,4 @@ class OccupiedSpaceCostFunctor {
} // namespace mapping_3d } // namespace mapping_3d
} // namespace cartographer } // 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_

6
cartographer/mapping_2d/scan_matching/ceres_scan_matcher.cc
View File

@ -22,8 +22,8 @@
#include "Eigen/Core" #include "Eigen/Core"
#include "cartographer/common/ceres_solver_options.h" #include "cartographer/common/ceres_solver_options.h"
#include "cartographer/common/lua_parameter_dictionary.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/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/rotation_delta_cost_functor.h"
#include "cartographer/mapping_2d/scan_matching/translation_delta_cost_functor.h" #include "cartographer/mapping_2d/scan_matching/translation_delta_cost_functor.h"
#include "cartographer/transform/transform.h" #include "cartographer/transform/transform.h"
@ -71,9 +71,9 @@ void CeresScanMatcher::Match(const transform::Rigid2d& previous_pose,
ceres::Problem problem; ceres::Problem problem;
CHECK_GT(options_.occupied_space_weight(), 0.); CHECK_GT(options_.occupied_space_weight(), 0.);
problem.AddResidualBlock( problem.AddResidualBlock(
new ceres::AutoDiffCostFunction<OccupiedSpaceCostFunctor, ceres::DYNAMIC, new ceres::AutoDiffCostFunction<OccupiedSpaceCostFunction, ceres::DYNAMIC,
3>( 3>(
new OccupiedSpaceCostFunctor( new OccupiedSpaceCostFunction(
options_.occupied_space_weight() / options_.occupied_space_weight() /
std::sqrt(static_cast<double>(point_cloud.size())), std::sqrt(static_cast<double>(point_cloud.size())),
point_cloud, probability_grid), point_cloud, probability_grid),

6
cartographer/mapping_3d/scan_matching/ceres_scan_matcher.cc
View File

@ -22,9 +22,9 @@
#include "cartographer/common/ceres_solver_options.h" #include "cartographer/common/ceres_solver_options.h"
#include "cartographer/common/make_unique.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/ceres_pose.h"
#include "cartographer/mapping_3d/rotation_parameterization.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/rotation_delta_cost_functor.h"
#include "cartographer/mapping_3d/scan_matching/translation_delta_cost_functor.h" #include "cartographer/mapping_3d/scan_matching/translation_delta_cost_functor.h"
#include "cartographer/transform/rigid_transform.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; *point_clouds_and_hybrid_grids[i].first;
const HybridGrid& hybrid_grid = *point_clouds_and_hybrid_grids[i].second; const HybridGrid& hybrid_grid = *point_clouds_and_hybrid_grids[i].second;
problem.AddResidualBlock( problem.AddResidualBlock(
new ceres::AutoDiffCostFunction<OccupiedSpaceCostFunctor, new ceres::AutoDiffCostFunction<OccupiedSpaceCostFunction,
ceres::DYNAMIC, 3, 4>( ceres::DYNAMIC, 3, 4>(
new OccupiedSpaceCostFunctor( new OccupiedSpaceCostFunction(
options_.occupied_space_weight(i) / options_.occupied_space_weight(i) /
std::sqrt(static_cast<double>(point_cloud.size())), std::sqrt(static_cast<double>(point_cloud.size())),
point_cloud, hybrid_grid), point_cloud, hybrid_grid),