pose_extrapolator_test.cc
Go to the documentation of this file.
1 /*
2  * Copyright 2018 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/mapping/pose_extrapolator.h"
18 
19 #include "Eigen/Geometry"
20 #include "cartographer/common/make_unique.h"
21 #include "gtest/gtest.h"
22 #include "transform/rigid_transform_test_helpers.h"
23 
24 namespace cartographer {
25 namespace mapping {
26 namespace {
27 
28 constexpr double kPoseQueueDuration = 0.5f;
29 constexpr double kGravityTimeConstant = 0.1f;
30 constexpr double kExtrapolateDuration = 0.1f;
31 constexpr double kPrecision = 1e-8;
32 constexpr double kExtrapolatePrecision = 1e-2;
33 
34 TEST(PoseExtrapolatorDeathTest, IncompleteInitialization) {
35  PoseExtrapolator extrapolator(common::FromSeconds(kPoseQueueDuration),
36  kGravityTimeConstant);
37  common::Time time = common::FromUniversal(123);
38  EXPECT_DEATH(extrapolator.EstimateGravityOrientation(time), "");
39  EXPECT_DEATH(extrapolator.ExtrapolatePose(time), "");
40  Eigen::Vector3d acceleration(0, 0, 9);
41  Eigen::Vector3d angular_velocity(0, 0, 0);
42  extrapolator.AddImuData(
43  sensor::ImuData{time, acceleration, angular_velocity});
44  EXPECT_DEATH(extrapolator.EstimateGravityOrientation(time), "");
45  EXPECT_DEATH(extrapolator.ExtrapolatePose(time), "");
46 }
47 
48 TEST(PoseExtrapolatorDeathTest, ExtrapolateInPast) {
49  PoseExtrapolator extrapolator(common::FromSeconds(kPoseQueueDuration),
50  kGravityTimeConstant);
51  common::Time time_present = common::FromUniversal(123);
52  transform::Rigid3d pose =
53  transform::Rigid3d::Translation(Eigen::Vector3d(0.1, 0.2, 0.3));
54  extrapolator.AddPose(time_present, pose);
55  extrapolator.EstimateGravityOrientation(time_present);
56  EXPECT_THAT(extrapolator.ExtrapolatePose(time_present),
57  transform::IsNearly(pose, kPrecision));
58  common::Time time_in_past = time_present - common::FromSeconds(10);
59  EXPECT_DEATH(extrapolator.EstimateGravityOrientation(time_in_past), "");
60  EXPECT_DEATH(extrapolator.ExtrapolatePose(time_in_past), "");
61  common::Time time_in_future = time_present + common::FromSeconds(20);
62  extrapolator.ExtrapolatePose(time_in_future);
63  EXPECT_DEATH(extrapolator.ExtrapolatePose(time_present), "");
64 }
65 
66 TEST(PoseExtrapolatorTest, EstimateGravityOrientationWithIMU) {
67  Eigen::Vector3d initial_gravity_acceleration(1.6, 2.0, 8.0);
68  Eigen::Vector3d angular_velocity(0, 0, 0);
69  common::Time current_time = common::FromUniversal(123);
70  sensor::ImuData imu_data{current_time, initial_gravity_acceleration,
71  angular_velocity};
72  auto extrapolator = PoseExtrapolator::InitializeWithImu(
73  common::FromSeconds(kPoseQueueDuration), kGravityTimeConstant, imu_data);
74  Eigen::Quaterniond expected_orientation;
75  expected_orientation.setFromTwoVectors(initial_gravity_acceleration,
76  Eigen::Vector3d::UnitZ());
77  EXPECT_NEAR(0.,
78  extrapolator->EstimateGravityOrientation(current_time)
79  .angularDistance(expected_orientation),
80  kPrecision);
81  Eigen::Vector3d gravity_acceleration(1.6, 2.0, 8.0);
82  for (int i = 0; i < 10; ++i) {
83  current_time += common::FromSeconds(kGravityTimeConstant);
84  extrapolator->AddImuData(
85  sensor::ImuData{current_time, gravity_acceleration, angular_velocity});
86  }
87  expected_orientation.setFromTwoVectors(gravity_acceleration,
88  Eigen::Vector3d::UnitZ());
89  EXPECT_NEAR(0.,
90  extrapolator->EstimateGravityOrientation(current_time)
91  .angularDistance(expected_orientation),
92  kPrecision);
93 }
94 
95 TEST(PoseExtrapolatorTest, ExtrapolateWithPoses) {
96  PoseExtrapolator extrapolator(common::FromSeconds(kPoseQueueDuration),
97  kGravityTimeConstant);
98  common::Time current_time = common::FromUniversal(123);
99  transform::Rigid3d current_pose =
100  transform::Rigid3d::Translation(Eigen::Vector3d(0.3, 0.7, 0.2));
101  Eigen::Vector3d velocity(0, 0.1, 0);
102  Eigen::Vector3d angular_velocity(0.01, 0, 0.1);
103  transform::Rigid3d motion_per_second(
104  velocity, Eigen::AngleAxisd(angular_velocity.norm(),
105  angular_velocity.normalized()));
106  extrapolator.AddPose(current_time, current_pose);
107  EXPECT_EQ(common::ToUniversal(extrapolator.GetLastPoseTime()),
108  common::ToUniversal(current_time));
109  EXPECT_THAT(extrapolator.ExtrapolatePose(current_time),
110  transform::IsNearly(current_pose, kPrecision));
111  for (int i = 0; i < 5; ++i) {
112  current_time += common::FromSeconds(1);
113  current_pose = current_pose * motion_per_second;
114  extrapolator.AddPose(current_time, current_pose);
115  EXPECT_THAT(extrapolator.ExtrapolatePose(current_time),
116  transform::IsNearly(current_pose, kPrecision));
117  transform::Rigid3d expected_pose =
118  current_pose *
119  transform::Rigid3d(
120  kExtrapolateDuration * velocity,
121  Eigen::AngleAxisd(kExtrapolateDuration * angular_velocity.norm(),
122  angular_velocity.normalized()));
123  EXPECT_THAT(extrapolator.ExtrapolatePose(
124  current_time + common::FromSeconds(kExtrapolateDuration)),
125  transform::IsNearly(expected_pose, kExtrapolatePrecision));
126  EXPECT_EQ(common::ToUniversal(extrapolator.GetLastExtrapolatedTime()),
127  common::ToUniversal(current_time +
128  common::FromSeconds(kExtrapolateDuration)));
129  }
130  Eigen::AngleAxisd gravity_axis(
131  extrapolator.EstimateGravityOrientation(current_time));
132  EXPECT_NEAR(
133  0.f, (gravity_axis.axis().normalized() - Eigen::Vector3d::UnitZ()).norm(),
134  kExtrapolatePrecision);
135 }
136 
137 TEST(PoseExtrapolatorTest, ExtrapolateWithIMU) {
138  Eigen::Vector3d initial_gravity_acceleration(0, 0, 9.8);
139  Eigen::Vector3d initial_angular_velocity(0, 0, 0);
140  common::Time current_time = common::FromUniversal(123);
141  sensor::ImuData imu_data{current_time, initial_gravity_acceleration,
142  initial_angular_velocity};
143  auto extrapolator = PoseExtrapolator::InitializeWithImu(
144  common::FromSeconds(kPoseQueueDuration), kGravityTimeConstant, imu_data);
145  transform::Rigid3d current_pose =
146  transform::Rigid3d::Translation(Eigen::Vector3d(0.3, 0.7, 0.2));
147  // Let velocity estimation come to rest.
148  for (int i = 0; i < kPoseQueueDuration + 2; ++i) {
149  current_time += common::FromSeconds(1);
150  extrapolator->AddImuData(sensor::ImuData{
151  current_time, initial_gravity_acceleration, initial_angular_velocity});
152  extrapolator->AddPose(current_time, current_pose);
153  }
154 
155  Eigen::Vector3d angular_velocity_yaw(0, 0, 0.1);
156  transform::Rigid3d expected_pose = current_pose;
157  for (int i = 0; i < 3; ++i) {
158  for (int j = 0; j < 10; ++j) {
159  current_time += common::FromSeconds(kGravityTimeConstant);
160  extrapolator->AddImuData(sensor::ImuData{
161  current_time, initial_gravity_acceleration, angular_velocity_yaw});
162  Eigen::Quaterniond expected_rotation(
163  Eigen::AngleAxisd(kGravityTimeConstant * angular_velocity_yaw.norm(),
164  angular_velocity_yaw.normalized()));
165  expected_pose =
166  expected_pose * transform::Rigid3d::Rotation(expected_rotation);
167  }
168  EXPECT_THAT(extrapolator->ExtrapolatePose(current_time),
169  transform::IsNearly(expected_pose, kExtrapolatePrecision));
170  extrapolator->AddPose(current_time, expected_pose);
171  }
172  Eigen::AngleAxisd gravity_axis(
173  extrapolator->EstimateGravityOrientation(current_time));
174  EXPECT_NEAR(
175  0.f, (gravity_axis.axis().normalized() - Eigen::Vector3d::UnitZ()).norm(),
176  kPrecision);
177 }
178 
179 } // namespace
180 } // namespace mapping
181 } // namespace cartographer
rigid_transform_test_helpers.h
cartographer::transform::Rigid3< double >::Rotation
static Rigid3 Rotation(const AngleAxis &angle_axis)
Definition: rigid_transform.h:135
pose_extrapolator.h
cartographer::transform::Rigid3d
Rigid3< double > Rigid3d
Definition: rigid_transform.h:225
cartographer::mapping::PoseExtrapolator::InitializeWithImu
static std::unique_ptr< PoseExtrapolator > InitializeWithImu(common::Duration pose_queue_duration, double imu_gravity_time_constant, const sensor::ImuData &imu_data)
Definition: pose_extrapolator.cc:35
cartographer::common::Time
UniversalTimeScaleClock::time_point Time
Definition: time.h:44
cartographer::common::FromUniversal
Time FromUniversal(const int64 ticks)
Definition: time.cc:34
time
static time_point time
Definition: rate_timer_test.cc:41
cartographer::common::FromSeconds
Duration FromSeconds(const double seconds)
Definition: time.cc:24
cartographer::common::ToUniversal
int64 ToUniversal(const Time time)
Definition: time.cc:36
make_unique.h
pose
transform::Rigid3d pose
Definition: local_trajectory_builder_3d_test.cc:44
cartographer::transform::Rigid3< double >::Translation
static Rigid3 Translation(const Vector &vector)
Definition: rigid_transform.h:143
cartographer::mapping::TEST
TEST(TrajectoryConnectivityStateTest, UnknownTrajectory)
Definition: trajectory_connectivity_state_test.cc:24

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