12345qiupeng
/
cartographer
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
cartographer/mapping/CMakeLists.txt
View File

@ -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

1
cartographer/mapping/global_trajectory_builder_interface.h
View File

@ -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"

31
cartographer/mapping/trajectory_builder.h
View File

@ -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;
};

6
cartographer/mapping_2d/local_trajectory_builder.cc
View File

@ -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>())};

12
cartographer/mapping_3d/kalman_local_trajectory_builder.cc
View File

@ -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>())};

21
cartographer/mapping_3d/optimizing_local_trajectory_builder.cc
View File

@ -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});
}

4
cartographer/mapping_3d/optimizing_local_trajectory_builder.h
View File

@ -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);