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 
20 #include "gtest/gtest.h"
21 
22 namespace cartographer {
23 namespace mapping {
24 namespace scan_matching {
25 namespace {
26 
27 TEST(SearchParameters, Construction) {
28  const SearchParameters search_parameters(4, 5, 0.03, 0.05);
29  EXPECT_EQ(5, search_parameters.num_angular_perturbations);
30  EXPECT_NEAR(0.03, search_parameters.angular_perturbation_step_size, 1e-9);
31  EXPECT_NEAR(0.05, search_parameters.resolution, 1e-9);
32  EXPECT_EQ(11, search_parameters.num_scans);
33  EXPECT_EQ(11, search_parameters.linear_bounds.size());
34  for (const SearchParameters::LinearBounds linear_bounds :
35  search_parameters.linear_bounds) {
36  EXPECT_EQ(-4, linear_bounds.min_x);
37  EXPECT_EQ(4, linear_bounds.max_x);
38  EXPECT_EQ(-4, linear_bounds.min_y);
39  EXPECT_EQ(4, linear_bounds.max_y);
40  }
41 }
42 
43 TEST(Candidate, Construction) {
44  const SearchParameters search_parameters(4, 5, 0.03, 0.05);
45  const Candidate2D candidate(3, 4, -5, search_parameters);
46  EXPECT_EQ(3, candidate.scan_index);
47  EXPECT_EQ(4, candidate.x_index_offset);
48  EXPECT_EQ(-5, candidate.y_index_offset);
49  EXPECT_NEAR(0.25, candidate.x, 1e-9);
50  EXPECT_NEAR(-0.2, candidate.y, 1e-9);
51  EXPECT_NEAR(-0.06, candidate.orientation, 1e-9);
52  EXPECT_NEAR(0., candidate.score, 1e-9);
53 
54  Candidate2D bigger_candidate(3, 4, 5, search_parameters);
55  bigger_candidate.score = 1.;
56  EXPECT_LT(candidate, bigger_candidate);
57 }
58 
60  sensor::PointCloud point_cloud;
61  point_cloud.emplace_back(-1.f, 1.f, 0.f);
62  const std::vector<sensor::PointCloud> scans =
63  GenerateRotatedScans(point_cloud, SearchParameters(0, 1, M_PI / 2., 0.));
64  EXPECT_EQ(3, scans.size());
65  EXPECT_NEAR(1., scans[0][0].x(), 1e-6);
66  EXPECT_NEAR(1., scans[0][0].y(), 1e-6);
67  EXPECT_NEAR(-1., scans[1][0].x(), 1e-6);
68  EXPECT_NEAR(1., scans[1][0].y(), 1e-6);
69  EXPECT_NEAR(-1., scans[2][0].x(), 1e-6);
70  EXPECT_NEAR(-1., scans[2][0].y(), 1e-6);
71 }
72 
74  sensor::PointCloud point_cloud;
75  point_cloud.emplace_back(0.025f, 0.175f, 0.f);
76  point_cloud.emplace_back(-0.025f, 0.175f, 0.f);
77  point_cloud.emplace_back(-0.075f, 0.175f, 0.f);
78  point_cloud.emplace_back(-0.125f, 0.175f, 0.f);
79  point_cloud.emplace_back(-0.125f, 0.125f, 0.f);
80  point_cloud.emplace_back(-0.125f, 0.075f, 0.f);
81  point_cloud.emplace_back(-0.125f, 0.025f, 0.f);
82  const MapLimits map_limits(0.05, Eigen::Vector2d(0.05, 0.25),
83  CellLimits(6, 6));
84  const std::vector<sensor::PointCloud> scans =
85  GenerateRotatedScans(point_cloud, SearchParameters(0, 0, 0., 0.));
86  const std::vector<DiscreteScan2D> discrete_scans =
87  DiscretizeScans(map_limits, scans, Eigen::Translation2f::Identity());
88  EXPECT_EQ(1, discrete_scans.size());
89  EXPECT_EQ(7, discrete_scans[0].size());
90  EXPECT_TRUE((Eigen::Array2i(1, 0) == discrete_scans[0][0]).all());
91  EXPECT_TRUE((Eigen::Array2i(1, 1) == discrete_scans[0][1]).all());
92  EXPECT_TRUE((Eigen::Array2i(1, 2) == discrete_scans[0][2]).all());
93  EXPECT_TRUE((Eigen::Array2i(1, 3) == discrete_scans[0][3]).all());
94  EXPECT_TRUE((Eigen::Array2i(2, 3) == discrete_scans[0][4]).all());
95  EXPECT_TRUE((Eigen::Array2i(3, 3) == discrete_scans[0][5]).all());
96  EXPECT_TRUE((Eigen::Array2i(4, 3) == discrete_scans[0][6]).all());
97 }
98 
99 } // namespace
100 } // namespace scan_matching
101 } // namespace mapping
102 } // namespace cartographer
std::vector< DiscreteScan2D > DiscretizeScans(const MapLimits &map_limits, const std::vector< sensor::PointCloud > &scans, const Eigen::Translation2f &initial_translation)
std::vector< sensor::PointCloud > GenerateRotatedScans(const sensor::PointCloud &point_cloud, const SearchParameters &search_parameters)
std::vector< Eigen::Vector3f > PointCloud
Definition: point_cloud.h:32
TEST(TrajectoryConnectivityStateTest, UnknownTrajectory)


cartographer
Author(s): The Cartographer Authors
autogenerated on Mon Feb 28 2022 22:00:58