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

Move occupied space cost function to .cc (#1200)

master
Kevin Daun 2018-06-19 12:31:11 +02:00 committed by Wally B. Feed
parent a9045fa375
commit ecaa95f3b0
4 changed files with 181 additions and 91 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
cartographer/mapping/internal/2d/scan_matching/ceres_scan_matcher_2d.cc
View File

@ -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<double>(point_cloud.size())),
point_cloud, grid),

119
cartographer/mapping/internal/2d/scan_matching/occupied_space_cost_function_2d.cc Normal file
View File

@ -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 <typename T>
bool operator()(const T* const pose, T* residual) const {
Eigen::Matrix<T, 2, 1> translation(pose[0], pose[1]);
Eigen::Rotation2D<T> rotation(pose[2]);
Eigen::Matrix<T, 2, 2> rotation_matrix = rotation.toRotationMatrix();
Eigen::Matrix<T, 3, 3> transform;
transform << rotation_matrix, translation, T(0.), T(0.), T(1.);
const GridArrayAdapter adapter(grid_);
ceres::BiCubicInterpolator<GridArrayAdapter> 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<T, 3, 1> point((T(point_cloud_[i].x())),
(T(point_cloud_[i].y())), T(1.));
const Eigen::Matrix<T, 3, 1> world = transform * point;
interpolator.Evaluate(
(limits.max().x() - world[0]) / limits.resolution() - 0.5 +
static_cast<double>(kPadding),
(limits.max().y() - world[1]) / limits.resolution() - 0.5 +
static_cast<double>(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<double>(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<OccupiedSpaceCostFunction2D,
ceres::DYNAMIC /* residuals */,
3 /* pose variables */>(
new OccupiedSpaceCostFunction2D(scaling_factor, point_cloud, grid),
point_cloud.size());
}
} // namespace scan_matching
} // namespace mapping
} // namespace cartographer

94
cartographer/mapping/internal/2d/scan_matching/occupied_space_cost_function_2d.h
View File

@ -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<OccupiedSpaceCostFunction2D,
ceres::DYNAMIC /* residuals */,
3 /* pose variables */>(
new OccupiedSpaceCostFunction2D(scaling_factor, point_cloud, grid),
point_cloud.size());
}
template <typename T>
bool operator()(const T* const pose, T* residual) const {
Eigen::Matrix<T, 2, 1> translation(pose[0], pose[1]);
Eigen::Rotation2D<T> rotation(pose[2]);
Eigen::Matrix<T, 2, 2> rotation_matrix = rotation.toRotationMatrix();
Eigen::Matrix<T, 3, 3> transform;
transform << rotation_matrix, translation, T(0.), T(0.), T(1.);
const GridArrayAdapter adapter(grid_);
ceres::BiCubicInterpolator<GridArrayAdapter> 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<T, 3, 1> point((T(point_cloud_[i].x())),
(T(point_cloud_[i].y())), T(1.));
const Eigen::Matrix<T, 3, 1> world = transform * point;
interpolator.Evaluate(
(limits.max().x() - world[0]) / limits.resolution() - 0.5 +
static_cast<double>(kPadding),
(limits.max().y() - world[1]) / limits.resolution() - 0.5 +
static_cast<double>(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<double>(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

57
cartographer/mapping/internal/2d/scan_matching/occupied_space_cost_function_2d_test.cc Normal file
View File

@ -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<ceres::CostFunction> cost_function(
CreateOccupiedSpaceCostFunction2D(1.f, point_cloud, grid));
const std::array<double, 3> pose_estimate{{0., 0., 0.}};
const std::array<const double*, 1> parameter_blocks{{pose_estimate.data()}};
std::array<double, 1> residuals;
std::array<std::array<double, 3>, 1> jacobians;
std::array<double*, 1> 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