mapping_2d/scan_matching/real_time_correlative_scan_matcher_test.cc
Go to the documentation of this file.
1 /*
2  * Copyright 2016 The Cartographer Authors
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  * http://www.apache.org/licenses/LICENSE-2.0
9  *
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  */
16 
18 
19 #include <cmath>
20 #include <memory>
21 
22 #include "Eigen/Geometry"
29 #include "gtest/gtest.h"
30 
31 namespace cartographer {
32 namespace mapping_2d {
33 namespace scan_matching {
34 namespace {
35 
36 class RealTimeCorrelativeScanMatcherTest : public ::testing::Test {
37  protected:
38  RealTimeCorrelativeScanMatcherTest()
40  MapLimits(0.05, Eigen::Vector2d(0.05, 0.25), CellLimits(6, 6))) {
41  {
42  auto parameter_dictionary = common::MakeDictionary(
43  "return { "
44  "insert_free_space = true, "
45  "hit_probability = 0.7, "
46  "miss_probability = 0.4, "
47  "}");
48  range_data_inserter_ = common::make_unique<RangeDataInserter>(
49  CreateRangeDataInserterOptions(parameter_dictionary.get()));
50  }
51  point_cloud_.emplace_back(0.025f, 0.175f, 0.f);
52  point_cloud_.emplace_back(-0.025f, 0.175f, 0.f);
53  point_cloud_.emplace_back(-0.075f, 0.175f, 0.f);
54  point_cloud_.emplace_back(-0.125f, 0.175f, 0.f);
55  point_cloud_.emplace_back(-0.125f, 0.125f, 0.f);
56  point_cloud_.emplace_back(-0.125f, 0.075f, 0.f);
57  point_cloud_.emplace_back(-0.125f, 0.025f, 0.f);
58  probability_grid_.StartUpdate();
59  range_data_inserter_->Insert(
60  sensor::RangeData{Eigen::Vector3f::Zero(), point_cloud_, {}},
62  {
63  auto parameter_dictionary = common::MakeDictionary(
64  "return {"
65  "linear_search_window = 0.6, "
66  "angular_search_window = 0.16, "
67  "translation_delta_cost_weight = 0., "
68  "rotation_delta_cost_weight = 0., "
69  "}");
71  common::make_unique<RealTimeCorrelativeScanMatcher>(
73  parameter_dictionary.get()));
74  }
75  }
76 
77  ProbabilityGrid probability_grid_;
78  std::unique_ptr<RangeDataInserter> range_data_inserter_;
80  std::unique_ptr<RealTimeCorrelativeScanMatcher>
82 };
83 
84 TEST_F(RealTimeCorrelativeScanMatcherTest,
85  ScorePerfectHighResolutionCandidate) {
86  const std::vector<sensor::PointCloud> scans =
87  GenerateRotatedScans(point_cloud_, SearchParameters(0, 0, 0., 0.));
88  const std::vector<DiscreteScan> discrete_scans = DiscretizeScans(
89  probability_grid_.limits(), scans, Eigen::Translation2f::Identity());
90  std::vector<Candidate> candidates;
91  candidates.emplace_back(0, 0, 0, SearchParameters(0, 0, 0., 0.));
92  real_time_correlative_scan_matcher_->ScoreCandidates(
93  probability_grid_, discrete_scans, SearchParameters(0, 0, 0., 0.),
94  &candidates);
95  EXPECT_EQ(0, candidates[0].scan_index);
96  EXPECT_EQ(0, candidates[0].x_index_offset);
97  EXPECT_EQ(0, candidates[0].y_index_offset);
98  // Every point should align perfectly.
99  EXPECT_NEAR(0.7, candidates[0].score, 1e-2);
100 }
101 
102 TEST_F(RealTimeCorrelativeScanMatcherTest,
103  ScorePartiallyCorrectHighResolutionCandidate) {
104  const std::vector<sensor::PointCloud> scans =
105  GenerateRotatedScans(point_cloud_, SearchParameters(0, 0, 0., 0.));
106  const std::vector<DiscreteScan> discrete_scans = DiscretizeScans(
107  probability_grid_.limits(), scans, Eigen::Translation2f::Identity());
108  std::vector<Candidate> candidates;
109  candidates.emplace_back(0, 0, 1, SearchParameters(0, 0, 0., 0.));
110  real_time_correlative_scan_matcher_->ScoreCandidates(
111  probability_grid_, discrete_scans, SearchParameters(0, 0, 0., 0.),
112  &candidates);
113  EXPECT_EQ(0, candidates[0].scan_index);
114  EXPECT_EQ(0, candidates[0].x_index_offset);
115  EXPECT_EQ(1, candidates[0].y_index_offset);
116  // 3 points should align perfectly.
117  EXPECT_LT(0.7 * 3. / 7., candidates[0].score);
118  EXPECT_GT(0.7, candidates[0].score);
119 }
120 
121 } // namespace
122 } // namespace scan_matching
123 } // namespace mapping_2d
124 } // namespace cartographer
std::vector< DiscreteScan > DiscretizeScans(const MapLimits &map_limits, const std::vector< sensor::PointCloud > &scans, const Eigen::Translation2f &initial_translation)
std::unique_ptr< RealTimeCorrelativeScanMatcher > real_time_correlative_scan_matcher_
std::unique_ptr< LuaParameterDictionary > MakeDictionary(const string &code)
proto::RangeDataInserterOptions CreateRangeDataInserterOptions(common::LuaParameterDictionary *const parameter_dictionary)
proto::RealTimeCorrelativeScanMatcherOptions CreateRealTimeCorrelativeScanMatcherOptions(common::LuaParameterDictionary *const parameter_dictionary)
std::vector< Eigen::Vector3f > PointCloud
Definition: point_cloud.h:30
std::unique_ptr< RangeDataInserter > range_data_inserter_
std::vector< sensor::PointCloud > GenerateRotatedScans(const sensor::PointCloud &point_cloud, const SearchParameters &search_parameters)


cartographer
Author(s):
autogenerated on Mon Jun 10 2019 12:51:39