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

Add support for multi-trajectory to 3D SLAM. (#146)

master
Wolfgang Hess 2016-11-28 12:52:45 +01:00 committed by GitHub
parent 318607ccd1
commit 51a0ec06a1
8 changed files with 115 additions and 88 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
cartographer/mapping_2d/local_trajectory_builder.cc
View File

@ -168,7 +168,7 @@ LocalTrajectoryBuilder::AddHorizontalLaserFan(
pose_estimate_ * odometry_correction_;
const transform::Rigid3d model_prediction = pose_estimate_;
// TODO(whess): Prefer IMU over odom orientation if configured?
const transform::Rigid3d pose_prediction = odometry_prediction;
const transform::Rigid3d& pose_prediction = odometry_prediction;
// Computes the rotation without yaw, as defined by GetYaw().
const transform::Rigid3d tracking_to_tracking_2d =

3
cartographer/mapping_3d/global_trajectory_builder.cc
View File

@ -38,7 +38,8 @@ void GlobalTrajectoryBuilder::AddImuData(
const Eigen::Vector3d& angular_velocity) {
local_trajectory_builder_->AddImuData(time, linear_acceleration,
angular_velocity);
sparse_pose_graph_->AddImuData(time, linear_acceleration, angular_velocity);
sparse_pose_graph_->AddImuData(local_trajectory_builder_->submaps(), time,
linear_acceleration, angular_velocity);
}
void GlobalTrajectoryBuilder::AddRangefinderData(

16
cartographer/mapping_3d/sparse_pose_graph.cc
View File

@ -136,12 +136,13 @@ void SparsePoseGraph::AddWorkItem(std::function<void()> work_item) {
}
}
void SparsePoseGraph::AddImuData(common::Time time,
void SparsePoseGraph::AddImuData(const mapping::Submaps* trajectory,
common::Time time,
const Eigen::Vector3d& linear_acceleration,
const Eigen::Vector3d& angular_velocity) {
common::MutexLocker locker(&mutex_);
AddWorkItem([=]() REQUIRES(mutex_) {
optimization_problem_.AddImuData(time, linear_acceleration,
optimization_problem_.AddImuData(trajectory, time, linear_acceleration,
angular_velocity);
});
}
@ -202,7 +203,10 @@ void SparsePoseGraph::ComputeConstraintsForScan(
submap_transforms_[matching_index] *
matching_submap->local_pose().inverse() * pose;
CHECK_EQ(scan_index, optimization_problem_.node_data().size());
optimization_problem_.AddTrajectoryNode(time, optimized_pose);
const mapping::Submaps* const scan_trajectory =
trajectory_nodes_[scan_index].constant_data->trajectory;
optimization_problem_.AddTrajectoryNode(scan_trajectory, time,
optimized_pose);
for (const Submap* submap : insertion_submaps) {
const int submap_index = GetSubmapIndex(submap);
CHECK(!submap_states_[submap_index].finished);
@ -319,17 +323,13 @@ void SparsePoseGraph::RunFinalOptimization() {
void SparsePoseGraph::RunOptimization() {
if (!submap_transforms_.empty()) {
transform::Rigid3d submap_0_pose;
std::vector<const mapping::Submaps*> trajectories;
{
common::MutexLocker locker(&mutex_);
CHECK(!submap_states_.empty());
submap_0_pose = submap_states_.front().submap->local_pose();
for (const auto& trajectory_node : trajectory_nodes_) {
trajectories.push_back(trajectory_node.constant_data->trajectory);
}
}
optimization_problem_.Solve(constraints_, submap_0_pose, trajectories,
optimization_problem_.Solve(constraints_, submap_0_pose,
&submap_transforms_);
common::MutexLocker locker(&mutex_);
has_new_optimized_poses_ = true;

3
cartographer/mapping_3d/sparse_pose_graph.h
View File

@ -79,7 +79,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
EXCLUDES(mutex_);
// Adds new IMU data to be used in the optimization.
void AddImuData(common::Time time, const Eigen::Vector3d& linear_acceleration,
void AddImuData(const mapping::Submaps* trajectory, common::Time time,
const Eigen::Vector3d& linear_acceleration,
const Eigen::Vector3d& angular_velocity);
void RunFinalOptimization() override;

25
cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc
View File

@ -222,15 +222,24 @@ void ConstraintBuilder::ComputeConstraint(
float score = 0.;
transform::Rigid3d pose_estimate = transform::Rigid3d::Identity();
CHECK(!match_full_submap) << "match_full_submap not supported for 3D.";
if (submap_scan_matcher->fast_correlative_scan_matcher->Match(
initial_pose, filtered_point_cloud, point_cloud, options_.min_score(),
&score, &pose_estimate)) {
// We've reported a successful local match.
CHECK_GT(score, options_.min_score());
if (match_full_submap) {
if (submap_scan_matcher->fast_correlative_scan_matcher->MatchFullSubmap(
initial_pose.rotation(), filtered_point_cloud, point_cloud,
options_.global_localization_min_score(), &score, &pose_estimate)) {
CHECK_GT(score, options_.global_localization_min_score());
trajectory_connectivity->Connect(scan_trajectory, submap_trajectory);
} else {
return;
}
} else {
return;
if (submap_scan_matcher->fast_correlative_scan_matcher->Match(
initial_pose, filtered_point_cloud, point_cloud,
options_.min_score(), &score, &pose_estimate)) {
// We've reported a successful local match.
CHECK_GT(score, options_.min_score());
} else {
return;
}
}
{
common::MutexLocker locker(&mutex_);

131
cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc
View File

@ -77,15 +77,18 @@ OptimizationProblem::OptimizationProblem(
OptimizationProblem::~OptimizationProblem() {}
void OptimizationProblem::AddImuData(common::Time time,
void OptimizationProblem::AddImuData(const mapping::Submaps* const trajectory,
const common::Time time,
const Eigen::Vector3d& linear_acceleration,
const Eigen::Vector3d& angular_velocity) {
imu_data_.push_back(ImuData{time, linear_acceleration, angular_velocity});
imu_data_[trajectory].push_back(
ImuData{time, linear_acceleration, angular_velocity});
}
void OptimizationProblem::AddTrajectoryNode(
common::Time time, const transform::Rigid3d& point_cloud_pose) {
node_data_.push_back(NodeData{time, point_cloud_pose});
const mapping::Submaps* const trajectory, const common::Time time,
const transform::Rigid3d& point_cloud_pose) {
node_data_.push_back(NodeData{trajectory, time, point_cloud_pose});
}
void OptimizationProblem::SetMaxNumIterations(const int32 max_num_iterations) {
@ -96,13 +99,17 @@ void OptimizationProblem::SetMaxNumIterations(const int32 max_num_iterations) {
void OptimizationProblem::Solve(
const std::vector<Constraint>& constraints,
const transform::Rigid3d& submap_0_transform,
const std::vector<const mapping::Submaps*>& trajectories,
std::vector<transform::Rigid3d>* submap_transforms) {
std::vector<transform::Rigid3d>* const submap_transforms) {
if (node_data_.empty()) {
// Nothing to optimize.
return;
}
CHECK(!imu_data_.empty());
// Compute the indices of consecutive nodes for each trajectory.
std::map<const mapping::Submaps*, std::vector<size_t>> nodes_per_trajectory;
for (size_t j = 0; j != node_data_.size(); ++j) {
nodes_per_trajectory[node_data_[j].trajectory].push_back(j);
}
ceres::Problem::Options problem_options;
ceres::Problem problem(problem_options);
@ -146,58 +153,68 @@ void OptimizationProblem::Solve(
C_point_clouds[constraint.j].translation());
}
CHECK(!node_data_.empty());
CHECK_GE(trajectories.size(), node_data_.size());
// Add constraints based on IMU observations of angular velocities and
// linear acceleration.
for (const auto& trajectory_nodes_pair : nodes_per_trajectory) {
const mapping::Submaps* const trajectory = trajectory_nodes_pair.first;
const std::vector<size_t>& node_indices = trajectory_nodes_pair.second;
const std::deque<ImuData>& imu_data = imu_data_.at(trajectory);
CHECK(!node_indices.empty());
CHECK(!imu_data.empty());
// Add constraints for IMU observed data: angular velocities and
// accelerations.
auto it = imu_data_.cbegin();
while ((it + 1) != imu_data_.cend() && (it + 1)->time <= node_data_[0].time) {
++it;
}
for (size_t j = 1; j < node_data_.size(); ++j) {
auto it2 = it;
const IntegrateImuResult<double> result = IntegrateImu(
imu_data_, node_data_[j - 1].time, node_data_[j].time, &it);
if (j + 1 < node_data_.size()) {
const common::Duration first_delta_time =
node_data_[j].time - node_data_[j - 1].time;
const common::Duration second_delta_time =
node_data_[j + 1].time - node_data_[j].time;
const common::Time first_center =
node_data_[j - 1].time + first_delta_time / 2;
const common::Time second_center =
node_data_[j].time + second_delta_time / 2;
const IntegrateImuResult<double> result_to_first_center =
IntegrateImu(imu_data_, node_data_[j - 1].time, first_center, &it2);
const IntegrateImuResult<double> result_center_to_center =
IntegrateImu(imu_data_, first_center, second_center, &it2);
// 'delta_velocity' is the change in velocity from the point in time
// halfway between the first and second poses to halfway between second
// and third pose. It is computed from IMU data and still contains a
// delta due to gravity. The orientation of this vector is in the IMU
// frame at the second pose.
const Eigen::Vector3d delta_velocity =
(result.delta_rotation.inverse() *
result_to_first_center.delta_rotation) *
result_center_to_center.delta_velocity;
problem.AddResidualBlock(
new ceres::AutoDiffCostFunction<AccelerationCostFunction, 3, 4, 3, 3,
3, 1>(new AccelerationCostFunction(
options_.acceleration_weight(), delta_velocity,
common::ToSeconds(first_delta_time),
common::ToSeconds(second_delta_time))),
nullptr, C_point_clouds[j].rotation(),
C_point_clouds[j - 1].translation(), C_point_clouds[j].translation(),
C_point_clouds[j + 1].translation(), &gravity_constant_);
// Skip IMU data before the first node of this trajectory.
auto it = imu_data.cbegin();
while ((it + 1) != imu_data.cend() &&
(it + 1)->time <= node_data_[node_indices[0]].time) {
++it;
}
for (size_t j = 1; j < node_indices.size(); ++j) {
auto it2 = it;
const IntegrateImuResult<double> result =
IntegrateImu(imu_data, node_data_[node_indices[j - 1]].time,
node_data_[node_indices[j]].time, &it);
if (j + 1 < node_indices.size()) {
const common::Time first_time = node_data_[node_indices[j - 1]].time;
const common::Time second_time = node_data_[node_indices[j]].time;
const common::Time third_time = node_data_[node_indices[j + 1]].time;
const common::Duration first_duration = second_time - first_time;
const common::Duration second_duration = third_time - second_time;
const common::Time first_center = first_time + first_duration / 2;
const common::Time second_center = second_time + second_duration / 2;
const IntegrateImuResult<double> result_to_first_center =
IntegrateImu(imu_data, first_time, first_center, &it2);
const IntegrateImuResult<double> result_center_to_center =
IntegrateImu(imu_data, first_center, second_center, &it2);
// 'delta_velocity' is the change in velocity from the point in time
// halfway between the first and second poses to halfway between second
// and third pose. It is computed from IMU data and still contains a
// delta due to gravity. The orientation of this vector is in the IMU
// frame at the second pose.
const Eigen::Vector3d delta_velocity =
(result.delta_rotation.inverse() *
result_to_first_center.delta_rotation) *
result_center_to_center.delta_velocity;
problem.AddResidualBlock(
new ceres::AutoDiffCostFunction<AccelerationCostFunction, 3, 4, 3,
3, 3, 1>(
new AccelerationCostFunction(
options_.acceleration_weight(), delta_velocity,
common::ToSeconds(first_duration),
common::ToSeconds(second_duration))),
nullptr, C_point_clouds[node_indices[j]].rotation(),
C_point_clouds[node_indices[j - 1]].translation(),
C_point_clouds[node_indices[j]].translation(),
C_point_clouds[node_indices[j + 1]].translation(),
&gravity_constant_);
}
problem.AddResidualBlock(
new ceres::AutoDiffCostFunction<RotationCostFunction, 3, 4, 4>(
new RotationCostFunction(options_.rotation_weight(),
result.delta_rotation)),
nullptr, C_point_clouds[node_indices[j - 1]].rotation(),
C_point_clouds[node_indices[j]].rotation());
}
problem.AddResidualBlock(
new ceres::AutoDiffCostFunction<RotationCostFunction, 3, 4, 4>(
new RotationCostFunction(options_.rotation_weight(),
result.delta_rotation)),
nullptr, C_point_clouds[j - 1].rotation(),
C_point_clouds[j].rotation());
}
// Solve.

14
cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h
View File

@ -36,6 +36,8 @@ namespace mapping_3d {
namespace sparse_pose_graph {
struct NodeData {
// TODO(whess): Keep nodes per trajectory instead.
const mapping::Submaps* trajectory;
common::Time time;
transform::Rigid3d point_cloud_pose;
};
@ -53,26 +55,24 @@ class OptimizationProblem {
OptimizationProblem(const OptimizationProblem&) = delete;
OptimizationProblem& operator=(const OptimizationProblem&) = delete;
void AddImuData(common::Time time, const Eigen::Vector3d& linear_acceleration,
void AddImuData(const mapping::Submaps* trajectory, common::Time time,
const Eigen::Vector3d& linear_acceleration,
const Eigen::Vector3d& angular_velocity);
void AddTrajectoryNode(common::Time time,
void AddTrajectoryNode(const mapping::Submaps* trajectory, common::Time time,
const transform::Rigid3d& point_cloud_pose);
void SetMaxNumIterations(int32 max_num_iterations);
// Computes the optimized poses. The point cloud at 'point_cloud_poses[i]'
// belongs to 'trajectories[i]'. Within a given trajectory, scans are expected
// to be contiguous.
// Computes the optimized poses.
void Solve(const std::vector<Constraint>& constraints,
const transform::Rigid3d& submap_0_transform,
const std::vector<const mapping::Submaps*>& trajectories,
std::vector<transform::Rigid3d>* submap_transforms);
const std::vector<NodeData>& node_data() const;
private:
mapping::sparse_pose_graph::proto::OptimizationProblemOptions options_;
std::deque<ImuData> imu_data_;
std::map<const mapping::Submaps*, std::deque<ImuData>> imu_data_;
std::vector<NodeData> node_data_;
double gravity_constant_ = 9.8;
};

9
cartographer/mapping_3d/sparse_pose_graph/optimization_problem_test.cc
View File

@ -118,15 +118,14 @@ TEST_F(OptimizationProblemTest, ReducesNoise) {
NoisyNode{RandomTransform(10., 3.), RandomYawOnlyTransform(0.2, 0.3)});
}
std::vector<const mapping::Submaps*> trajectories;
common::Time now = common::FromUniversal(0);
for (const NoisyNode& node : test_data) {
trajectories.push_back(kTrajectory);
const transform::Rigid3d pose =
AddNoise(node.ground_truth_pose, node.noise);
optimization_problem_.AddImuData(now, Eigen::Vector3d::UnitZ() * 9.81,
optimization_problem_.AddImuData(kTrajectory, now,
Eigen::Vector3d::UnitZ() * 9.81,
Eigen::Vector3d::Zero());
optimization_problem_.AddTrajectoryNode(now, pose);
optimization_problem_.AddTrajectoryNode(kTrajectory, now, pose);
now += common::FromSeconds(0.01);
}
@ -166,7 +165,7 @@ TEST_F(OptimizationProblemTest, ReducesNoise) {
node_data[j].point_cloud_pose);
}
optimization_problem_.Solve(constraints, kSubmap0Transform, trajectories,
optimization_problem_.Solve(constraints, kSubmap0Transform,
&submap_transforms);
double translation_error_after = 0.;