From 32dad63edd74e8e951b8e181bae6acc244367e00 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Martin=20Schw=C3=B6rer?= Date: Wed, 29 Jul 2020 14:08:40 +0200 Subject: [PATCH] extracted eigen quaterniond from 2 vectors for faster compilation (#1729) Signed-off-by: mschworer --- .../eigen_quaterniond_from_two_vectors.cc | 28 +++++++++++++++ .../eigen_quaterniond_from_two_vectors.h | 34 +++++++++++++++++++ cartographer/mapping/imu_tracker.cc | 3 +- cartographer/mapping/imu_tracker_test.cc | 6 ++-- .../mapping/pose_extrapolator_test.cc | 10 +++--- 5 files changed, 72 insertions(+), 9 deletions(-) create mode 100644 cartographer/mapping/eigen_quaterniond_from_two_vectors.cc create mode 100644 cartographer/mapping/eigen_quaterniond_from_two_vectors.h diff --git a/cartographer/mapping/eigen_quaterniond_from_two_vectors.cc b/cartographer/mapping/eigen_quaterniond_from_two_vectors.cc new file mode 100644 index 0000000..af0aac7 --- /dev/null +++ b/cartographer/mapping/eigen_quaterniond_from_two_vectors.cc @@ -0,0 +1,28 @@ +/* + * 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/eigen_quaterniond_from_two_vectors.h" + +namespace cartographer { +namespace mapping { + +Eigen::Quaterniond FromTwoVectors(const Eigen::Vector3d& a, + const Eigen::Vector3d& b) { + return Eigen::Quaterniond::FromTwoVectors(a, b); +} + +} // namespace mapping +} // namespace cartographer diff --git a/cartographer/mapping/eigen_quaterniond_from_two_vectors.h b/cartographer/mapping/eigen_quaterniond_from_two_vectors.h new file mode 100644 index 0000000..e8d04b3 --- /dev/null +++ b/cartographer/mapping/eigen_quaterniond_from_two_vectors.h @@ -0,0 +1,34 @@ +/* + * 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. + */ + +#ifndef CARTOGRAPHER_MAPPING_EIGEN_QUATERNIOND_FROM_TWO_VECTORS_H_ +#define CARTOGRAPHER_MAPPING_EIGEN_QUATERNIOND_FROM_TWO_VECTORS_H_ + +#include "Eigen/Geometry" + +namespace cartographer { +namespace mapping { + +// Calls Eigen::Quaterniond::FromTwoVectors(). This is in its own compilation +// unit since it can take more than 10 s to build while using more than 1 GB of +// memory causing slow build times and high peak memory usage. +Eigen::Quaterniond FromTwoVectors(const Eigen::Vector3d& a, + const Eigen::Vector3d& b); + +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_EIGEN_QUATERNIOND_FROM_TWO_VECTORS_H_ diff --git a/cartographer/mapping/imu_tracker.cc b/cartographer/mapping/imu_tracker.cc index 39cac08..45391d0 100644 --- a/cartographer/mapping/imu_tracker.cc +++ b/cartographer/mapping/imu_tracker.cc @@ -20,6 +20,7 @@ #include #include "cartographer/common/math.h" +#include "cartographer/mapping/eigen_quaterniond_from_two_vectors.h" #include "cartographer/transform/transform.h" #include "glog/logging.h" @@ -60,7 +61,7 @@ void ImuTracker::AddImuLinearAccelerationObservation( (1. - alpha) * gravity_vector_ + alpha * imu_linear_acceleration; // Change the 'orientation_' so that it agrees with the current // 'gravity_vector_'. - const Eigen::Quaterniond rotation = Eigen::Quaterniond::FromTwoVectors( + const Eigen::Quaterniond rotation = FromTwoVectors( gravity_vector_, orientation_.conjugate() * Eigen::Vector3d::UnitZ()); orientation_ = (orientation_ * rotation).normalized(); CHECK_GT((orientation_ * gravity_vector_).z(), 0.); diff --git a/cartographer/mapping/imu_tracker_test.cc b/cartographer/mapping/imu_tracker_test.cc index 8f3b452..88ae846 100644 --- a/cartographer/mapping/imu_tracker_test.cc +++ b/cartographer/mapping/imu_tracker_test.cc @@ -17,6 +17,7 @@ #include "cartographer/mapping/imu_tracker.h" #include "absl/memory/memory.h" +#include "cartographer/mapping/eigen_quaterniond_from_two_vectors.h" #include "gtest/gtest.h" namespace cartographer { @@ -82,9 +83,8 @@ TEST_F(ImuTrackerTest, IntegrateFullRotation) { TEST_F(ImuTrackerTest, LearnGravityVector) { linear_acceleration_ = Eigen::Vector3d(0.5, 0.3, 9.5); AdvanceImu(); - Eigen::Quaterniond expected_orientation; - expected_orientation.setFromTwoVectors(linear_acceleration_, - Eigen::Vector3d::UnitZ()); + const Eigen::Quaterniond expected_orientation = + FromTwoVectors(linear_acceleration_, Eigen::Vector3d::UnitZ()); EXPECT_NEAR(0., imu_tracker_->orientation().angularDistance(expected_orientation), kPrecision); diff --git a/cartographer/mapping/pose_extrapolator_test.cc b/cartographer/mapping/pose_extrapolator_test.cc index fe17e1b..38afc84 100644 --- a/cartographer/mapping/pose_extrapolator_test.cc +++ b/cartographer/mapping/pose_extrapolator_test.cc @@ -18,6 +18,7 @@ #include "Eigen/Geometry" #include "absl/memory/memory.h" +#include "cartographer/mapping/eigen_quaterniond_from_two_vectors.h" #include "cartographer/transform/rigid_transform_test_helpers.h" #include "gtest/gtest.h" @@ -71,9 +72,8 @@ TEST(PoseExtrapolatorTest, EstimateGravityOrientationWithIMU) { 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()); + Eigen::Quaterniond expected_orientation = + FromTwoVectors(initial_gravity_acceleration, Eigen::Vector3d::UnitZ()); EXPECT_NEAR(0., extrapolator->EstimateGravityOrientation(current_time) .angularDistance(expected_orientation), @@ -84,8 +84,8 @@ TEST(PoseExtrapolatorTest, EstimateGravityOrientationWithIMU) { extrapolator->AddImuData( sensor::ImuData{current_time, gravity_acceleration, angular_velocity}); } - expected_orientation.setFromTwoVectors(gravity_acceleration, - Eigen::Vector3d::UnitZ()); + expected_orientation = + FromTwoVectors(gravity_acceleration, Eigen::Vector3d::UnitZ()); EXPECT_NEAR(0., extrapolator->EstimateGravityOrientation(current_time) .angularDistance(expected_orientation),