Remove UKF-related debug output. (cartographer-project#142)

cartographer/mapping/CMakeLists.txt

@@ -51,7 +51,6 @@ google_library(mapping_global_trajectory_builder_interface
     global_trajectory_builder_interface.h
   DEPENDS
     common_time
-    kalman_filter_pose_tracker
     mapping_submaps
     mapping_trajectory_builder
     sensor_laser
@@ -167,7 +166,6 @@ google_library(mapping_trajectory_builder
     common_make_unique
     common_port
     common_time
-    kalman_filter_pose_tracker
     mapping_submaps
     sensor_data
     sensor_laser

cartographer/mapping/global_trajectory_builder_interface.h

@@ -22,7 +22,6 @@
 #include <string>
 
 #include "cartographer/common/time.h"
-#include "cartographer/kalman_filter/pose_tracker.h"
 #include "cartographer/mapping/submaps.h"
 #include "cartographer/mapping/trajectory_builder.h"
 #include "cartographer/sensor/laser.h"

cartographer/mapping/trajectory_builder.h

@@ -24,7 +24,6 @@
 #include "cartographer/common/make_unique.h"
 #include "cartographer/common/port.h"
 #include "cartographer/common/time.h"
-#include "cartographer/kalman_filter/pose_tracker.h"
 #include "cartographer/mapping/submaps.h"
 #include "cartographer/sensor/data.h"
 #include "cartographer/sensor/laser.h"
@@ -36,37 +35,15 @@ namespace mapping {
 // This interface is used for both 2D and 3D SLAM.
 class TrajectoryBuilder {
  public:
-  // Represents a newly computed pose. Each of the following steps in the pose
-  // estimation pipeline are provided for debugging:
-  //
-  // 1. UKF prediction
-  // 2. Absolute pose observation (e.g. from scan matching)
-  // 3. UKF estimate after integrating any measurements
-  //
-  // Finally, 'pose' is the end-user visualization of orientation and
-  // 'point_cloud' is the point cloud, in the local map frame.
+  // Represents a newly computed pose. 'pose' is the end-user visualization of
+  // orientation and 'point_cloud' is the point cloud, in the local map frame.
   struct PoseEstimate {
     PoseEstimate() = default;
-    PoseEstimate(common::Time time,
-                 const kalman_filter::PoseAndCovariance& prediction,
-                 const kalman_filter::PoseAndCovariance& observation,
-                 const kalman_filter::PoseAndCovariance& estimate,
-                 const transform::Rigid3d& pose,
+    PoseEstimate(common::Time time, const transform::Rigid3d& pose,
                  const sensor::PointCloud& point_cloud)
-        : time(time),
-          prediction(prediction),
-          observation(observation),
-          estimate(estimate),
-          pose(pose),
-          point_cloud(point_cloud) {}
+        : time(time), pose(pose), point_cloud(point_cloud) {}
 
     common::Time time = common::Time::min();
-    kalman_filter::PoseAndCovariance prediction = {
-        transform::Rigid3d::Identity(), kalman_filter::PoseCovariance::Zero()};
-    kalman_filter::PoseAndCovariance observation = {
-        transform::Rigid3d::Identity(), kalman_filter::PoseCovariance::Zero()};
-    kalman_filter::PoseAndCovariance estimate = {
-        transform::Rigid3d::Identity(), kalman_filter::PoseCovariance::Zero()};
     transform::Rigid3d pose = transform::Rigid3d::Identity();
     sensor::PointCloud point_cloud;
   };

cartographer/mapping_2d/local_trajectory_builder.cc

@@ -216,11 +216,7 @@ LocalTrajectoryBuilder::AddHorizontalLaserFan(
   const transform::Rigid3d tracking_2d_to_map =
       pose_estimate_ * tracking_to_tracking_2d.inverse();
   last_pose_estimate_ = {
-      time,
-      {pose_prediction, covariance_observation},
-      {pose_estimate_, covariance_observation},
-      {pose_estimate_, covariance_observation},
-      pose_estimate_,
+      time, pose_estimate_,
       sensor::TransformPointCloud(laser_fan_in_tracking_2d.returns,
                                   tracking_2d_to_map.cast<float>())};
 

cartographer/mapping_3d/kalman_local_trajectory_builder.cc

@@ -137,9 +137,9 @@ KalmanLocalTrajectoryBuilder::AddAccumulatedLaserFan(
   }
 
   transform::Rigid3d pose_prediction;
-  kalman_filter::PoseCovariance covariance_prediction;
-  pose_tracker_->GetPoseEstimateMeanAndCovariance(time, &pose_prediction,
-                                                  &covariance_prediction);
+  kalman_filter::PoseCovariance unused_covariance_prediction;
+  pose_tracker_->GetPoseEstimateMeanAndCovariance(
+      time, &pose_prediction, &unused_covariance_prediction);
 
   transform::Rigid3d initial_ceres_pose = pose_prediction;
   sensor::AdaptiveVoxelFilter adaptive_voxel_filter(
@@ -176,11 +176,7 @@ KalmanLocalTrajectoryBuilder::AddAccumulatedLaserFan(
       time, &scan_matcher_pose_estimate_, &covariance_estimate);
 
   last_pose_estimate_ = {
-      time,
-      {pose_prediction, covariance_prediction},
-      {pose_observation, covariance_observation},
-      {scan_matcher_pose_estimate_, covariance_estimate},
-      scan_matcher_pose_estimate_,
+      time, scan_matcher_pose_estimate_,
       sensor::TransformPointCloud(filtered_laser_fan.returns,
                                   pose_observation.cast<float>())};
 

cartographer/mapping_3d/optimizing_local_trajectory_builder.cc

@@ -19,6 +19,7 @@
 #include "cartographer/common/ceres_solver_options.h"
 #include "cartographer/common/make_unique.h"
 #include "cartographer/common/time.h"
+#include "cartographer/kalman_filter/pose_tracker.h"
 #include "cartographer/mapping_3d/proto/optimizing_local_trajectory_builder_options.pb.h"
 #include "cartographer/mapping_3d/rotation_cost_function.h"
 #include "cartographer/mapping_3d/scan_matching/occupied_space_cost_functor.h"
@@ -366,19 +367,12 @@ OptimizingLocalTrajectoryBuilder::AddAccumulatedLaserFan(
     return nullptr;
   }
 
-  const kalman_filter::PoseCovariance covariance =
-      1e-7 * kalman_filter::PoseCovariance::Identity();
-
   last_pose_estimate_ = {
-      time,
-      {optimized_pose, covariance},
-      {optimized_pose, covariance},
-      {optimized_pose, covariance},
-      optimized_pose,
+      time, optimized_pose,
       sensor::TransformPointCloud(filtered_laser_fan.returns,
                                   optimized_pose.cast<float>())};
 
-  return InsertIntoSubmap(time, filtered_laser_fan, optimized_pose, covariance);
+  return InsertIntoSubmap(time, filtered_laser_fan, optimized_pose);
 }
 
 const OptimizingLocalTrajectoryBuilder::PoseEstimate&
@@ -394,8 +388,7 @@ void OptimizingLocalTrajectoryBuilder::AddTrajectoryNodeIndex(
 std::unique_ptr<OptimizingLocalTrajectoryBuilder::InsertionResult>
 OptimizingLocalTrajectoryBuilder::InsertIntoSubmap(
     const common::Time time, const sensor::LaserFan& laser_fan_in_tracking,
-    const transform::Rigid3d& pose_observation,
-    const kalman_filter::PoseCovariance& covariance_estimate) {
+    const transform::Rigid3d& pose_observation) {
   if (motion_filter_.IsSimilar(time, pose_observation)) {
     return nullptr;
   }
@@ -407,8 +400,12 @@ OptimizingLocalTrajectoryBuilder::InsertIntoSubmap(
   }
   submaps_->InsertLaserFan(sensor::TransformLaserFan(
       laser_fan_in_tracking, pose_observation.cast<float>()));
+
+  const kalman_filter::PoseCovariance kCovariance =
+      1e-7 * kalman_filter::PoseCovariance::Identity();
+
   return std::unique_ptr<InsertionResult>(new InsertionResult{
-      time, laser_fan_in_tracking, pose_observation, covariance_estimate,
+      time, laser_fan_in_tracking, pose_observation, kCovariance,
       submaps_.get(), matching_submap, insertion_submaps});
 }
 

cartographer/mapping_3d/optimizing_local_trajectory_builder.h

@@ -22,7 +22,6 @@
 #include <memory>
 
 #include "cartographer/common/time.h"
-#include "cartographer/kalman_filter/pose_tracker.h"
 #include "cartographer/mapping/global_trajectory_builder_interface.h"
 #include "cartographer/mapping_3d/imu_integration.h"
 #include "cartographer/mapping_3d/local_trajectory_builder_interface.h"
@@ -107,8 +106,7 @@ class OptimizingLocalTrajectoryBuilder
 
   std::unique_ptr<InsertionResult> InsertIntoSubmap(
       const common::Time time, const sensor::LaserFan& laser_fan_in_tracking,
-      const transform::Rigid3d& pose_observation,
-      const kalman_filter::PoseCovariance& covariance_estimate);
+      const transform::Rigid3d& pose_observation);
 
   std::unique_ptr<InsertionResult> MaybeOptimize(common::Time time);