00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017 #include "cartographer/mapping/pose_extrapolator.h"
00018
00019 #include "Eigen/Geometry"
00020 #include "absl/memory/memory.h"
00021 #include "cartographer/transform/rigid_transform_test_helpers.h"
00022 #include "gtest/gtest.h"
00023
00024 namespace cartographer {
00025 namespace mapping {
00026 namespace {
00027
00028 constexpr double kPoseQueueDuration = 0.5f;
00029 constexpr double kGravityTimeConstant = 0.1f;
00030 constexpr double kExtrapolateDuration = 0.1f;
00031 constexpr double kPrecision = 1e-8;
00032 constexpr double kExtrapolatePrecision = 1e-2;
00033
00034 TEST(PoseExtrapolatorDeathTest, IncompleteInitialization) {
00035 PoseExtrapolator extrapolator(common::FromSeconds(kPoseQueueDuration),
00036 kGravityTimeConstant);
00037 common::Time time = common::FromUniversal(123);
00038 EXPECT_DEATH(extrapolator.EstimateGravityOrientation(time), "");
00039 EXPECT_DEATH(extrapolator.ExtrapolatePose(time), "");
00040 Eigen::Vector3d acceleration(0, 0, 9);
00041 Eigen::Vector3d angular_velocity(0, 0, 0);
00042 extrapolator.AddImuData(
00043 sensor::ImuData{time, acceleration, angular_velocity});
00044 EXPECT_DEATH(extrapolator.EstimateGravityOrientation(time), "");
00045 EXPECT_DEATH(extrapolator.ExtrapolatePose(time), "");
00046 }
00047
00048 TEST(PoseExtrapolatorDeathTest, ExtrapolateInPast) {
00049 PoseExtrapolator extrapolator(common::FromSeconds(kPoseQueueDuration),
00050 kGravityTimeConstant);
00051 common::Time time_present = common::FromUniversal(123);
00052 transform::Rigid3d pose =
00053 transform::Rigid3d::Translation(Eigen::Vector3d(0.1, 0.2, 0.3));
00054 extrapolator.AddPose(time_present, pose);
00055 extrapolator.EstimateGravityOrientation(time_present);
00056 EXPECT_THAT(extrapolator.ExtrapolatePose(time_present),
00057 transform::IsNearly(pose, kPrecision));
00058 common::Time time_in_past = time_present - common::FromSeconds(10);
00059 EXPECT_DEATH(extrapolator.EstimateGravityOrientation(time_in_past), "");
00060 EXPECT_DEATH(extrapolator.ExtrapolatePose(time_in_past), "");
00061 common::Time time_in_future = time_present + common::FromSeconds(20);
00062 extrapolator.ExtrapolatePose(time_in_future);
00063 EXPECT_DEATH(extrapolator.ExtrapolatePose(time_present), "");
00064 }
00065
00066 TEST(PoseExtrapolatorTest, EstimateGravityOrientationWithIMU) {
00067 Eigen::Vector3d initial_gravity_acceleration(1.6, 2.0, 8.0);
00068 Eigen::Vector3d angular_velocity(0, 0, 0);
00069 common::Time current_time = common::FromUniversal(123);
00070 sensor::ImuData imu_data{current_time, initial_gravity_acceleration,
00071 angular_velocity};
00072 auto extrapolator = PoseExtrapolator::InitializeWithImu(
00073 common::FromSeconds(kPoseQueueDuration), kGravityTimeConstant, imu_data);
00074 Eigen::Quaterniond expected_orientation;
00075 expected_orientation.setFromTwoVectors(initial_gravity_acceleration,
00076 Eigen::Vector3d::UnitZ());
00077 EXPECT_NEAR(0.,
00078 extrapolator->EstimateGravityOrientation(current_time)
00079 .angularDistance(expected_orientation),
00080 kPrecision);
00081 Eigen::Vector3d gravity_acceleration(1.6, 2.0, 8.0);
00082 for (int i = 0; i < 10; ++i) {
00083 current_time += common::FromSeconds(kGravityTimeConstant);
00084 extrapolator->AddImuData(
00085 sensor::ImuData{current_time, gravity_acceleration, angular_velocity});
00086 }
00087 expected_orientation.setFromTwoVectors(gravity_acceleration,
00088 Eigen::Vector3d::UnitZ());
00089 EXPECT_NEAR(0.,
00090 extrapolator->EstimateGravityOrientation(current_time)
00091 .angularDistance(expected_orientation),
00092 kPrecision);
00093 }
00094
00095 TEST(PoseExtrapolatorTest, ExtrapolateWithPoses) {
00096 PoseExtrapolator extrapolator(common::FromSeconds(kPoseQueueDuration),
00097 kGravityTimeConstant);
00098 common::Time current_time = common::FromUniversal(123);
00099 transform::Rigid3d current_pose =
00100 transform::Rigid3d::Translation(Eigen::Vector3d(0.3, 0.7, 0.2));
00101 Eigen::Vector3d velocity(0, 0.1, 0);
00102 Eigen::Vector3d angular_velocity(0.01, 0, 0.1);
00103 transform::Rigid3d motion_per_second(
00104 velocity, Eigen::AngleAxisd(angular_velocity.norm(),
00105 angular_velocity.normalized()));
00106 extrapolator.AddPose(current_time, current_pose);
00107 EXPECT_EQ(common::ToUniversal(extrapolator.GetLastPoseTime()),
00108 common::ToUniversal(current_time));
00109 EXPECT_THAT(extrapolator.ExtrapolatePose(current_time),
00110 transform::IsNearly(current_pose, kPrecision));
00111 for (int i = 0; i < 5; ++i) {
00112 current_time += common::FromSeconds(1);
00113 current_pose = current_pose * motion_per_second;
00114 extrapolator.AddPose(current_time, current_pose);
00115 EXPECT_THAT(extrapolator.ExtrapolatePose(current_time),
00116 transform::IsNearly(current_pose, kPrecision));
00117 transform::Rigid3d expected_pose =
00118 current_pose *
00119 transform::Rigid3d(
00120 kExtrapolateDuration * velocity,
00121 Eigen::AngleAxisd(kExtrapolateDuration * angular_velocity.norm(),
00122 angular_velocity.normalized()));
00123 EXPECT_THAT(extrapolator.ExtrapolatePose(
00124 current_time + common::FromSeconds(kExtrapolateDuration)),
00125 transform::IsNearly(expected_pose, kExtrapolatePrecision));
00126 EXPECT_EQ(common::ToUniversal(extrapolator.GetLastExtrapolatedTime()),
00127 common::ToUniversal(current_time +
00128 common::FromSeconds(kExtrapolateDuration)));
00129 }
00130 Eigen::AngleAxisd gravity_axis(
00131 extrapolator.EstimateGravityOrientation(current_time));
00132 EXPECT_NEAR(
00133 0.f, (gravity_axis.axis().normalized() - Eigen::Vector3d::UnitZ()).norm(),
00134 kExtrapolatePrecision);
00135 }
00136
00137 TEST(PoseExtrapolatorTest, ExtrapolateWithIMU) {
00138 Eigen::Vector3d initial_gravity_acceleration(0, 0, 9.8);
00139 Eigen::Vector3d initial_angular_velocity(0, 0, 0);
00140 common::Time current_time = common::FromUniversal(123);
00141 sensor::ImuData imu_data{current_time, initial_gravity_acceleration,
00142 initial_angular_velocity};
00143 auto extrapolator = PoseExtrapolator::InitializeWithImu(
00144 common::FromSeconds(kPoseQueueDuration), kGravityTimeConstant, imu_data);
00145 transform::Rigid3d current_pose =
00146 transform::Rigid3d::Translation(Eigen::Vector3d(0.3, 0.7, 0.2));
00147
00148 for (int i = 0; i < kPoseQueueDuration + 2; ++i) {
00149 current_time += common::FromSeconds(1);
00150 extrapolator->AddImuData(sensor::ImuData{
00151 current_time, initial_gravity_acceleration, initial_angular_velocity});
00152 extrapolator->AddPose(current_time, current_pose);
00153 }
00154
00155 Eigen::Vector3d angular_velocity_yaw(0, 0, 0.1);
00156 transform::Rigid3d expected_pose = current_pose;
00157 for (int i = 0; i < 3; ++i) {
00158 for (int j = 0; j < 10; ++j) {
00159 current_time += common::FromSeconds(kGravityTimeConstant);
00160 extrapolator->AddImuData(sensor::ImuData{
00161 current_time, initial_gravity_acceleration, angular_velocity_yaw});
00162 Eigen::Quaterniond expected_rotation(
00163 Eigen::AngleAxisd(kGravityTimeConstant * angular_velocity_yaw.norm(),
00164 angular_velocity_yaw.normalized()));
00165 expected_pose =
00166 expected_pose * transform::Rigid3d::Rotation(expected_rotation);
00167 }
00168 EXPECT_THAT(extrapolator->ExtrapolatePose(current_time),
00169 transform::IsNearly(expected_pose, kExtrapolatePrecision));
00170 extrapolator->AddPose(current_time, expected_pose);
00171 }
00172 Eigen::AngleAxisd gravity_axis(
00173 extrapolator->EstimateGravityOrientation(current_time));
00174 EXPECT_NEAR(
00175 0.f, (gravity_axis.axis().normalized() - Eigen::Vector3d::UnitZ()).norm(),
00176 kPrecision);
00177 }
00178
00179 }
00180 }
00181 }