mapping_3d/scan_matching/ceres_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"
27 #include "gtest/gtest.h"
28 
29 namespace cartographer {
30 namespace mapping_3d {
31 namespace scan_matching {
32 namespace {
33 
34 class CeresScanMatcherTest : public ::testing::Test {
35  protected:
36  CeresScanMatcherTest()
37  : hybrid_grid_(1.f),
39  transform::Rigid3d::Translation(Eigen::Vector3d(-1., 0., 0.))) {
40  for (const auto& point :
41  {Eigen::Vector3f(-3.f, 2.f, 0.f), Eigen::Vector3f(-4.f, 2.f, 0.f),
42  Eigen::Vector3f(-5.f, 2.f, 0.f), Eigen::Vector3f(-6.f, 2.f, 0.f),
43  Eigen::Vector3f(-6.f, 3.f, 1.f), Eigen::Vector3f(-6.f, 4.f, 2.f),
44  Eigen::Vector3f(-7.f, 3.f, 1.f)}) {
45  point_cloud_.push_back(point);
46  hybrid_grid_.SetProbability(
47  hybrid_grid_.GetCellIndex(expected_pose_.cast<float>() * point), 1.);
48  }
49 
50  auto parameter_dictionary = common::MakeDictionary(R"text(
51  return {
52  occupied_space_weight_0 = 1.,
53  translation_weight = 0.01,
54  rotation_weight = 0.1,
55  only_optimize_yaw = false,
56  ceres_solver_options = {
57  use_nonmonotonic_steps = true,
58  max_num_iterations = 10,
59  num_threads = 1,
60  },
61  })text");
62  options_ = CreateCeresScanMatcherOptions(parameter_dictionary.get());
63  ceres_scan_matcher_.reset(new CeresScanMatcher(options_));
64  }
65 
66  void TestFromInitialPose(const transform::Rigid3d& initial_pose) {
68 
69  ceres::Solver::Summary summary;
70  ceres_scan_matcher_->Match(initial_pose, initial_pose,
72  &summary);
73  EXPECT_NEAR(0., summary.final_cost, 1e-2) << summary.FullReport();
74  EXPECT_THAT(pose, transform::IsNearly(expected_pose_, 3e-2));
75  }
76 
77  HybridGrid hybrid_grid_;
80  proto::CeresScanMatcherOptions options_;
81  std::unique_ptr<CeresScanMatcher> ceres_scan_matcher_;
82 };
83 
84 TEST_F(CeresScanMatcherTest, PerfectEstimate) {
85  TestFromInitialPose(
86  transform::Rigid3d::Translation(Eigen::Vector3d(-1., 0., 0.)));
87 }
88 
89 TEST_F(CeresScanMatcherTest, AlongX) {
90  ceres_scan_matcher_.reset(new CeresScanMatcher(options_));
91  TestFromInitialPose(
92  transform::Rigid3d::Translation(Eigen::Vector3d(-0.8, 0., 0.)));
93 }
94 
95 TEST_F(CeresScanMatcherTest, AlongZ) {
96  TestFromInitialPose(
97  transform::Rigid3d::Translation(Eigen::Vector3d(-1., 0., -0.2)));
98 }
99 
100 TEST_F(CeresScanMatcherTest, AlongXYZ) {
101  TestFromInitialPose(
102  transform::Rigid3d::Translation(Eigen::Vector3d(-0.9, -0.2, 0.2)));
103 }
104 
105 TEST_F(CeresScanMatcherTest, FullPoseCorrection) {
106  // We try to find the rotation around z...
107  const auto additional_transform = transform::Rigid3d::Rotation(
108  Eigen::AngleAxisd(0.05, Eigen::Vector3d(0., 0., 1.)));
110  point_cloud_, additional_transform.cast<float>());
111  expected_pose_ = expected_pose_ * additional_transform.inverse();
112  // ...starting initially with rotation around x.
113  TestFromInitialPose(
114  transform::Rigid3d(Eigen::Vector3d(-0.95, -0.05, 0.05),
115  Eigen::AngleAxisd(0.05, Eigen::Vector3d(1., 0., 0.))));
116 }
117 
118 } // namespace
119 } // namespace scan_matching
120 } // namespace mapping_3d
121 } // namespace cartographer
PointCloud TransformPointCloud(const PointCloud &point_cloud, const transform::Rigid3f &transform)
Definition: point_cloud.cc:25
static Rigid3 Rotation(const AngleAxis &angle_axis)
Rigid3< double > Rigid3d
transform::Rigid3d pose
std::unique_ptr< LuaParameterDictionary > MakeDictionary(const string &code)
proto::CeresScanMatcherOptions options_
proto::CeresScanMatcherOptions CreateCeresScanMatcherOptions(common::LuaParameterDictionary *const parameter_dictionary)
std::unique_ptr< CeresScanMatcher > ceres_scan_matcher_
std::vector< Eigen::Vector3f > PointCloud
Definition: point_cloud.h:30
static Rigid3 Translation(const Vector &vector)


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