Unify to AddLaserFan for both 2D and 3D. (#81)

cartographer/mapping/global_trajectory_builder_interface.h

@@ -78,13 +78,11 @@ class GlobalTrajectoryBuilderInterface {
   virtual kalman_filter::PoseTracker* pose_tracker() const = 0;
   virtual const PoseEstimate& pose_estimate() const = 0;
 
-  virtual void AddHorizontalLaserFan(common::Time time,
+  virtual void AddLaserFan(common::Time time,
-                                     const sensor::LaserFan& laser_fan) = 0;
+                           const sensor::LaserFan& laser_fan) = 0;
   virtual void AddImuData(common::Time time,
                           const Eigen::Vector3d& linear_acceleration,
                           const Eigen::Vector3d& angular_velocity) = 0;
-  virtual void AddLaserFan3D(common::Time time,
-                             const sensor::LaserFan& laser_fan) = 0;
   virtual void AddOdometerPose(
       common::Time time, const transform::Rigid3d& pose,
       const kalman_filter::PoseCovariance& covariance) = 0;

cartographer/mapping_2d/global_trajectory_builder.cc

@@ -40,8 +40,8 @@ kalman_filter::PoseTracker* GlobalTrajectoryBuilder::pose_tracker() const {
   return local_trajectory_builder_.pose_tracker();
 }
 
-void GlobalTrajectoryBuilder::AddHorizontalLaserFan(
+void GlobalTrajectoryBuilder::AddLaserFan(const common::Time time,
-    const common::Time time, const sensor::LaserFan& laser_fan) {
+                                          const sensor::LaserFan& laser_fan) {
   std::unique_ptr<LocalTrajectoryBuilder::InsertionResult> insertion_result =
       local_trajectory_builder_.AddHorizontalLaserFan(time, laser_fan);
   if (insertion_result != nullptr) {

cartographer/mapping_2d/global_trajectory_builder.h

@@ -40,16 +40,14 @@ class GlobalTrajectoryBuilder
   const mapping::GlobalTrajectoryBuilderInterface::PoseEstimate& pose_estimate()
       const override;
 
-  void AddHorizontalLaserFan(common::Time time,
+  // Handles approximately horizontal laser fans.
-                             const sensor::LaserFan& laser_fan) override;
+  void AddLaserFan(common::Time time,
+                   const sensor::LaserFan& laser_fan) override;
   void AddImuData(common::Time time, const Eigen::Vector3d& linear_acceleration,
                   const Eigen::Vector3d& angular_velocity) override;
   void AddOdometerPose(
       common::Time time, const transform::Rigid3d& pose,
       const kalman_filter::PoseCovariance& covariance) override;
-  void AddLaserFan3D(common::Time, const sensor::LaserFan&) override {
-    LOG(FATAL) << "Not implemented.";
-  };
 
  private:
   const proto::LocalTrajectoryBuilderOptions options_;

cartographer/mapping_3d/global_trajectory_builder.cc

@@ -49,10 +49,10 @@ void GlobalTrajectoryBuilder::AddImuData(
   sparse_pose_graph_->AddImuData(time, linear_acceleration, angular_velocity);
 }
 
-void GlobalTrajectoryBuilder::AddLaserFan3D(const common::Time time,
+void GlobalTrajectoryBuilder::AddLaserFan(const common::Time time,
-                                            const sensor::LaserFan& laser_fan) {
+                                          const sensor::LaserFan& laser_fan) {
   auto insertion_result =
-      local_trajectory_builder_->AddLaserFan3D(time, laser_fan);
+      local_trajectory_builder_->AddLaserFan(time, laser_fan);
 
   if (insertion_result == nullptr) {
     return;

cartographer/mapping_3d/global_trajectory_builder.h

@@ -42,17 +42,13 @@ class GlobalTrajectoryBuilder
   kalman_filter::PoseTracker* pose_tracker() const override;
   void AddImuData(common::Time time, const Eigen::Vector3d& linear_acceleration,
                   const Eigen::Vector3d& angular_velocity) override;
-  void AddLaserFan3D(common::Time time,
+  void AddLaserFan(common::Time time,
-                     const sensor::LaserFan& laser_fan) override;
+                   const sensor::LaserFan& laser_fan) override;
   void AddOdometerPose(
       common::Time time, const transform::Rigid3d& pose,
       const kalman_filter::PoseCovariance& covariance) override;
   const PoseEstimate& pose_estimate() const override;
 
-  void AddHorizontalLaserFan(common::Time, const sensor::LaserFan&) override {
-    LOG(FATAL) << "Not implemented.";
-  }
-
  private:
   mapping_3d::SparsePoseGraph* const sparse_pose_graph_;
   std::unique_ptr<LocalTrajectoryBuilderInterface> local_trajectory_builder_;

cartographer/mapping_3d/kalman_local_trajectory_builder.cc

@@ -74,8 +74,8 @@ void KalmanLocalTrajectoryBuilder::AddImuData(
 }
 
 std::unique_ptr<KalmanLocalTrajectoryBuilder::InsertionResult>
-KalmanLocalTrajectoryBuilder::AddLaserFan3D(const common::Time time,
+KalmanLocalTrajectoryBuilder::AddLaserFan(const common::Time time,
-                                            const sensor::LaserFan& laser_fan) {
+                                          const sensor::LaserFan& laser_fan) {
   if (!pose_tracker_) {
     LOG(INFO) << "PoseTracker not yet initialized.";
     return nullptr;

cartographer/mapping_3d/kalman_local_trajectory_builder.h

@@ -48,7 +48,7 @@ class KalmanLocalTrajectoryBuilder : public LocalTrajectoryBuilderInterface {
 
   void AddImuData(common::Time time, const Eigen::Vector3d& linear_acceleration,
                   const Eigen::Vector3d& angular_velocity) override;
-  std::unique_ptr<InsertionResult> AddLaserFan3D(
+  std::unique_ptr<InsertionResult> AddLaserFan(
       common::Time time, const sensor::LaserFan& laser_fan) override;
   void AddOdometerPose(
       common::Time time, const transform::Rigid3d& pose,

cartographer/mapping_3d/kalman_local_trajectory_builder_test.cc

@@ -256,7 +256,7 @@ class KalmanLocalTrajectoryBuilderTest : public ::testing::Test {
     int num_poses = 0;
     for (const TrajectoryNode& node : expected_trajectory) {
       AddLinearOnlyImuObservation(node.time, node.pose);
-      if (local_trajectory_builder_->AddLaserFan3D(
+      if (local_trajectory_builder_->AddLaserFan(
               node.time, GenerateLaserFan(node.pose)) != nullptr) {
         const auto pose_estimate = local_trajectory_builder_->pose_estimate();
         EXPECT_THAT(pose_estimate.pose, transform::IsNearly(node.pose, 1e-1));

cartographer/mapping_3d/local_trajectory_builder_interface.h

@@ -53,7 +53,7 @@ class LocalTrajectoryBuilderInterface {
   virtual void AddImuData(common::Time time,
                           const Eigen::Vector3d& linear_acceleration,
                           const Eigen::Vector3d& angular_velocity) = 0;
-  virtual std::unique_ptr<InsertionResult> AddLaserFan3D(
+  virtual std::unique_ptr<InsertionResult> AddLaserFan(
       common::Time time, const sensor::LaserFan& laser_fan) = 0;
   virtual void AddOdometerPose(
       common::Time time, const transform::Rigid3d& pose,

cartographer/mapping_3d/optimizing_local_trajectory_builder.cc

@@ -133,7 +133,7 @@ void OptimizingLocalTrajectoryBuilder::AddOdometerPose(
 }
 
 std::unique_ptr<OptimizingLocalTrajectoryBuilder::InsertionResult>
-OptimizingLocalTrajectoryBuilder::AddLaserFan3D(
+OptimizingLocalTrajectoryBuilder::AddLaserFan(
     const common::Time time, const sensor::LaserFan& laser_fan_in_tracking) {
   CHECK_GT(laser_fan_in_tracking.returns.size(), 0);
 

cartographer/mapping_3d/optimizing_local_trajectory_builder.h

@@ -52,7 +52,7 @@ class OptimizingLocalTrajectoryBuilder
 
   void AddImuData(common::Time time, const Eigen::Vector3d& linear_acceleration,
                   const Eigen::Vector3d& angular_velocity) override;
-  std::unique_ptr<InsertionResult> AddLaserFan3D(
+  std::unique_ptr<InsertionResult> AddLaserFan(
       common::Time time,
       const sensor::LaserFan& laser_fan_in_tracking) override;
 

cartographer/mapping_3d/sparse_pose_graph/spa_cost_function.h

@@ -46,20 +46,18 @@ class SpaCostFunction {
       const T* const c_i_translation, const T* const c_j_rotation,
       const T* const c_j_translation) {
     const Eigen::Quaternion<T> R_i_inverse(c_i_rotation[0], -c_i_rotation[1],
-                                           -c_i_rotation[2],
+                                           -c_i_rotation[2], -c_i_rotation[3]);
-                                           -c_i_rotation[3]);
 
-    const Eigen::Matrix<T, 3, 1> delta(
+    const Eigen::Matrix<T, 3, 1> delta(c_j_translation[0] - c_i_translation[0],
-        c_j_translation[0] - c_i_translation[0],
+                                       c_j_translation[1] - c_i_translation[1],
-        c_j_translation[1] - c_i_translation[1],
+                                       c_j_translation[2] - c_i_translation[2]);
-        c_j_translation[2] - c_i_translation[2]);
     const Eigen::Matrix<T, 3, 1> h_translation = R_i_inverse * delta;
 
     const Eigen::Quaternion<T> h_rotation_inverse =
         Eigen::Quaternion<T>(c_j_rotation[0], -c_j_rotation[1],
                              -c_j_rotation[2], -c_j_rotation[3]) *
-        Eigen::Quaternion<T>(c_i_rotation[0], c_i_rotation[1],
+        Eigen::Quaternion<T>(c_i_rotation[0], c_i_rotation[1], c_i_rotation[2],
-                             c_i_rotation[2], c_i_rotation[3]);
+                             c_i_rotation[3]);
 
     const Eigen::Matrix<T, 3, 1> angle_axis_difference =
         transform::RotationQuaternionToAngleAxisVector(