Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017 #include "cartographer/mapping/internal/2d/scan_matching/ceres_scan_matcher_2d.h"
00018
00019 #include <memory>
00020
00021 #include "absl/memory/memory.h"
00022 #include "cartographer/common/lua_parameter_dictionary.h"
00023 #include "cartographer/common/lua_parameter_dictionary_test_helpers.h"
00024 #include "cartographer/mapping/2d/probability_grid.h"
00025 #include "cartographer/mapping/probability_values.h"
00026 #include "cartographer/sensor/point_cloud.h"
00027 #include "cartographer/transform/rigid_transform_test_helpers.h"
00028 #include "gtest/gtest.h"
00029
00030 namespace cartographer {
00031 namespace mapping {
00032 namespace scan_matching {
00033 namespace {
00034
00035 class CeresScanMatcherTest : public ::testing::Test {
00036 protected:
00037 CeresScanMatcherTest()
00038 : probability_grid_(
00039 MapLimits(1., Eigen::Vector2d(10., 10.), CellLimits(20, 20)),
00040 &conversion_tables_) {
00041 probability_grid_.SetProbability(
00042 probability_grid_.limits().GetCellIndex(Eigen::Vector2f(-3.5f, 2.5f)),
00043 kMaxProbability);
00044
00045 point_cloud_.push_back({Eigen::Vector3f{-3.f, 2.f, 0.f}});
00046
00047 auto parameter_dictionary = common::MakeDictionary(R"text(
00048 return {
00049 occupied_space_weight = 1.,
00050 translation_weight = 0.1,
00051 rotation_weight = 1.5,
00052 ceres_solver_options = {
00053 use_nonmonotonic_steps = true,
00054 max_num_iterations = 50,
00055 num_threads = 1,
00056 },
00057 })text");
00058 const proto::CeresScanMatcherOptions2D options =
00059 CreateCeresScanMatcherOptions2D(parameter_dictionary.get());
00060 ceres_scan_matcher_ = absl::make_unique<CeresScanMatcher2D>(options);
00061 }
00062
00063 void TestFromInitialPose(const transform::Rigid2d& initial_pose) {
00064 transform::Rigid2d pose;
00065 const transform::Rigid2d expected_pose =
00066 transform::Rigid2d::Translation({-0.5, 0.5});
00067 ceres::Solver::Summary summary;
00068 ceres_scan_matcher_->Match(initial_pose.translation(), initial_pose,
00069 point_cloud_, probability_grid_, &pose,
00070 &summary);
00071 EXPECT_NEAR(0., summary.final_cost, 1e-2) << summary.FullReport();
00072 EXPECT_THAT(pose, transform::IsNearly(expected_pose, 1e-2))
00073 << "Actual: " << transform::ToProto(pose).DebugString()
00074 << "\nExpected: " << transform::ToProto(expected_pose).DebugString();
00075 }
00076
00077 ValueConversionTables conversion_tables_;
00078 ProbabilityGrid probability_grid_;
00079 sensor::PointCloud point_cloud_;
00080 std::unique_ptr<CeresScanMatcher2D> ceres_scan_matcher_;
00081 };
00082
00083 TEST_F(CeresScanMatcherTest, testPerfectEstimate) {
00084 TestFromInitialPose(transform::Rigid2d::Translation({-0.5, 0.5}));
00085 }
00086
00087 TEST_F(CeresScanMatcherTest, testOptimizeAlongX) {
00088 TestFromInitialPose(transform::Rigid2d::Translation({-0.3, 0.5}));
00089 }
00090
00091 TEST_F(CeresScanMatcherTest, testOptimizeAlongY) {
00092 TestFromInitialPose(transform::Rigid2d::Translation({-0.45, 0.3}));
00093 }
00094
00095 TEST_F(CeresScanMatcherTest, testOptimizeAlongXY) {
00096 TestFromInitialPose(transform::Rigid2d::Translation({-0.3, 0.3}));
00097 }
00098
00099 }
00100 }
00101 }
00102 }