12345qiupeng
/
cartographer
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Test PoseExtrapolator (#926)

master
gaschler 2018-02-21 16:43:58 +01:00 committed by Wally B. Feed
parent 7d13383dec
commit 2711f4492f
2 changed files with 182 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

181
cartographer/mapping/pose_extrapolator_test.cc Normal file
View File

@ -0,0 +1,181 @@
/*
* Copyright 2018 The Cartographer Authors
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#include "cartographer/mapping/pose_extrapolator.h"
#include "Eigen/Geometry"
#include "cartographer/common/make_unique.h"
#include "gtest/gtest.h"
#include "transform/rigid_transform_test_helpers.h"
namespace cartographer {
namespace mapping {
namespace {
constexpr double kPoseQueueDuration = 0.5f;
constexpr double kGravityTimeConstant = 0.1f;
constexpr double kExtrapolateDuration = 0.1f;
constexpr double kPrecision = 1e-8;
constexpr double kExtrapolatePrecision = 1e-2;
TEST(PoseExtrapolatorDeathTest, IncompleteInitialization) {
PoseExtrapolator extrapolator(common::FromSeconds(kPoseQueueDuration),
kGravityTimeConstant);
common::Time time = common::FromUniversal(123);
EXPECT_DEATH(extrapolator.EstimateGravityOrientation(time), "");
EXPECT_DEATH(extrapolator.ExtrapolatePose(time), "");
Eigen::Vector3d acceleration(0, 0, 9);
Eigen::Vector3d angular_velocity(0, 0, 0);
extrapolator.AddImuData(
sensor::ImuData{time, acceleration, angular_velocity});
EXPECT_DEATH(extrapolator.EstimateGravityOrientation(time), "");
EXPECT_DEATH(extrapolator.ExtrapolatePose(time), "");
}
TEST(PoseExtrapolatorDeathTest, ExtrapolateInPast) {
PoseExtrapolator extrapolator(common::FromSeconds(kPoseQueueDuration),
kGravityTimeConstant);
common::Time time_present = common::FromUniversal(123);
transform::Rigid3d pose =
transform::Rigid3d::Translation(Eigen::Vector3d(0.1, 0.2, 0.3));
extrapolator.AddPose(time_present, pose);
extrapolator.EstimateGravityOrientation(time_present);
EXPECT_THAT(extrapolator.ExtrapolatePose(time_present),
transform::IsNearly(pose, kPrecision));
common::Time time_in_past = time_present - common::FromSeconds(10);
EXPECT_DEATH(extrapolator.EstimateGravityOrientation(time_in_past), "");
EXPECT_DEATH(extrapolator.ExtrapolatePose(time_in_past), "");
common::Time time_in_future = time_present + common::FromSeconds(20);
extrapolator.ExtrapolatePose(time_in_future);
EXPECT_DEATH(extrapolator.ExtrapolatePose(time_present), "");
}
TEST(PoseExtrapolatorTest, EstimateGravityOrientationWithIMU) {
Eigen::Vector3d initial_gravity_acceleration(1.6, 2.0, 8.0);
Eigen::Vector3d angular_velocity(0, 0, 0);
common::Time current_time = common::FromUniversal(123);
sensor::ImuData imu_data{current_time, initial_gravity_acceleration,
angular_velocity};
auto extrapolator = PoseExtrapolator::InitializeWithImu(
common::FromSeconds(kPoseQueueDuration), kGravityTimeConstant, imu_data);
Eigen::Quaterniond expected_orientation;
expected_orientation.setFromTwoVectors(initial_gravity_acceleration,
Eigen::Vector3d::UnitZ());
EXPECT_NEAR(0.,
extrapolator->EstimateGravityOrientation(current_time)
.angularDistance(expected_orientation),
kPrecision);
Eigen::Vector3d gravity_acceleration(1.6, 2.0, 8.0);
for (int i = 0; i < 10; ++i) {
current_time += common::FromSeconds(kGravityTimeConstant);
extrapolator->AddImuData(
sensor::ImuData{current_time, gravity_acceleration, angular_velocity});
}
expected_orientation.setFromTwoVectors(gravity_acceleration,
Eigen::Vector3d::UnitZ());
EXPECT_NEAR(0.,
extrapolator->EstimateGravityOrientation(current_time)
.angularDistance(expected_orientation),
kPrecision);
}
TEST(PoseExtrapolatorTest, ExtrapolateWithPoses) {
PoseExtrapolator extrapolator(common::FromSeconds(kPoseQueueDuration),
kGravityTimeConstant);
common::Time current_time = common::FromUniversal(123);
transform::Rigid3d current_pose =
transform::Rigid3d::Translation(Eigen::Vector3d(0.3, 0.7, 0.2));
Eigen::Vector3d velocity(0, 0.1, 0);
Eigen::Vector3d angular_velocity(0.01, 0, 0.1);
transform::Rigid3d motion_per_second(
velocity, Eigen::AngleAxisd(angular_velocity.norm(),
angular_velocity.normalized()));
extrapolator.AddPose(current_time, current_pose);
EXPECT_EQ(common::ToUniversal(extrapolator.GetLastPoseTime()),
common::ToUniversal(current_time));
EXPECT_THAT(extrapolator.ExtrapolatePose(current_time),
transform::IsNearly(current_pose, kPrecision));
for (int i = 0; i < 5; ++i) {
current_time += common::FromSeconds(1);
current_pose = current_pose * motion_per_second;
extrapolator.AddPose(current_time, current_pose);
EXPECT_THAT(extrapolator.ExtrapolatePose(current_time),
transform::IsNearly(current_pose, kPrecision));
transform::Rigid3d expected_pose =
current_pose *
transform::Rigid3d(
kExtrapolateDuration * velocity,
Eigen::AngleAxisd(kExtrapolateDuration * angular_velocity.norm(),
angular_velocity.normalized()));
EXPECT_THAT(extrapolator.ExtrapolatePose(
current_time + common::FromSeconds(kExtrapolateDuration)),
transform::IsNearly(expected_pose, kExtrapolatePrecision));
EXPECT_EQ(common::ToUniversal(extrapolator.GetLastExtrapolatedTime()),
common::ToUniversal(current_time +
common::FromSeconds(kExtrapolateDuration)));
}
Eigen::AngleAxisd gravity_axis(
extrapolator.EstimateGravityOrientation(current_time));
EXPECT_NEAR(
0.f, (gravity_axis.axis().normalized() - Eigen::Vector3d::UnitZ()).norm(),
kExtrapolatePrecision);
}
TEST(PoseExtrapolatorTest, ExtrapolateWithIMU) {
Eigen::Vector3d initial_gravity_acceleration(0, 0, 9.8);
Eigen::Vector3d initial_angular_velocity(0, 0, 0);
common::Time current_time = common::FromUniversal(123);
sensor::ImuData imu_data{current_time, initial_gravity_acceleration,
initial_angular_velocity};
auto extrapolator = PoseExtrapolator::InitializeWithImu(
common::FromSeconds(kPoseQueueDuration), kGravityTimeConstant, imu_data);
transform::Rigid3d current_pose =
transform::Rigid3d::Translation(Eigen::Vector3d(0.3, 0.7, 0.2));
// Let velocity estimation come to rest.
for (int i = 0; i < kPoseQueueDuration + 2; ++i) {
current_time += common::FromSeconds(1);
extrapolator->AddImuData(sensor::ImuData{
current_time, initial_gravity_acceleration, initial_angular_velocity});
extrapolator->AddPose(current_time, current_pose);
}
Eigen::Vector3d angular_velocity_yaw(0, 0, 0.1);
transform::Rigid3d expected_pose = current_pose;
for (int i = 0; i < 3; ++i) {
for (int j = 0; j < 10; ++j) {
current_time += common::FromSeconds(kGravityTimeConstant);
extrapolator->AddImuData(sensor::ImuData{
current_time, initial_gravity_acceleration, angular_velocity_yaw});
Eigen::Quaterniond expected_rotation(
Eigen::AngleAxisd(kGravityTimeConstant * angular_velocity_yaw.norm(),
angular_velocity_yaw.normalized()));
expected_pose =
expected_pose * transform::Rigid3d::Rotation(expected_rotation);
}
EXPECT_THAT(extrapolator->ExtrapolatePose(current_time),
transform::IsNearly(expected_pose, kExtrapolatePrecision));
extrapolator->AddPose(current_time, expected_pose);
}
Eigen::AngleAxisd gravity_axis(
extrapolator->EstimateGravityOrientation(current_time));
EXPECT_NEAR(
0.f, (gravity_axis.axis().normalized() - Eigen::Vector3d::UnitZ()).norm(),
kPrecision);
}
} // namespace
} // namespace mapping
} // namespace cartographer

2
cartographer/transform/rigid_transform_test_helpers.h
View File

@ -40,7 +40,7 @@ Eigen::Transform<T, 3, Eigen::Affine> ToEigen(const Rigid3<T>& rigid3) {
}
MATCHER_P2(IsNearly, rigid, epsilon,
std::string(std::string(negation ? "isn't" : "is", " nearly ") +
std::string(std::string(negation ? "isn't nearly " : "is nearly ") +
rigid.DebugString())) {
return ToEigen(arg).isApprox(ToEigen(rigid), epsilon);
}