Remove UKF-related debug output. (#142)
parent
574a56bbbc
commit
4c9c77034b
|
@ -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
|
||||||
|
|
|
@ -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"
|
||||||
|
|
|
@ -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;
|
||||||
};
|
};
|
||||||
|
|
|
@ -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>())};
|
||||||
|
|
||||||
|
|
|
@ -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>())};
|
||||||
|
|
||||||
|
|
|
@ -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});
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -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);
|
||||||
|
|
||||||
|
|
Loading…
Reference in New Issue