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

Signed-off-by: mschworer <mschworer@lyft.com>

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

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_

cartographer/mapping/imu_tracker.cc

@@ -20,6 +20,7 @@
 #include <limits>
 
 #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.);

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;
+  const Eigen::Quaterniond expected_orientation =
-  expected_orientation.setFromTwoVectors(linear_acceleration_,
+      FromTwoVectors(linear_acceleration_, Eigen::Vector3d::UnitZ());
-                                         Eigen::Vector3d::UnitZ());
   EXPECT_NEAR(0.,
               imu_tracker_->orientation().angularDistance(expected_orientation),
               kPrecision);

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;
+  Eigen::Quaterniond expected_orientation =
-  expected_orientation.setFromTwoVectors(initial_gravity_acceleration,
+      FromTwoVectors(initial_gravity_acceleration, Eigen::Vector3d::UnitZ());
-                                         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,
+  expected_orientation =
-                                         Eigen::Vector3d::UnitZ());
+      FromTwoVectors(gravity_acceleration, Eigen::Vector3d::UnitZ());
   EXPECT_NEAR(0.,
               extrapolator->EstimateGravityOrientation(current_time)
                   .angularDistance(expected_orientation),