mapping_3d/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 <memory>
20 
21 #include "Eigen/Core"
29 #include "gtest/gtest.h"
30 
31 namespace cartographer {
32 namespace mapping_3d {
33 namespace scan_matching {
34 namespace {
35 
36 class RealTimeCorrelativeScanMatcherTest : public ::testing::Test {
37  protected:
38  RealTimeCorrelativeScanMatcherTest()
39  : hybrid_grid_(0.1f),
40  expected_pose_(Eigen::Vector3d(-1., 0., 0.),
41  Eigen::Quaterniond::Identity()) {
42  for (const auto& point :
43  {Eigen::Vector3f(-3.f, 2.f, 0.f), Eigen::Vector3f(-4.f, 2.f, 0.f),
44  Eigen::Vector3f(-5.f, 2.f, 0.f), Eigen::Vector3f(-6.f, 2.f, 0.f),
45  Eigen::Vector3f(-6.f, 3.f, 1.f), Eigen::Vector3f(-6.f, 4.f, 2.f),
46  Eigen::Vector3f(-7.f, 3.f, 1.f)}) {
47  point_cloud_.push_back(point);
48  hybrid_grid_.SetProbability(
49  hybrid_grid_.GetCellIndex(expected_pose_.cast<float>() * point), 1.);
50  }
51 
52  auto parameter_dictionary = common::MakeDictionary(R"text(
53  return {
54  linear_search_window = 0.3,
55  angular_search_window = math.rad(1.),
56  translation_delta_cost_weight = 1e-1,
57  rotation_delta_cost_weight = 1.,
58  })text");
60  new RealTimeCorrelativeScanMatcher(
61  mapping_2d::scan_matching::
63  parameter_dictionary.get())));
64  }
65 
66  void TestFromInitialPose(const transform::Rigid3d& initial_pose) {
68 
69  const float score = real_time_correlative_scan_matcher_->Match(
70  initial_pose, point_cloud_, hybrid_grid_, &pose);
71  LOG(INFO) << "Score: " << score;
72  EXPECT_THAT(pose, transform::IsNearly(expected_pose_, 1e-3));
73  }
74 
75  HybridGrid hybrid_grid_;
78  std::unique_ptr<RealTimeCorrelativeScanMatcher>
80 };
81 
82 TEST_F(RealTimeCorrelativeScanMatcherTest, PerfectEstimate) {
83  TestFromInitialPose(
84  transform::Rigid3d::Translation(Eigen::Vector3d(-1., 0., 0.)));
85 }
86 
87 TEST_F(RealTimeCorrelativeScanMatcherTest, AlongX) {
88  TestFromInitialPose(
89  transform::Rigid3d::Translation(Eigen::Vector3d(-0.8, 0., 0.)));
90 }
91 
92 TEST_F(RealTimeCorrelativeScanMatcherTest, AlongZ) {
93  TestFromInitialPose(
94  transform::Rigid3d::Translation(Eigen::Vector3d(-1., 0., -0.2)));
95 }
96 
97 TEST_F(RealTimeCorrelativeScanMatcherTest, AlongXYZ) {
98  TestFromInitialPose(
99  transform::Rigid3d::Translation(Eigen::Vector3d(-0.9, -0.2, 0.2)));
100 }
101 
102 TEST_F(RealTimeCorrelativeScanMatcherTest, RotationAroundX) {
103  TestFromInitialPose(transform::Rigid3d(
104  Eigen::Vector3d(-1., 0., 0.),
105  Eigen::AngleAxisd(0.8 / 180. * M_PI, Eigen::Vector3d(1., 0., 0.))));
106 }
107 
108 TEST_F(RealTimeCorrelativeScanMatcherTest, RotationAroundY) {
109  TestFromInitialPose(transform::Rigid3d(
110  Eigen::Vector3d(-1., 0., 0.),
111  Eigen::AngleAxisd(0.8 / 180. * M_PI, Eigen::Vector3d(0., 1., 0.))));
112 }
113 
114 TEST_F(RealTimeCorrelativeScanMatcherTest, RotationAroundYZ) {
115  TestFromInitialPose(transform::Rigid3d(
116  Eigen::Vector3d(-1., 0., 0.),
117  Eigen::AngleAxisd(0.8 / 180. * M_PI, Eigen::Vector3d(0., 1., 1.))));
118 }
119 
120 } // namespace
121 } // namespace scan_matching
122 } // namespace mapping_3d
123 } // namespace cartographer
Rigid3< double > Rigid3d
std::unique_ptr< RealTimeCorrelativeScanMatcher > real_time_correlative_scan_matcher_
transform::Rigid3d pose
std::unique_ptr< LuaParameterDictionary > MakeDictionary(const string &code)
proto::RealTimeCorrelativeScanMatcherOptions CreateRealTimeCorrelativeScanMatcherOptions(common::LuaParameterDictionary *const parameter_dictionary)
std::vector< Eigen::Vector3f > PointCloud
Definition: point_cloud.h:30
static Rigid3 Translation(const Vector &vector)


cartographer
Author(s):
autogenerated on Wed Jun 5 2019 21:57:58