Make the OccupiedSpaceCostFunction internal. (#703)
[RFC=0003](https://github.com/googlecartographer/rfcs/blob/master/text/0003-internal-headers.md)master
Wolfgang Hess
Wally B. Feed
parent
70a0f41364
commit
8e6101de5c
|
|
@ -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 <typename T>
|
||||
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_
|
||||
|
|
@ -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 <typename T>
|
||||
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_
|
||||
|
|
@ -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<OccupiedSpaceCostFunctor, ceres::DYNAMIC,
|
||||
new ceres::AutoDiffCostFunction<OccupiedSpaceCostFunction, ceres::DYNAMIC,
|
||||
3>(
|
||||
new OccupiedSpaceCostFunctor(
|
||||
new OccupiedSpaceCostFunction(
|
||||
options_.occupied_space_weight() /
|
||||
std::sqrt(static_cast<double>(point_cloud.size())),
|
||||
point_cloud, probability_grid),
|
||||
|
|
|
|||
|
|
@ -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<OccupiedSpaceCostFunctor,
|
||||
new ceres::AutoDiffCostFunction<OccupiedSpaceCostFunction,
|
||||
ceres::DYNAMIC, 3, 4>(
|
||||
new OccupiedSpaceCostFunctor(
|
||||
new OccupiedSpaceCostFunction(
|
||||
options_.occupied_space_weight(i) /
|
||||
std::sqrt(static_cast<double>(point_cloud.size())),
|
||||
point_cloud, hybrid_grid),
|
||||
|
|
|
|||
Loading…
Reference in New Issue