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

Add TSDF match cost function. (#1359)

master
Kevin Daun 2018-08-01 23:46:33 +02:00 committed by Wally B. Feed
parent 6c070acff5
commit 748deb910a
4 changed files with 299 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
cartographer/mapping/internal/2d/scan_matching/interpolated_tsdf_2d.h
View File

@ -96,11 +96,11 @@ class InterpolatedTSDF2D {
T InterpolateBilinear(const T& x, const T& y, float x1, float y1, float x2,
float y2, float q11, float q12, float q21,
float q22) const {
const T normalized_x = (x - x1) / (x2 - x1);
const T normalized_y = (y - y1) / (y2 - y1);
const T q1 = (q12 - q11) * normalized_y + q11;
const T q2 = (q22 - q21) * normalized_y + q21;
return (q2 - q1) * normalized_x + q1;
const T normalized_x = (x - T(x1)) / T(x2 - x1);
const T normalized_y = (y - T(y1)) / T(y2 - y1);
const T q1 = T(q12 - q11) * normalized_y + T(q11);
const T q2 = T(q22 - q21) * normalized_y + T(q21);
return T(q2 - q1) * normalized_x + T(q1);
}
// Center of the next lower pixel, i.e., not necessarily the pixel containing

88
cartographer/mapping/internal/2d/scan_matching/tsdf_match_cost_function_2d.cc Normal file
View File

@ -0,0 +1,88 @@
/*
* 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 "Eigen/Core"
#include "Eigen/Geometry"
#include "cartographer/mapping/internal/2d/scan_matching/interpolated_tsdf_2d.h"
#include "cartographer/sensor/point_cloud.h"
#include "ceres/ceres.h"
namespace cartographer {
namespace mapping {
namespace scan_matching {
// Computes a cost for matching the 'point_cloud' in the 'grid' at
// a 'pose'. The cost increases with the signed distance of the matched point
// location in the 'grid'.
class TSDFMatchCostFunction2D {
public:
TSDFMatchCostFunction2D(const double residual_scaling_factor,
const sensor::PointCloud& point_cloud,
const TSDF2D& grid)
: residual_scaling_factor_(residual_scaling_factor),
point_cloud_(point_cloud),
interpolated_grid_(grid) {}
template <typename T>
bool operator()(const T* const pose, T* residual) const {
const Eigen::Matrix<T, 2, 1> translation(pose[0], pose[1]);
const Eigen::Rotation2D<T> rotation(pose[2]);
const 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.);
T summed_weight = T(0);
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;
const T point_weight = interpolated_grid_.GetWeight(world[0], world[1]);
summed_weight += point_weight;
residual[i] =
T(point_cloud_.size()) * residual_scaling_factor_ *
interpolated_grid_.GetCorrespondenceCost(world[0], world[1]) *
point_weight;
}
if (summed_weight == T(0)) return false;
for (size_t i = 0; i < point_cloud_.size(); ++i) {
residual[i] /= summed_weight;
}
return true;
}
private:
TSDFMatchCostFunction2D(const TSDFMatchCostFunction2D&) = delete;
TSDFMatchCostFunction2D& operator=(const TSDFMatchCostFunction2D&) = delete;
const double residual_scaling_factor_;
const sensor::PointCloud& point_cloud_;
const InterpolatedTSDF2D interpolated_grid_;
};
ceres::CostFunction* CreateTSDFMatchCostFunction2D(
const double scaling_factor, const sensor::PointCloud& point_cloud,
const TSDF2D& tsdf) {
return new ceres::AutoDiffCostFunction<TSDFMatchCostFunction2D,
ceres::DYNAMIC /* residuals */,
3 /* pose variables */>(
new TSDFMatchCostFunction2D(scaling_factor, point_cloud, tsdf),
point_cloud.size());
}
} // namespace scan_matching
} // namespace mapping
} // namespace cartographer

39
cartographer/mapping/internal/2d/scan_matching/tsdf_match_cost_function_2d.h Normal file
View File

@ -0,0 +1,39 @@
/*
* 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.
*/
#ifndef CARTOGRAPHER_MAPPING_INTERNAL_2D_SCAN_MATCHING_TSDF_MATCH_COST_FUNCTION_2D_H_
#define CARTOGRAPHER_MAPPING_INTERNAL_2D_SCAN_MATCHING_TSDF_MATCH_COST_FUNCTION_2D_H_
#include "cartographer/mapping/2d/tsdf_2d.h"
#include "cartographer/sensor/point_cloud.h"
#include "ceres/ceres.h"
namespace cartographer {
namespace mapping {
namespace scan_matching {
// Creates a cost function for matching the 'point_cloud' in the 'grid' at
// a 'pose'. The cost increases with the signed distance of the matched point
// location in the 'grid'.
ceres::CostFunction* CreateTSDFMatchCostFunction2D(
const double scaling_factor, const sensor::PointCloud& point_cloud,
const TSDF2D& grid);
} // namespace scan_matching
} // namespace mapping
} // namespace cartographer
#endif // CARTOGRAPHER_MAPPING_INTERNAL_2D_SCAN_MATCHING_TSDF_MATCH_COST_FUNCTION_2D_H_

167
cartographer/mapping/internal/2d/scan_matching/tsdf_match_cost_function_2d_test.cc Normal file
View File

@ -0,0 +1,167 @@
/*
* 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/tsdf_match_cost_function_2d.h"
#include "cartographer/common/lua_parameter_dictionary.h"
#include "cartographer/common/lua_parameter_dictionary_test_helpers.h"
#include "cartographer/mapping/2d/tsdf_2d.h"
#include "cartographer/mapping/2d/tsdf_range_data_inserter_2d.h"
#include "gmock/gmock.h"
#include "gtest/gtest.h"
namespace cartographer {
namespace mapping {
namespace scan_matching {
namespace {
using ::testing::DoubleNear;
using ::testing::ElementsAre;
class TSDFSpaceCostFunction2DTest : public ::testing::Test {
protected:
TSDFSpaceCostFunction2DTest()
: tsdf_(MapLimits(0.1, Eigen::Vector2d(2.05, 2.05), CellLimits(40, 40)),
0.3, 1.0, &conversion_tables_) {
auto parameter_dictionary = common::MakeDictionary(
"return { "
"truncation_distance = 0.3,"
"maximum_weight = 1.0,"
"update_free_space = false,"
"normal_estimation_options = {"
"num_normal_samples = 2,"
"sample_radius = 10.,"
"},"
"project_sdf_distance_to_scan_normal = true,"
"update_weight_range_exponent = 0,"
"update_weight_angle_scan_normal_to_ray_kernel_bandwith = 0,"
"update_weight_distance_cell_to_hit_kernel_bandwith = 0,"
"}");
options_ = CreateTSDFRangeDataInserterOptions2D(parameter_dictionary.get());
range_data_inserter_ = absl::make_unique<TSDFRangeDataInserter2D>(options_);
}
void InsertPointcloud() {
sensor::RangeData range_data;
for (float x = -.5; x < 0.5f; x += 0.1) {
range_data.returns.emplace_back(x, 1.0f, 0.f);
}
range_data.origin.x() = -0.5f;
range_data.origin.y() = -0.5f;
range_data_inserter_->Insert(range_data, &tsdf_);
tsdf_.FinishUpdate();
}
ValueConversionTables conversion_tables_;
proto::TSDFRangeDataInserterOptions2D options_;
TSDF2D tsdf_;
std::unique_ptr<TSDFRangeDataInserter2D> range_data_inserter_;
};
TEST_F(TSDFSpaceCostFunction2DTest, MatchEmptyTSDF) {
const sensor::PointCloud matching_cloud = {Eigen::Vector3f{0.f, 0.f, 0.f}};
std::unique_ptr<ceres::CostFunction> cost_function(
CreateTSDFMatchCostFunction2D(1.f, matching_cloud, tsdf_));
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();
bool valid_result = cost_function->Evaluate(
parameter_blocks.data(), residuals.data(), jacobians_ptrs.data());
EXPECT_FALSE(valid_result);
}
TEST_F(TSDFSpaceCostFunction2DTest, ExactInitialPose) {
InsertPointcloud();
const sensor::PointCloud matching_cloud = {Eigen::Vector3f{0.f, 1.0f, 0.f}};
std::unique_ptr<ceres::CostFunction> cost_function(
CreateTSDFMatchCostFunction2D(1.f, matching_cloud, tsdf_));
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();
const bool valid_result = cost_function->Evaluate(
parameter_blocks.data(), residuals.data(), jacobians_ptrs.data());
EXPECT_TRUE(valid_result);
EXPECT_THAT(residuals, ElementsAre(DoubleNear(0., 1e-3)));
EXPECT_THAT(jacobians[0],
ElementsAre(DoubleNear(0., 1e-3), DoubleNear(-1., 1e-3),
DoubleNear(0., 1e-3)));
}
TEST_F(TSDFSpaceCostFunction2DTest, PertubatedInitialPose) {
InsertPointcloud();
sensor::PointCloud matching_cloud = {Eigen::Vector3f{0.f, 1.0f, 0.f}};
std::unique_ptr<ceres::CostFunction> cost_function(
CreateTSDFMatchCostFunction2D(1.f, matching_cloud, tsdf_));
std::array<double, 3> pose_estimate{{0., 0.1, 0.}};
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();
bool valid_result = cost_function->Evaluate(
parameter_blocks.data(), residuals.data(), jacobians_ptrs.data());
EXPECT_TRUE(valid_result);
EXPECT_THAT(residuals, ElementsAre(DoubleNear(-0.1, 1e-3)));
EXPECT_THAT(jacobians[0],
ElementsAre(DoubleNear(0., 1e-3), DoubleNear(-1., 1e-3),
DoubleNear(0., 1e-3)));
pose_estimate[1] = -0.1;
parameter_blocks = {{pose_estimate.data()}};
valid_result = cost_function->Evaluate(
parameter_blocks.data(), residuals.data(), jacobians_ptrs.data());
EXPECT_TRUE(valid_result);
EXPECT_THAT(residuals, ElementsAre(DoubleNear(0.1, 1e-3)));
EXPECT_THAT(jacobians[0],
ElementsAre(DoubleNear(0., 1e-3), DoubleNear(-1., 1e-3),
DoubleNear(0., 1e-3)));
}
TEST_F(TSDFSpaceCostFunction2DTest, InvalidInitialPose) {
InsertPointcloud();
sensor::PointCloud matching_cloud = {Eigen::Vector3f{0.f, 1.0f, 0.f}};
std::unique_ptr<ceres::CostFunction> cost_function(
CreateTSDFMatchCostFunction2D(1.f, matching_cloud, tsdf_));
std::array<double, 3> pose_estimate{{0., 0.4, 0.}};
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();
bool valid_result = cost_function->Evaluate(
parameter_blocks.data(), residuals.data(), jacobians_ptrs.data());
EXPECT_FALSE(valid_result);
pose_estimate[1] = -0.4;
parameter_blocks = {{pose_estimate.data()}};
valid_result = cost_function->Evaluate(
parameter_blocks.data(), residuals.data(), jacobians_ptrs.data());
EXPECT_FALSE(valid_result);
}
} // namespace
} // namespace scan_matching
} // namespace mapping
} // namespace cartographer