add pose extrapolator interface and extrapolator options (#1726)

This adds an interface around the extrapolator without further integration into any call sides.

Currently there is only a constant-velocity extrapolator implementation. The next step is to add an extrapolator implementation which integrates imu measurements for pose extrapolation.

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

cartographer/mapping/pose_extrapolator.cc

@@ -242,5 +242,21 @@ Eigen::Vector3d PoseExtrapolator::ExtrapolateTranslation(common::Time time) {
   return extrapolation_delta * linear_velocity_from_odometry_;
 }
 
+PoseExtrapolator::ExtrapolationResult
+PoseExtrapolator::ExtrapolatePosesWithGravity(
+    const std::vector<common::Time>& times) {
+  std::vector<transform::Rigid3f> poses;
+  for (auto it = times.begin(); it != std::prev(times.end()); ++it) {
+    poses.push_back(ExtrapolatePose(*it).cast<float>());
+  }
+
+  const Eigen::Vector3d current_velocity = odometry_data_.size() < 2
+                                               ? linear_velocity_from_poses_
+                                               : linear_velocity_from_odometry_;
+  return ExtrapolationResult{poses, ExtrapolatePose(times.back()),
+                             current_velocity,
+                             EstimateGravityOrientation(times.back())};
+}
+
 }  // namespace mapping
 }  // namespace cartographer

cartographer/mapping/pose_extrapolator.h

@@ -22,6 +22,7 @@
 
 #include "cartographer/common/time.h"
 #include "cartographer/mapping/imu_tracker.h"
+#include "cartographer/mapping/pose_extrapolator_interface.h"
 #include "cartographer/sensor/imu_data.h"
 #include "cartographer/sensor/odometry_data.h"
 #include "cartographer/transform/rigid_transform.h"
@@ -32,7 +33,7 @@ namespace mapping {
 // Keep poses for a certain duration to estimate linear and angular velocity.
 // Uses the velocities to extrapolate motion. Uses IMU and/or odometry data if
 // available to improve the extrapolation.
-class PoseExtrapolator {
+class PoseExtrapolator : public PoseExtrapolatorInterface {
  public:
   explicit PoseExtrapolator(common::Duration pose_queue_duration,
                             double imu_gravity_time_constant);
@@ -46,17 +47,20 @@ class PoseExtrapolator {
 
   // Returns the time of the last added pose or Time::min() if no pose was added
   // yet.
-  common::Time GetLastPoseTime() const;
+  common::Time GetLastPoseTime() const override;
-  common::Time GetLastExtrapolatedTime() const;
+  common::Time GetLastExtrapolatedTime() const override;
 
-  void AddPose(common::Time time, const transform::Rigid3d& pose);
+  void AddPose(common::Time time, const transform::Rigid3d& pose) override;
-  void AddImuData(const sensor::ImuData& imu_data);
+  void AddImuData(const sensor::ImuData& imu_data) override;
-  void AddOdometryData(const sensor::OdometryData& odometry_data);
+  void AddOdometryData(const sensor::OdometryData& odometry_data) override;
-  transform::Rigid3d ExtrapolatePose(common::Time time);
+  transform::Rigid3d ExtrapolatePose(common::Time time) override;
+
+  ExtrapolationResult ExtrapolatePosesWithGravity(
+      const std::vector<common::Time>& times) override;
 
   // Returns the current gravity alignment estimate as a rotation from
   // the tracking frame into a gravity aligned frame.
-  Eigen::Quaterniond EstimateGravityOrientation(common::Time time);
+  Eigen::Quaterniond EstimateGravityOrientation(common::Time time) override;
 
  private:
   void UpdateVelocitiesFromPoses();

cartographer/mapping/pose_extrapolator_interface.cc

@@ -0,0 +1,61 @@
+/*
+ * 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_interface.h"
+
+#include "cartographer/common/time.h"
+#include "cartographer/mapping/pose_extrapolator.h"
+
+namespace cartographer {
+namespace mapping {
+
+namespace {
+
+proto::ConstantVelocityPoseExtrapolatorOptions
+CreateConstantVelocityPoseExtrapolatorOptions(
+    common::LuaParameterDictionary* const parameter_dictionary) {
+  proto::ConstantVelocityPoseExtrapolatorOptions options;
+  options.set_pose_queue_duration(
+      parameter_dictionary->GetDouble("pose_queue_duration"));
+  options.set_imu_gravity_time_constant(
+      parameter_dictionary->GetDouble("imu_gravity_time_constant"));
+  return options;
+}
+
+}
+
+proto::PoseExtrapolatorOptions CreatePoseExtrapolatorOptions(
+    common::LuaParameterDictionary* const parameter_dictionary) {
+  proto::PoseExtrapolatorOptions options;
+  *options.mutable_constant_velocity() =
+      CreateConstantVelocityPoseExtrapolatorOptions(
+          parameter_dictionary->GetDictionary("constant_velocity").get());
+  return options;
+}
+
+std::unique_ptr<PoseExtrapolatorInterface>
+PoseExtrapolatorInterface::CreateWithImuData(
+    const proto::PoseExtrapolatorOptions& options,
+    const std::vector<sensor::ImuData>& imu_data) {
+  CHECK(!imu_data.empty());
+  return PoseExtrapolator::InitializeWithImu(
+        common::FromSeconds(options.constant_velocity().pose_queue_duration()),
+        options.constant_velocity().imu_gravity_time_constant(),
+        imu_data.back());
+}
+
+}
+}

cartographer/mapping/pose_extrapolator_interface.h

@@ -0,0 +1,80 @@
+/*
+ * 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_POSE_EXTRAPOLATOR_INTERFACE_H_
+#define CARTOGRAPHER_MAPPING_POSE_EXTRAPOLATOR_INTERFACE_H_
+
+#include <memory>
+#include <tuple>
+
+#include "cartographer/common/time.h"
+#include "cartographer/mapping/proto/pose_extrapolator_options.pb.h"
+#include "cartographer/sensor/imu_data.h"
+#include "cartographer/sensor/odometry_data.h"
+#include "cartographer/transform/rigid_transform.h"
+#include "cartographer/transform/timestamped_transform.h"
+
+namespace cartographer {
+namespace mapping {
+
+proto::PoseExtrapolatorOptions CreatePoseExtrapolatorOptions(
+    common::LuaParameterDictionary* const parameter_dictionary);
+
+class PoseExtrapolatorInterface {
+ public:
+  struct ExtrapolationResult {
+    // The poses for the requested times at index 0 to N-1.
+    std::vector<transform::Rigid3f> previous_poses;
+    // The pose for the requested time at index N.
+    transform::Rigid3d current_pose;
+    Eigen::Vector3d current_velocity;
+    Eigen::Quaterniond gravity_from_tracking;
+  };
+
+  PoseExtrapolatorInterface(const PoseExtrapolatorInterface&) = delete;
+  PoseExtrapolatorInterface& operator=(const PoseExtrapolatorInterface&) = delete;
+  virtual ~PoseExtrapolatorInterface() {}
+
+  // TODO: Remove dependency cycle.
+  static std::unique_ptr<PoseExtrapolatorInterface> CreateWithImuData(
+      const proto::PoseExtrapolatorOptions& options,
+      const std::vector<sensor::ImuData>& imu_data);
+
+  // Returns the time of the last added pose or Time::min() if no pose was added
+  // yet.
+  virtual common::Time GetLastPoseTime() const = 0;
+  virtual common::Time GetLastExtrapolatedTime() const = 0;
+
+  virtual void AddPose(common::Time time, const transform::Rigid3d& pose) = 0;
+  virtual void AddImuData(const sensor::ImuData& imu_data) = 0;
+  virtual void AddOdometryData(const sensor::OdometryData& odometry_data) = 0;
+  virtual transform::Rigid3d ExtrapolatePose(common::Time time) = 0;
+
+  virtual ExtrapolationResult ExtrapolatePosesWithGravity(
+      const std::vector<common::Time>& times) = 0;
+
+  // Returns the current gravity alignment estimate as a rotation from
+  // the tracking frame into a gravity aligned frame.
+  virtual Eigen::Quaterniond EstimateGravityOrientation(common::Time time) = 0;
+
+ protected:
+  PoseExtrapolatorInterface() {}
+};
+
+}  // namespace mapping
+}  // namespace cartographer
+
+#endif  // CARTOGRAPHER_MAPPING_POSE_EXTRAPOLATOR_INTERFACE_H_

cartographer/mapping/proto/pose_extrapolator_options.proto

@@ -0,0 +1,32 @@
+// 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.
+
+syntax = "proto3";
+
+package cartographer.mapping.proto;
+
+message ConstantVelocityPoseExtrapolatorOptions {
+  // Time constant in seconds for the orientation moving average based on
+  // observed gravity via the IMU. It should be chosen so that the error
+  // 1. from acceleration measurements not due to gravity (which gets worse when
+  // the constant is reduced) and
+  // 2. from integration of angular velocities (which gets worse when the
+  // constant is increased) is balanced.
+  double imu_gravity_time_constant = 1;
+  double pose_queue_duration = 2;
+}
+
+message PoseExtrapolatorOptions {
+  ConstantVelocityPoseExtrapolatorOptions constant_velocity = 2;
+}