extracted eigen quaterniond from 2 vectors for faster compilation (#1729)

Signed-off-by: mschworer <mschworer@lyft.com>
master
Martin Schwörer 2020-07-29 14:08:40 +02:00 committed by GitHub
parent 1cae11593d
commit 32dad63edd
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
5 changed files with 72 additions and 9 deletions

View File

@ -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

View File

@ -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_

View File

@ -20,6 +20,7 @@
#include <limits> #include <limits>
#include "cartographer/common/math.h" #include "cartographer/common/math.h"
#include "cartographer/mapping/eigen_quaterniond_from_two_vectors.h"
#include "cartographer/transform/transform.h" #include "cartographer/transform/transform.h"
#include "glog/logging.h" #include "glog/logging.h"
@ -60,7 +61,7 @@ void ImuTracker::AddImuLinearAccelerationObservation(
(1. - alpha) * gravity_vector_ + alpha * imu_linear_acceleration; (1. - alpha) * gravity_vector_ + alpha * imu_linear_acceleration;
// Change the 'orientation_' so that it agrees with the current // Change the 'orientation_' so that it agrees with the current
// 'gravity_vector_'. // 'gravity_vector_'.
const Eigen::Quaterniond rotation = Eigen::Quaterniond::FromTwoVectors( const Eigen::Quaterniond rotation = FromTwoVectors(
gravity_vector_, orientation_.conjugate() * Eigen::Vector3d::UnitZ()); gravity_vector_, orientation_.conjugate() * Eigen::Vector3d::UnitZ());
orientation_ = (orientation_ * rotation).normalized(); orientation_ = (orientation_ * rotation).normalized();
CHECK_GT((orientation_ * gravity_vector_).z(), 0.); CHECK_GT((orientation_ * gravity_vector_).z(), 0.);

View File

@ -17,6 +17,7 @@
#include "cartographer/mapping/imu_tracker.h" #include "cartographer/mapping/imu_tracker.h"
#include "absl/memory/memory.h" #include "absl/memory/memory.h"
#include "cartographer/mapping/eigen_quaterniond_from_two_vectors.h"
#include "gtest/gtest.h" #include "gtest/gtest.h"
namespace cartographer { namespace cartographer {
@ -82,9 +83,8 @@ TEST_F(ImuTrackerTest, IntegrateFullRotation) {
TEST_F(ImuTrackerTest, LearnGravityVector) { TEST_F(ImuTrackerTest, LearnGravityVector) {
linear_acceleration_ = Eigen::Vector3d(0.5, 0.3, 9.5); linear_acceleration_ = Eigen::Vector3d(0.5, 0.3, 9.5);
AdvanceImu(); AdvanceImu();
Eigen::Quaterniond expected_orientation; const Eigen::Quaterniond expected_orientation =
expected_orientation.setFromTwoVectors(linear_acceleration_, FromTwoVectors(linear_acceleration_, Eigen::Vector3d::UnitZ());
Eigen::Vector3d::UnitZ());
EXPECT_NEAR(0., EXPECT_NEAR(0.,
imu_tracker_->orientation().angularDistance(expected_orientation), imu_tracker_->orientation().angularDistance(expected_orientation),
kPrecision); kPrecision);

View File

@ -18,6 +18,7 @@
#include "Eigen/Geometry" #include "Eigen/Geometry"
#include "absl/memory/memory.h" #include "absl/memory/memory.h"
#include "cartographer/mapping/eigen_quaterniond_from_two_vectors.h"
#include "cartographer/transform/rigid_transform_test_helpers.h" #include "cartographer/transform/rigid_transform_test_helpers.h"
#include "gtest/gtest.h" #include "gtest/gtest.h"
@ -71,9 +72,8 @@ TEST(PoseExtrapolatorTest, EstimateGravityOrientationWithIMU) {
angular_velocity}; angular_velocity};
auto extrapolator = PoseExtrapolator::InitializeWithImu( auto extrapolator = PoseExtrapolator::InitializeWithImu(
common::FromSeconds(kPoseQueueDuration), kGravityTimeConstant, imu_data); common::FromSeconds(kPoseQueueDuration), kGravityTimeConstant, imu_data);
Eigen::Quaterniond expected_orientation; Eigen::Quaterniond expected_orientation =
expected_orientation.setFromTwoVectors(initial_gravity_acceleration, FromTwoVectors(initial_gravity_acceleration, Eigen::Vector3d::UnitZ());
Eigen::Vector3d::UnitZ());
EXPECT_NEAR(0., EXPECT_NEAR(0.,
extrapolator->EstimateGravityOrientation(current_time) extrapolator->EstimateGravityOrientation(current_time)
.angularDistance(expected_orientation), .angularDistance(expected_orientation),
@ -84,8 +84,8 @@ TEST(PoseExtrapolatorTest, EstimateGravityOrientationWithIMU) {
extrapolator->AddImuData( extrapolator->AddImuData(
sensor::ImuData{current_time, gravity_acceleration, angular_velocity}); sensor::ImuData{current_time, gravity_acceleration, angular_velocity});
} }
expected_orientation.setFromTwoVectors(gravity_acceleration, expected_orientation =
Eigen::Vector3d::UnitZ()); FromTwoVectors(gravity_acceleration, Eigen::Vector3d::UnitZ());
EXPECT_NEAR(0., EXPECT_NEAR(0.,
extrapolator->EstimateGravityOrientation(current_time) extrapolator->EstimateGravityOrientation(current_time)
.angularDistance(expected_orientation), .angularDistance(expected_orientation),