pose_tracker_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 
17 #include "cartographer/kalman_filter/pose_tracker.h"
18 
19 #include <random>
20 
21 #include "cartographer/common/lua_parameter_dictionary.h"
22 #include "cartographer/common/lua_parameter_dictionary_test_helpers.h"
23 #include "cartographer/common/make_unique.h"
24 #include "cartographer/transform/rigid_transform.h"
25 #include "cartographer/transform/rigid_transform_test_helpers.h"
26 #include "gtest/gtest.h"
27 
28 namespace cartographer {
29 namespace kalman_filter {
30 namespace {
31 
32 constexpr double kOdometerVariance = 1e-12;
33 
34 using transform::IsNearly;
35 using transform::Rigid3d;
36 using ::testing::Not;
37 
38 class PoseTrackerTest : public ::testing::Test {
39  protected:
40  PoseTrackerTest() {
41  auto parameter_dictionary = common::MakeDictionary(R"text(
42  return {
43  orientation_model_variance = 1e-8,
44  position_model_variance = 1e-8,
45  velocity_model_variance = 1e-8,
46  imu_gravity_time_constant = 100.,
47  imu_gravity_variance = 1e-9,
48  num_odometry_states = 1,
49  }
50  )text");
51  const proto::PoseTrackerOptions options =
52  CreatePoseTrackerOptions(parameter_dictionary.get());
53  pose_tracker_ =
54  common::make_unique<PoseTracker>(options, common::FromUniversal(1000));
55  }
56 
57  std::unique_ptr<PoseTracker> pose_tracker_;
58 };
59 
60 TEST_F(PoseTrackerTest, SaveAndRestore) {
61  std::vector<Rigid3d> poses(3);
62  std::vector<PoseCovariance> covariances(3);
63  pose_tracker_->GetPoseEstimateMeanAndCovariance(common::FromUniversal(1500),
64  &poses[0], &covariances[0]);
65 
66  pose_tracker_->AddImuLinearAccelerationObservation(
67  common::FromUniversal(2000), Eigen::Vector3d(1, 1, 9));
68 
69  PoseTracker copy_of_pose_tracker = *pose_tracker_;
70 
71  const Eigen::Vector3d observation(2, 0, 8);
72  pose_tracker_->AddImuLinearAccelerationObservation(
73  common::FromUniversal(3000), observation);
74 
75  pose_tracker_->GetPoseEstimateMeanAndCovariance(common::FromUniversal(3500),
76  &poses[1], &covariances[1]);
77 
78  copy_of_pose_tracker.AddImuLinearAccelerationObservation(
79  common::FromUniversal(3000), observation);
80  copy_of_pose_tracker.GetPoseEstimateMeanAndCovariance(
81  common::FromUniversal(3500), &poses[2], &covariances[2]);
82 
83  EXPECT_THAT(poses[0], Not(IsNearly(poses[1], 1e-6)));
84  EXPECT_FALSE((covariances[0].array() == covariances[1].array()).all());
85  EXPECT_THAT(poses[1], IsNearly(poses[2], 1e-6));
86  EXPECT_TRUE((covariances[1].array() == covariances[2].array()).all());
87 }
88 
89 TEST_F(PoseTrackerTest, AddImuLinearAccelerationObservation) {
90  auto time = common::FromUniversal(1000);
91 
92  for (int i = 0; i < 300; ++i) {
93  time += std::chrono::seconds(5);
94  pose_tracker_->AddImuLinearAccelerationObservation(
95  time, Eigen::Vector3d(0., 0., 10.));
96  }
97 
98  {
99  Rigid3d pose;
100  PoseCovariance covariance;
101  pose_tracker_->GetPoseEstimateMeanAndCovariance(time, &pose, &covariance);
102  const Eigen::Quaterniond actual = Eigen::Quaterniond(pose.rotation());
103  const Eigen::Quaterniond expected = Eigen::Quaterniond::Identity();
104  EXPECT_TRUE(actual.isApprox(expected, 1e-3)) << expected.coeffs() << " vs\n"
105  << actual.coeffs();
106  }
107 
108  for (int i = 0; i < 300; ++i) {
109  time += std::chrono::seconds(5);
110  pose_tracker_->AddImuLinearAccelerationObservation(
111  time, Eigen::Vector3d(0., 10., 0.));
112  }
113 
114  time += std::chrono::milliseconds(5);
115 
116  Rigid3d pose;
117  PoseCovariance covariance;
118  pose_tracker_->GetPoseEstimateMeanAndCovariance(time, &pose, &covariance);
119  const Eigen::Quaterniond actual = Eigen::Quaterniond(pose.rotation());
120  const Eigen::Quaterniond expected = Eigen::Quaterniond(
121  Eigen::AngleAxisd(M_PI / 2., Eigen::Vector3d::UnitX()));
122  EXPECT_TRUE(actual.isApprox(expected, 1e-3)) << expected.coeffs() << " vs\n"
123  << actual.coeffs();
124 }
125 
126 TEST_F(PoseTrackerTest, AddImuAngularVelocityObservation) {
127  auto time = common::FromUniversal(1000);
128 
129  for (int i = 0; i < 300; ++i) {
130  time += std::chrono::milliseconds(5);
131  pose_tracker_->AddImuAngularVelocityObservation(time,
132  Eigen::Vector3d::Zero());
133  }
134 
135  {
136  Rigid3d pose;
137  PoseCovariance covariance;
138  pose_tracker_->GetPoseEstimateMeanAndCovariance(time, &pose, &covariance);
139  const Eigen::Quaterniond actual = Eigen::Quaterniond(pose.rotation());
140  const Eigen::Quaterniond expected = Eigen::Quaterniond::Identity();
141  EXPECT_TRUE(actual.isApprox(expected, 1e-3)) << expected.coeffs() << " vs\n"
142  << actual.coeffs();
143  }
144 
145  const double target_radians = M_PI / 2.;
146  const double num_observations = 300.;
147  const double angular_velocity = target_radians / (num_observations * 5e-3);
148  for (int i = 0; i < num_observations; ++i) {
149  time += std::chrono::milliseconds(5);
150  pose_tracker_->AddImuAngularVelocityObservation(
151  time, Eigen::Vector3d(angular_velocity, 0., 0.));
152  }
153 
154  time += std::chrono::milliseconds(5);
155 
156  Rigid3d pose;
157  PoseCovariance covariance;
158  pose_tracker_->GetPoseEstimateMeanAndCovariance(time, &pose, &covariance);
159  const Eigen::Quaterniond actual = Eigen::Quaterniond(pose.rotation());
160  const Eigen::Quaterniond expected = Eigen::Quaterniond(
161  Eigen::AngleAxisd(M_PI / 2., Eigen::Vector3d::UnitX()));
162  EXPECT_TRUE(actual.isApprox(expected, 1e-3)) << expected.coeffs() << " vs\n"
163  << actual.coeffs();
164 }
165 
166 TEST_F(PoseTrackerTest, AddPoseObservation) {
167  auto time = common::FromUniversal(1000);
168 
169  for (int i = 0; i < 300; ++i) {
170  time += std::chrono::milliseconds(5);
171  pose_tracker_->AddPoseObservation(
172  time, Rigid3d::Identity(),
173  Eigen::Matrix<double, 6, 6>::Identity() * 1e-6);
174  }
175 
176  {
177  Rigid3d actual;
178  PoseCovariance covariance;
179  pose_tracker_->GetPoseEstimateMeanAndCovariance(time, &actual, &covariance);
180  EXPECT_THAT(actual, IsNearly(Rigid3d::Identity(), 1e-3));
181  }
182 
183  const Rigid3d expected =
184  Rigid3d::Translation(Eigen::Vector3d(1., 2., 3.)) *
185  Rigid3d::Rotation(Eigen::AngleAxisd(
186  M_PI / 2., Eigen::Vector3d(0., 0., 3.).normalized()));
187 
188  for (int i = 0; i < 300; ++i) {
189  time += std::chrono::milliseconds(15);
190  pose_tracker_->AddPoseObservation(
191  time, expected, Eigen::Matrix<double, 6, 6>::Identity() * 1e-9);
192  }
193 
194  time += std::chrono::milliseconds(15);
195 
196  Rigid3d actual;
197  PoseCovariance covariance;
198  pose_tracker_->GetPoseEstimateMeanAndCovariance(time, &actual, &covariance);
199  EXPECT_THAT(actual, IsNearly(expected, 1e-3));
200 }
201 
202 TEST_F(PoseTrackerTest, AddOdometerPoseObservation) {
203  common::Time time = common::FromUniversal(0);
204 
205  std::vector<Rigid3d> odometer_track;
206  odometer_track.push_back(Rigid3d::Identity());
207  odometer_track.push_back(
208  Rigid3d::Rotation(Eigen::AngleAxisd(0.3, Eigen::Vector3d::UnitZ())));
209  odometer_track.push_back(
210  Rigid3d::Translation(Eigen::Vector3d(0.2, 0., 0.)) *
211  Rigid3d::Rotation(Eigen::AngleAxisd(0.3, Eigen::Vector3d::UnitZ())));
212  odometer_track.push_back(
213  Rigid3d::Translation(Eigen::Vector3d(0.3, 0.1, 0.)) *
214  Rigid3d::Rotation(Eigen::AngleAxisd(0.6, Eigen::Vector3d::UnitZ())));
215  odometer_track.push_back(
216  Rigid3d::Translation(Eigen::Vector3d(0.2, 0.2, 0.1)) *
217  Rigid3d::Rotation(Eigen::AngleAxisd(0.3, Eigen::Vector3d::UnitZ())));
218  odometer_track.push_back(
219  Rigid3d::Translation(Eigen::Vector3d(0.1, 0.2, 0.2)) *
220  Rigid3d::Rotation(Eigen::AngleAxisd(0.6, Eigen::Vector3d::UnitZ())));
221  odometer_track.push_back(Rigid3d::Translation(Eigen::Vector3d(0., 0.1, 0.2)));
222 
223  Rigid3d actual;
224  PoseCovariance unused_covariance;
225  for (const Rigid3d& pose : odometer_track) {
226  time += std::chrono::seconds(1);
227  pose_tracker_->AddOdometerPoseObservation(
228  time, pose, kOdometerVariance * PoseCovariance::Identity());
229  pose_tracker_->GetPoseEstimateMeanAndCovariance(time, &actual,
230  &unused_covariance);
231  EXPECT_THAT(actual, IsNearly(pose, 1e-2));
232  }
233  // Sanity check that the test has signal:
234  EXPECT_THAT(actual, Not(IsNearly(odometer_track[0], 1e-2)));
235 }
236 
237 } // namespace
238 } // namespace kalman_filter
239 } // namespace cartographer
rigid_transform_test_helpers.h
cartographer::kalman_filter::CreatePoseTrackerOptions
proto::PoseTrackerOptions CreatePoseTrackerOptions(common::LuaParameterDictionary *const parameter_dictionary)
Definition: pose_tracker.cc:125
cartographer::transform::Rigid3d
Rigid3< double > Rigid3d
Definition: rigid_transform.h:217
cartographer::kalman_filter::PoseCovariance
Eigen::Matrix< double, 6, 6 > PoseCovariance
Definition: pose_tracker.h:40
pose_tracker.h
cartographer::common::Time
UniversalTimeScaleClock::time_point Time
Definition: time.h:44
pose
transform::Rigid3d pose
Definition: kalman_local_trajectory_builder_test.cc:42
cartographer::common::MakeDictionary
std::unique_ptr< LuaParameterDictionary > MakeDictionary(const string &code)
Definition: lua_parameter_dictionary_test_helpers.h:49
time
common::Time time
Definition: kalman_local_trajectory_builder_test.cc:41
cartographer::common::FromUniversal
Time FromUniversal(const int64 ticks)
Definition: time.cc:34
lua_parameter_dictionary.h
lua_parameter_dictionary_test_helpers.h
make_unique.h
rigid_transform.h
pose_tracker_
std::unique_ptr< PoseTracker > pose_tracker_
Definition: pose_tracker_test.cc:57

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