From ecaa95f3b0916dd2945b9ff1e2b4d3ae86504df1 Mon Sep 17 00:00:00 2001 From: Kevin Daun Date: Tue, 19 Jun 2018 12:31:11 +0200 Subject: [PATCH] Move occupied space cost function to .cc (#1200) --- .../2d/scan_matching/ceres_scan_matcher_2d.cc | 2 +- .../occupied_space_cost_function_2d.cc | 119 ++++++++++++++++++ .../occupied_space_cost_function_2d.h | 94 +------------- .../occupied_space_cost_function_2d_test.cc | 57 +++++++++ 4 files changed, 181 insertions(+), 91 deletions(-) create mode 100644 cartographer/mapping/internal/2d/scan_matching/occupied_space_cost_function_2d.cc create mode 100644 cartographer/mapping/internal/2d/scan_matching/occupied_space_cost_function_2d_test.cc diff --git a/cartographer/mapping/internal/2d/scan_matching/ceres_scan_matcher_2d.cc b/cartographer/mapping/internal/2d/scan_matching/ceres_scan_matcher_2d.cc index 0de8168..4809bac 100644 --- a/cartographer/mapping/internal/2d/scan_matching/ceres_scan_matcher_2d.cc +++ b/cartographer/mapping/internal/2d/scan_matching/ceres_scan_matcher_2d.cc @@ -71,7 +71,7 @@ void CeresScanMatcher2D::Match(const Eigen::Vector2d& target_translation, ceres::Problem problem; CHECK_GT(options_.occupied_space_weight(), 0.); problem.AddResidualBlock( - OccupiedSpaceCostFunction2D::CreateAutoDiffCostFunction( + CreateOccupiedSpaceCostFunction2D( options_.occupied_space_weight() / std::sqrt(static_cast(point_cloud.size())), point_cloud, grid), diff --git a/cartographer/mapping/internal/2d/scan_matching/occupied_space_cost_function_2d.cc b/cartographer/mapping/internal/2d/scan_matching/occupied_space_cost_function_2d.cc new file mode 100644 index 0000000..f3d7122 --- /dev/null +++ b/cartographer/mapping/internal/2d/scan_matching/occupied_space_cost_function_2d.cc @@ -0,0 +1,119 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/mapping/internal/2d/scan_matching/occupied_space_cost_function_2d.h" +#include "cartographer/mapping/probability_values.h" +#include "ceres/cubic_interpolation.h" + +namespace cartographer { +namespace mapping { +namespace scan_matching { +namespace { + +// Computes a cost for matching the 'point_cloud' to the 'grid' with +// a 'pose'. The cost increases with poorer correspondence of the grid and the +// point observation (e.g. points falling into less occupied space). +class OccupiedSpaceCostFunction2D { + public: + OccupiedSpaceCostFunction2D(const double scaling_factor, + const sensor::PointCloud& point_cloud, + const Grid2D& grid) + : scaling_factor_(scaling_factor), + point_cloud_(point_cloud), + grid_(grid) {} + + template + bool operator()(const T* const pose, T* residual) const { + Eigen::Matrix translation(pose[0], pose[1]); + Eigen::Rotation2D rotation(pose[2]); + Eigen::Matrix rotation_matrix = rotation.toRotationMatrix(); + Eigen::Matrix transform; + transform << rotation_matrix, translation, T(0.), T(0.), T(1.); + + const GridArrayAdapter adapter(grid_); + ceres::BiCubicInterpolator interpolator(adapter); + const MapLimits& limits = grid_.limits(); + + for (size_t i = 0; i < point_cloud_.size(); ++i) { + // Note that this is a 2D point. The third component is a scaling factor. + const Eigen::Matrix point((T(point_cloud_[i].x())), + (T(point_cloud_[i].y())), T(1.)); + const Eigen::Matrix world = transform * point; + interpolator.Evaluate( + (limits.max().x() - world[0]) / limits.resolution() - 0.5 + + static_cast(kPadding), + (limits.max().y() - world[1]) / limits.resolution() - 0.5 + + static_cast(kPadding), + &residual[i]); + residual[i] = scaling_factor_ * residual[i]; + } + return true; + } + + private: + static constexpr int kPadding = INT_MAX / 4; + class GridArrayAdapter { + public: + enum { DATA_DIMENSION = 1 }; + + explicit GridArrayAdapter(const Grid2D& grid) : grid_(grid) {} + + void GetValue(const int row, const int column, double* const value) const { + if (row < kPadding || column < kPadding || row >= NumRows() - kPadding || + column >= NumCols() - kPadding) { + *value = kMaxCorrespondenceCost; + } else { + *value = static_cast(grid_.GetCorrespondenceCost( + Eigen::Array2i(column - kPadding, row - kPadding))); + } + } + + int NumRows() const { + return grid_.limits().cell_limits().num_y_cells + 2 * kPadding; + } + + int NumCols() const { + return grid_.limits().cell_limits().num_x_cells + 2 * kPadding; + } + + private: + const Grid2D& grid_; + }; + + OccupiedSpaceCostFunction2D(const OccupiedSpaceCostFunction2D&) = delete; + OccupiedSpaceCostFunction2D& operator=(const OccupiedSpaceCostFunction2D&) = + delete; + + const double scaling_factor_; + const sensor::PointCloud& point_cloud_; + const Grid2D& grid_; +}; + +} // namespace + +ceres::CostFunction* CreateOccupiedSpaceCostFunction2D( + const double scaling_factor, const sensor::PointCloud& point_cloud, + const Grid2D& grid) { + return new ceres::AutoDiffCostFunction( + new OccupiedSpaceCostFunction2D(scaling_factor, point_cloud, grid), + point_cloud.size()); +} + +} // namespace scan_matching +} // namespace mapping +} // namespace cartographer diff --git a/cartographer/mapping/internal/2d/scan_matching/occupied_space_cost_function_2d.h b/cartographer/mapping/internal/2d/scan_matching/occupied_space_cost_function_2d.h index d1ebfbf..7e0c7f7 100644 --- a/cartographer/mapping/internal/2d/scan_matching/occupied_space_cost_function_2d.h +++ b/cartographer/mapping/internal/2d/scan_matching/occupied_space_cost_function_2d.h @@ -17,106 +17,20 @@ #ifndef CARTOGRAPHER_MAPPING_INTERNAL_2D_SCAN_MATCHING_OCCUPIED_SPACE_COST_FUNCTION_2D_H_ #define CARTOGRAPHER_MAPPING_INTERNAL_2D_SCAN_MATCHING_OCCUPIED_SPACE_COST_FUNCTION_2D_H_ -#include "Eigen/Core" -#include "Eigen/Geometry" #include "cartographer/mapping/2d/grid_2d.h" -#include "cartographer/mapping/probability_values.h" #include "cartographer/sensor/point_cloud.h" #include "ceres/ceres.h" -#include "ceres/cubic_interpolation.h" namespace cartographer { namespace mapping { namespace scan_matching { -// Computes a cost for matching the 'point_cloud' to the 'grid' with +// Creates a cost function for matching the 'point_cloud' to the 'grid' with // a 'pose'. The cost increases with poorer correspondence of the grid and the // point observation (e.g. points falling into less occupied space). -class OccupiedSpaceCostFunction2D { - public: - static ceres::CostFunction* CreateAutoDiffCostFunction( - const double scaling_factor, const sensor::PointCloud& point_cloud, - const Grid2D& grid) { - return new ceres::AutoDiffCostFunction( - new OccupiedSpaceCostFunction2D(scaling_factor, point_cloud, grid), - point_cloud.size()); - } - - template - bool operator()(const T* const pose, T* residual) const { - Eigen::Matrix translation(pose[0], pose[1]); - Eigen::Rotation2D rotation(pose[2]); - Eigen::Matrix rotation_matrix = rotation.toRotationMatrix(); - Eigen::Matrix transform; - transform << rotation_matrix, translation, T(0.), T(0.), T(1.); - - const GridArrayAdapter adapter(grid_); - ceres::BiCubicInterpolator interpolator(adapter); - const MapLimits& limits = grid_.limits(); - - for (size_t i = 0; i < point_cloud_.size(); ++i) { - // Note that this is a 2D point. The third component is a scaling factor. - const Eigen::Matrix point((T(point_cloud_[i].x())), - (T(point_cloud_[i].y())), T(1.)); - const Eigen::Matrix world = transform * point; - interpolator.Evaluate( - (limits.max().x() - world[0]) / limits.resolution() - 0.5 + - static_cast(kPadding), - (limits.max().y() - world[1]) / limits.resolution() - 0.5 + - static_cast(kPadding), - &residual[i]); - residual[i] = scaling_factor_ * residual[i]; - } - return true; - } - - private: - static constexpr int kPadding = INT_MAX / 4; - class GridArrayAdapter { - public: - enum { DATA_DIMENSION = 1 }; - - explicit GridArrayAdapter(const Grid2D& grid) : grid_(grid) {} - - void GetValue(const int row, const int column, double* const value) const { - if (row < kPadding || column < kPadding || row >= NumRows() - kPadding || - column >= NumCols() - kPadding) { - *value = kMaxCorrespondenceCost; - } else { - *value = static_cast(grid_.GetCorrespondenceCost( - Eigen::Array2i(column - kPadding, row - kPadding))); - } - } - - int NumRows() const { - return grid_.limits().cell_limits().num_y_cells + 2 * kPadding; - } - - int NumCols() const { - return grid_.limits().cell_limits().num_x_cells + 2 * kPadding; - } - - private: - const Grid2D& grid_; - }; - - OccupiedSpaceCostFunction2D(const double scaling_factor, - const sensor::PointCloud& point_cloud, - const Grid2D& grid) - : scaling_factor_(scaling_factor), - point_cloud_(point_cloud), - grid_(grid) {} - - OccupiedSpaceCostFunction2D(const OccupiedSpaceCostFunction2D&) = delete; - OccupiedSpaceCostFunction2D& operator=(const OccupiedSpaceCostFunction2D&) = - delete; - - const double scaling_factor_; - const sensor::PointCloud& point_cloud_; - const Grid2D& grid_; -}; +ceres::CostFunction* CreateOccupiedSpaceCostFunction2D( + const double scaling_factor, const sensor::PointCloud& point_cloud, + const Grid2D& grid); } // namespace scan_matching } // namespace mapping diff --git a/cartographer/mapping/internal/2d/scan_matching/occupied_space_cost_function_2d_test.cc b/cartographer/mapping/internal/2d/scan_matching/occupied_space_cost_function_2d_test.cc new file mode 100644 index 0000000..37281a7 --- /dev/null +++ b/cartographer/mapping/internal/2d/scan_matching/occupied_space_cost_function_2d_test.cc @@ -0,0 +1,57 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/mapping/internal/2d/scan_matching/occupied_space_cost_function_2d.h" + +#include "cartographer/mapping/2d/probability_grid.h" +#include "cartographer/mapping/probability_values.h" + +#include "gmock/gmock.h" +#include "gtest/gtest.h" + +namespace cartographer { +namespace mapping { +namespace scan_matching { +namespace { + +using ::testing::DoubleEq; +using ::testing::ElementsAre; + +TEST(OccupiedSpaceCostFunction2DTest, SmokeTest) { + ProbabilityGrid grid( + MapLimits(1., Eigen::Vector2d(1., 1.), CellLimits(2, 2))); + sensor::PointCloud point_cloud = {Eigen::Vector3f{0.f, 0.f, 0.f}}; + ceres::Problem problem; + std::unique_ptr cost_function( + CreateOccupiedSpaceCostFunction2D(1.f, point_cloud, grid)); + + const std::array pose_estimate{{0., 0., 0.}}; + const std::array parameter_blocks{{pose_estimate.data()}}; + + std::array residuals; + std::array, 1> jacobians; + std::array jacobians_ptrs; + for (int i = 0; i < 1; ++i) jacobians_ptrs[i] = jacobians[i].data(); + cost_function->Evaluate(parameter_blocks.data(), residuals.data(), + jacobians_ptrs.data()); + + EXPECT_THAT(residuals, ElementsAre(DoubleEq(kMaxProbability))); +} + +} // namespace +} // namespace scan_matching +} // namespace mapping +} // namespace cartographer \ No newline at end of file