Remove UKF-related debug output. (#142)

master
Wolfgang Hess 2016-11-23 14:27:36 +01:00 committed by GitHub
parent 574a56bbbc
commit 4c9c77034b
7 changed files with 19 additions and 58 deletions

View File

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

View File

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

View File

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

View File

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

View File

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

View File

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

View File

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