From 4c9c77034b152e06a60edb71371ad086d1b5e0a3 Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Wed, 23 Nov 2016 14:27:36 +0100 Subject: [PATCH] Remove UKF-related debug output. (#142) --- cartographer/mapping/CMakeLists.txt | 2 -- .../global_trajectory_builder_interface.h | 1 - cartographer/mapping/trajectory_builder.h | 31 +++---------------- .../mapping_2d/local_trajectory_builder.cc | 6 +--- .../kalman_local_trajectory_builder.cc | 12 +++---- .../optimizing_local_trajectory_builder.cc | 21 ++++++------- .../optimizing_local_trajectory_builder.h | 4 +-- 7 files changed, 19 insertions(+), 58 deletions(-) diff --git a/cartographer/mapping/CMakeLists.txt b/cartographer/mapping/CMakeLists.txt index ec4138f..8884a5d 100644 --- a/cartographer/mapping/CMakeLists.txt +++ b/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 diff --git a/cartographer/mapping/global_trajectory_builder_interface.h b/cartographer/mapping/global_trajectory_builder_interface.h index 467bfb9..12b97e2 100644 --- a/cartographer/mapping/global_trajectory_builder_interface.h +++ b/cartographer/mapping/global_trajectory_builder_interface.h @@ -22,7 +22,6 @@ #include #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" diff --git a/cartographer/mapping/trajectory_builder.h b/cartographer/mapping/trajectory_builder.h index 311047a..2a227b4 100644 --- a/cartographer/mapping/trajectory_builder.h +++ b/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; }; diff --git a/cartographer/mapping_2d/local_trajectory_builder.cc b/cartographer/mapping_2d/local_trajectory_builder.cc index 1b317e6..116e37f 100644 --- a/cartographer/mapping_2d/local_trajectory_builder.cc +++ b/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())}; diff --git a/cartographer/mapping_3d/kalman_local_trajectory_builder.cc b/cartographer/mapping_3d/kalman_local_trajectory_builder.cc index 1489f8d..96fd6c4 100644 --- a/cartographer/mapping_3d/kalman_local_trajectory_builder.cc +++ b/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())}; diff --git a/cartographer/mapping_3d/optimizing_local_trajectory_builder.cc b/cartographer/mapping_3d/optimizing_local_trajectory_builder.cc index 1d20407..211ec5d 100644 --- a/cartographer/mapping_3d/optimizing_local_trajectory_builder.cc +++ b/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())}; - 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::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())); + + const kalman_filter::PoseCovariance kCovariance = + 1e-7 * kalman_filter::PoseCovariance::Identity(); + return std::unique_ptr(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}); } diff --git a/cartographer/mapping_3d/optimizing_local_trajectory_builder.h b/cartographer/mapping_3d/optimizing_local_trajectory_builder.h index 12775d7..348490b 100644 --- a/cartographer/mapping_3d/optimizing_local_trajectory_builder.h +++ b/cartographer/mapping_3d/optimizing_local_trajectory_builder.h @@ -22,7 +22,6 @@ #include #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 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 MaybeOptimize(common::Time time);