optimization_problem_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 <random>
20 
21 #include "Eigen/Core"
26 #include "glog/logging.h"
27 #include "gmock/gmock.h"
28 
29 namespace cartographer {
30 namespace mapping_3d {
31 namespace sparse_pose_graph {
32 namespace {
33 
34 class OptimizationProblemTest : public ::testing::Test {
35  protected:
36  OptimizationProblemTest()
37  : optimization_problem_(CreateOptions(), OptimizationProblem::FixZ::kNo),
38  rng_(45387) {}
39 
40  mapping::sparse_pose_graph::proto::OptimizationProblemOptions
41  CreateOptions() {
42  auto parameter_dictionary = common::MakeDictionary(R"text(
43  return {
44  acceleration_weight = 1e-4,
45  rotation_weight = 1e-2,
46  huber_scale = 1.,
47  consecutive_scan_translation_penalty_factor = 1e-2,
48  consecutive_scan_rotation_penalty_factor = 1e-2,
49  log_solver_summary = true,
50  ceres_solver_options = {
51  use_nonmonotonic_steps = false,
52  max_num_iterations = 200,
53  num_threads = 4,
54  },
55  })text");
57  parameter_dictionary.get());
58  }
59 
60  transform::Rigid3d RandomTransform(double translation_size,
61  double rotation_size) {
62  std::uniform_real_distribution<double> translation_distribution(
63  -translation_size, translation_size);
64  const double x = translation_distribution(rng_);
65  const double y = translation_distribution(rng_);
66  const double z = translation_distribution(rng_);
67  std::uniform_real_distribution<double> rotation_distribution(-rotation_size,
68  rotation_size);
69  const double rx = rotation_distribution(rng_);
70  const double ry = rotation_distribution(rng_);
71  const double rz = rotation_distribution(rng_);
72  return transform::Rigid3d(Eigen::Vector3d(x, y, z),
74  Eigen::Vector3d(rx, ry, rz)));
75  }
76 
77  transform::Rigid3d RandomYawOnlyTransform(double translation_size,
78  double rotation_size) {
79  std::uniform_real_distribution<double> translation_distribution(
80  -translation_size, translation_size);
81  const double x = translation_distribution(rng_);
82  const double y = translation_distribution(rng_);
83  const double z = translation_distribution(rng_);
84  std::uniform_real_distribution<double> rotation_distribution(-rotation_size,
85  rotation_size);
86  const double rz = rotation_distribution(rng_);
87  return transform::Rigid3d(Eigen::Vector3d(x, y, z),
89  Eigen::Vector3d(0., 0., rz)));
90  }
91 
92  OptimizationProblem optimization_problem_;
93  std::mt19937 rng_;
94 };
95 
96 transform::Rigid3d AddNoise(const transform::Rigid3d& transform,
97  const transform::Rigid3d& noise) {
98  const Eigen::Quaterniond noisy_rotation(noise.rotation() *
99  transform.rotation());
100  return transform::Rigid3d(transform.translation() + noise.translation(),
101  noisy_rotation);
102 }
103 
104 TEST_F(OptimizationProblemTest, ReducesNoise) {
105  constexpr int kNumNodes = 100;
106  const transform::Rigid3d kSubmap0Transform = transform::Rigid3d::Identity();
107  const transform::Rigid3d kSubmap2Transform = transform::Rigid3d::Rotation(
108  Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitZ()));
109  const int kTrajectoryId = 0;
110 
111  struct NoisyNode {
112  transform::Rigid3d ground_truth_pose;
113  transform::Rigid3d noise;
114  };
115  std::vector<NoisyNode> test_data;
116  for (int j = 0; j != kNumNodes; ++j) {
117  test_data.push_back(
118  NoisyNode{RandomTransform(10., 3.), RandomYawOnlyTransform(0.2, 0.3)});
119  }
120 
122  for (const NoisyNode& node : test_data) {
123  const transform::Rigid3d pose =
124  AddNoise(node.ground_truth_pose, node.noise);
125  optimization_problem_.AddImuData(kTrajectoryId, now,
126  Eigen::Vector3d::UnitZ() * 9.81,
127  Eigen::Vector3d::Zero());
128  optimization_problem_.AddTrajectoryNode(kTrajectoryId, now, pose);
129  now += common::FromSeconds(0.01);
130  }
131 
132  std::vector<OptimizationProblem::Constraint> constraints;
133  for (int j = 0; j != kNumNodes; ++j) {
134  constraints.push_back(OptimizationProblem::Constraint{
135  mapping::SubmapId{0, 0}, mapping::NodeId{0, j},
136  OptimizationProblem::Constraint::Pose{
137  AddNoise(test_data[j].ground_truth_pose, test_data[j].noise), 1.,
138  1.}});
139  // We add an additional independent, but equally noisy observation.
140  constraints.push_back(OptimizationProblem::Constraint{
141  mapping::SubmapId{0, 1}, mapping::NodeId{0, j},
142  OptimizationProblem::Constraint::Pose{
143  AddNoise(test_data[j].ground_truth_pose,
144  RandomYawOnlyTransform(0.2, 0.3)),
145  1., 1.}});
146  // We add very noisy data with a low weight to verify it is mostly ignored.
147  constraints.push_back(OptimizationProblem::Constraint{
148  mapping::SubmapId{0, 2}, mapping::NodeId{0, j},
149  OptimizationProblem::Constraint::Pose{
150  kSubmap2Transform.inverse() * test_data[j].ground_truth_pose *
151  RandomTransform(1e3, 3.),
152  1e-9, 1e-9}});
153  }
154 
155  double translation_error_before = 0.;
156  double rotation_error_before = 0.;
157  const auto& node_data = optimization_problem_.node_data().at(0);
158  for (int j = 0; j != kNumNodes; ++j) {
159  translation_error_before += (test_data[j].ground_truth_pose.translation() -
160  node_data[j].point_cloud_pose.translation())
161  .norm();
162  rotation_error_before +=
163  transform::GetAngle(test_data[j].ground_truth_pose.inverse() *
164  node_data[j].point_cloud_pose);
165  }
166 
167  optimization_problem_.AddSubmap(kTrajectoryId, kSubmap0Transform);
168  optimization_problem_.AddSubmap(kTrajectoryId, kSubmap0Transform);
169  optimization_problem_.AddSubmap(kTrajectoryId, kSubmap2Transform);
170  optimization_problem_.Solve(constraints);
171 
172  double translation_error_after = 0.;
173  double rotation_error_after = 0.;
174  for (int j = 0; j != kNumNodes; ++j) {
175  translation_error_after += (test_data[j].ground_truth_pose.translation() -
176  node_data[j].point_cloud_pose.translation())
177  .norm();
178  rotation_error_after +=
179  transform::GetAngle(test_data[j].ground_truth_pose.inverse() *
180  node_data[j].point_cloud_pose);
181  }
182 
183  EXPECT_GT(0.8 * translation_error_before, translation_error_after);
184  EXPECT_GT(0.8 * rotation_error_before, rotation_error_after);
185 }
186 
187 } // namespace
188 } // namespace sparse_pose_graph
189 } // namespace mapping_3d
190 } // namespace cartographer
Eigen::Quaternion< T > AngleAxisVectorToRotationQuaternion(const Eigen::Matrix< T, 3, 1 > &angle_axis)
Definition: transform.h:86
static Rigid3 Rotation(const AngleAxis &angle_axis)
Rigid3< double > Rigid3d
std::mt19937 rng_
UniversalTimeScaleClock::time_point Time
Definition: time.h:44
transform::Rigid3d pose
std::unique_ptr< LuaParameterDictionary > MakeDictionary(const string &code)
Time FromUniversal(const int64 ticks)
Definition: time.cc:34
Duration FromSeconds(const double seconds)
Definition: time.cc:24
proto::OptimizationProblemOptions CreateOptimizationProblemOptions(common::LuaParameterDictionary *const parameter_dictionary)
OptimizationProblem optimization_problem_
FloatType GetAngle(const Rigid3< FloatType > &transform)
Definition: transform.h:34


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