From 56fc2a9a9221953849c83cd61c0ac4d289cde95d Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Thu, 18 May 2017 16:55:11 +0200 Subject: [PATCH] Fix 3D SLAM loop closure issues. (#296) 3D submaps are now oriented approximately gravity aligned. This is so that accumulating error in the local SLAM frame is no longer a problem for finding loop closures. It also ensures that the z search window size is approximately in the gravity direction. We now also pass an estimate of gravity orientation when doing multi-trajectory matches. Otherwise trajectories starting with different orientation relative to gravity could not be connected. The gravity alignment is currently derived from the ImuTracker. It might be possible to further improve on this by using the latest gravity direction from the optimized poses. --- cartographer/kalman_filter/pose_tracker.h | 4 ++ cartographer/mapping/imu_tracker.h | 2 +- cartographer/mapping/submaps.h | 14 +++--- cartographer/mapping_2d/sparse_pose_graph.cc | 10 ++--- .../sparse_pose_graph/constraint_builder.cc | 2 +- cartographer/mapping_2d/submaps.cc | 5 ++- .../kalman_local_trajectory_builder.cc | 10 +++-- .../optimizing_local_trajectory_builder.cc | 13 ++++-- cartographer/mapping_3d/sparse_pose_graph.cc | 44 ++++++++++++------- .../sparse_pose_graph/constraint_builder.cc | 9 ++-- .../sparse_pose_graph/constraint_builder.h | 5 ++- cartographer/mapping_3d/submaps.cc | 25 ++++++----- cartographer/mapping_3d/submaps.h | 12 +++-- 13 files changed, 93 insertions(+), 62 deletions(-) diff --git a/cartographer/kalman_filter/pose_tracker.h b/cartographer/kalman_filter/pose_tracker.h index 73b30e0..d0bd5f3 100644 --- a/cartographer/kalman_filter/pose_tracker.h +++ b/cartographer/kalman_filter/pose_tracker.h @@ -122,6 +122,10 @@ class PoseTracker { const mapping::OdometryStateTracker::OdometryStates& odometry_states() const; + Eigen::Quaterniond gravity_orientation() const { + return imu_tracker_.orientation(); + } + private: // Returns the distribution required to initialize the KalmanFilter. static Distribution KalmanFilterInit(); diff --git a/cartographer/mapping/imu_tracker.h b/cartographer/mapping/imu_tracker.h index 2e0aa6a..a1a755c 100644 --- a/cartographer/mapping/imu_tracker.h +++ b/cartographer/mapping/imu_tracker.h @@ -41,7 +41,7 @@ class ImuTracker { const Eigen::Vector3d& imu_angular_velocity); // Query the current orientation estimate. - Eigen::Quaterniond orientation() { return orientation_; } + Eigen::Quaterniond orientation() const { return orientation_; } private: const double imu_gravity_time_constant_; diff --git a/cartographer/mapping/submaps.h b/cartographer/mapping/submaps.h index 332d8b8..1e85a4f 100644 --- a/cartographer/mapping/submaps.h +++ b/cartographer/mapping/submaps.h @@ -52,19 +52,15 @@ inline uint8 ProbabilityToLogOddsInteger(const float probability) { return value; } -// An individual submap, which has an initial position 'origin', keeps track of -// how many range data were inserted into it, and sets the +// An individual submap, which has a 'local_pose' in the local SLAM frame, keeps +// track of how many range data were inserted into it, and sets the // 'finished_probability_grid' to be used for loop closing once the map no // longer changes. struct Submap { - Submap(const Eigen::Vector3f& origin) : origin(origin) {} + Submap(const transform::Rigid3d& local_pose) : local_pose(local_pose) {} - transform::Rigid3d local_pose() const { - return transform::Rigid3d::Translation(origin.cast()); - } - - // Origin of this submap. - const Eigen::Vector3f origin; + // Local SLAM pose of this submap. + const transform::Rigid3d local_pose; // Number of RangeData inserted. int num_range_data = 0; diff --git a/cartographer/mapping_2d/sparse_pose_graph.cc b/cartographer/mapping_2d/sparse_pose_graph.cc index 155be5e..d699d39 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.cc +++ b/cartographer/mapping_2d/sparse_pose_graph.cc @@ -441,8 +441,7 @@ transform::Rigid3d SparsePoseGraph::GetLocalToGlobalTransform( return extrapolated_submap_transforms.back() * submap_states_.at(trajectory_id) .at(extrapolated_submap_transforms.size() - 1) - .submap->local_pose() - .inverse(); + .submap->local_pose.inverse(); } std::vector> SparsePoseGraph::GetConnectedTrajectories() { @@ -476,14 +475,13 @@ std::vector SparsePoseGraph::ExtrapolateSubmapTransforms( } else if (result.empty()) { result.push_back(transform::Rigid3d::Identity()); } else { - // Extrapolate to the remaining submaps. Accessing local_pose() in Submaps + // Extrapolate to the remaining submaps. Accessing 'local_pose' in Submaps // is okay, since the member is const. result.push_back(result.back() * submap_states_.at(trajectory_id) .at(result.size() - 1) - .submap->local_pose() - .inverse() * - state.submap->local_pose()); + .submap->local_pose.inverse() * + state.submap->local_pose); } } diff --git a/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc b/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc index eb3a820..d602b91 100644 --- a/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc +++ b/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc @@ -40,7 +40,7 @@ namespace mapping_2d { namespace sparse_pose_graph { transform::Rigid2d ComputeSubmapPose(const mapping::Submap& submap) { - return transform::Project2D(submap.local_pose()); + return transform::Project2D(submap.local_pose); } ConstraintBuilder::ConstraintBuilder( diff --git a/cartographer/mapping_2d/submaps.cc b/cartographer/mapping_2d/submaps.cc index 1ea41a2..9dfa001 100644 --- a/cartographer/mapping_2d/submaps.cc +++ b/cartographer/mapping_2d/submaps.cc @@ -102,7 +102,8 @@ proto::SubmapsOptions CreateSubmapsOptions( } Submap::Submap(const MapLimits& limits, const Eigen::Vector2f& origin) - : mapping::Submap(Eigen::Vector3f(origin.x(), origin.y(), 0.)), + : mapping::Submap(transform::Rigid3d::Translation( + Eigen::Vector3d(origin.x(), origin.y(), 0.))), probability_grid(limits) {} Submaps::Submaps(const proto::SubmapsOptions& options) @@ -137,7 +138,7 @@ int Submaps::size() const { return submaps_.size(); } void Submaps::SubmapToProto( const int index, const transform::Rigid3d&, mapping::proto::SubmapQuery::Response* const response) const { - AddProbabilityGridToResponse(Get(index)->local_pose(), + AddProbabilityGridToResponse(Get(index)->local_pose, Get(index)->probability_grid, response); } diff --git a/cartographer/mapping_3d/kalman_local_trajectory_builder.cc b/cartographer/mapping_3d/kalman_local_trajectory_builder.cc index 69cb072..eea1691 100644 --- a/cartographer/mapping_3d/kalman_local_trajectory_builder.cc +++ b/cartographer/mapping_3d/kalman_local_trajectory_builder.cc @@ -143,7 +143,7 @@ KalmanLocalTrajectoryBuilder::AddAccumulatedRangeData( const Submap* const matching_submap = submaps_->Get(submaps_->matching_index()); transform::Rigid3d initial_ceres_pose = - matching_submap->local_pose().inverse() * pose_prediction; + matching_submap->local_pose.inverse() * pose_prediction; sensor::AdaptiveVoxelFilter adaptive_voxel_filter( options_.high_resolution_adaptive_voxel_filter_options()); const sensor::PointCloud filtered_point_cloud_in_tracking = @@ -171,7 +171,7 @@ KalmanLocalTrajectoryBuilder::AddAccumulatedRangeData( &matching_submap->low_resolution_hybrid_grid}}, &pose_observation_in_submap, &summary); const transform::Rigid3d pose_observation = - matching_submap->local_pose() * pose_observation_in_submap; + matching_submap->local_pose * pose_observation_in_submap; pose_tracker_->AddPoseObservation( time, pose_observation, options_.kalman_local_trajectory_builder_options() @@ -227,8 +227,10 @@ KalmanLocalTrajectoryBuilder::InsertIntoSubmap( for (int insertion_index : submaps_->insertion_indices()) { insertion_submaps.push_back(submaps_->Get(insertion_index)); } - submaps_->InsertRangeData(sensor::TransformRangeData( - range_data_in_tracking, pose_observation.cast())); + submaps_->InsertRangeData( + sensor::TransformRangeData(range_data_in_tracking, + pose_observation.cast()), + pose_tracker_->gravity_orientation()); return std::unique_ptr(new InsertionResult{ time, range_data_in_tracking, pose_observation, covariance_estimate, matching_submap, insertion_submaps}); diff --git a/cartographer/mapping_3d/optimizing_local_trajectory_builder.cc b/cartographer/mapping_3d/optimizing_local_trajectory_builder.cc index ab1c927..61c215c 100644 --- a/cartographer/mapping_3d/optimizing_local_trajectory_builder.cc +++ b/cartographer/mapping_3d/optimizing_local_trajectory_builder.cc @@ -242,7 +242,7 @@ OptimizingLocalTrajectoryBuilder::MaybeOptimize(const common::Time time) { // We transform the states in 'batches_' in place to be in the submap frame as // expected by the OccupiedSpaceCostFunctor. This is reverted after solving // the optimization problem. - TransformStates(matching_submap->local_pose().inverse()); + TransformStates(matching_submap->local_pose.inverse()); for (size_t i = 0; i < batches_.size(); ++i) { Batch& batch = batches_[i]; problem.AddResidualBlock( @@ -351,7 +351,7 @@ OptimizingLocalTrajectoryBuilder::MaybeOptimize(const common::Time time) { ceres::Solve(ceres_solver_options_, &problem, &summary); // The optimized states in 'batches_' are in the submap frame and we transform // them in place to be in the local SLAM frame again. - TransformStates(matching_submap->local_pose()); + TransformStates(matching_submap->local_pose); if (num_accumulated_ < options_.scans_per_accumulation()) { return nullptr; } @@ -416,8 +416,13 @@ OptimizingLocalTrajectoryBuilder::InsertIntoSubmap( for (int insertion_index : submaps_->insertion_indices()) { insertion_submaps.push_back(submaps_->Get(insertion_index)); } - submaps_->InsertRangeData(sensor::TransformRangeData( - range_data_in_tracking, pose_observation.cast())); + // TODO(whess): Use an ImuTracker to track the gravity direction. + const Eigen::Quaterniond kFakeGravityOrientation = + Eigen::Quaterniond::Identity(); + submaps_->InsertRangeData( + sensor::TransformRangeData(range_data_in_tracking, + pose_observation.cast()), + kFakeGravityOrientation); const kalman_filter::PoseCovariance kCovariance = 1e-7 * kalman_filter::PoseCovariance::Identity(); diff --git a/cartographer/mapping_3d/sparse_pose_graph.cc b/cartographer/mapping_3d/sparse_pose_graph.cc index 686c2c2..2f87f39 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.cc +++ b/cartographer/mapping_3d/sparse_pose_graph.cc @@ -81,8 +81,8 @@ void SparsePoseGraph::GrowSubmapTransformsAsNeeded( submap_data.at(trajectory_id).at(first_submap_id.submap_index).pose; optimization_problem_.AddSubmap( trajectory_id, first_submap_pose * - insertion_submaps[0]->local_pose().inverse() * - insertion_submaps[1]->local_pose()); + insertion_submaps[0]->local_pose.inverse() * + insertion_submaps[1]->local_pose); } } @@ -164,6 +164,12 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id, .at(submap_id.submap_index) .pose.inverse(); + const transform::Rigid3d initial_relative_pose = + inverse_submap_pose * optimization_problem_.node_data() + .at(node_id.trajectory_id) + .at(node_id.node_index) + .point_cloud_pose; + std::vector submap_nodes; for (const mapping::NodeId& submap_node_id : submap_states_.at(submap_id.trajectory_id) @@ -181,6 +187,18 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id, // Only globally match against submaps not in this trajectory. if (node_id.trajectory_id != submap_id.trajectory_id && global_localization_samplers_[node_id.trajectory_id]->Pulse()) { + // In this situation, 'initial_relative_pose' is: + // + // submap <- global map 2 <- global map 1 <- tracking + // (agreeing on gravity) + // + // Since they possibly came from two disconnected trajectories, the only + // guaranteed connection between the tracking and the submap frames is + // an agreement on the direction of gravity. Therefore, excluding yaw, + // 'initial_relative_pose.rotation()' is a good estimate of the relative + // orientation of the point cloud in the submap frame. Finding the correct + // yaw component will be handled by the matching procedure in the + // FastCorrelativeScanMatcher, and the given yaw is essentially ignored. constraint_builder_.MaybeAddGlobalConstraint( submap_id, submap_states_.at(submap_id.trajectory_id) @@ -190,7 +208,8 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id, &trajectory_nodes_.at(node_id.trajectory_id) .at(node_id.node_index) .constant_data->range_data_3d.returns, - submap_nodes, &trajectory_connectivity_); + submap_nodes, initial_relative_pose.rotation(), + &trajectory_connectivity_); } else { const bool scan_and_submap_trajectories_connected = reverse_connected_components_.count(node_id.trajectory_id) > 0 && @@ -199,11 +218,6 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id, reverse_connected_components_.at(submap_id.trajectory_id); if (node_id.trajectory_id == submap_id.trajectory_id || scan_and_submap_trajectories_connected) { - const transform::Rigid3d initial_relative_pose = - inverse_submap_pose * optimization_problem_.node_data() - .at(node_id.trajectory_id) - .at(node_id.node_index) - .point_cloud_pose; constraint_builder_.MaybeAddConstraint( submap_id, submap_states_.at(submap_id.trajectory_id) @@ -242,7 +256,7 @@ void SparsePoseGraph::ComputeConstraintsForScan( .at(matching_id.trajectory_id) .at(matching_id.submap_index) .pose * - matching_submap->local_pose().inverse() * pose; + matching_submap->local_pose.inverse() * pose; CHECK_EQ(scan_index, scan_index_to_node_id_.size()); const mapping::NodeId node_id{ matching_id.trajectory_id, @@ -266,7 +280,7 @@ void SparsePoseGraph::ComputeConstraintsForScan( .node_ids.emplace(node_id); // Unchanged covariance as (submap <- map) is a translation. const transform::Rigid3d constraint_transform = - submap->local_pose().inverse() * pose; + submap->local_pose.inverse() * pose; constraints_.push_back( Constraint{submap_id, node_id, @@ -447,8 +461,7 @@ transform::Rigid3d SparsePoseGraph::GetLocalToGlobalTransform( return extrapolated_submap_transforms.back() * submap_states_.at(trajectory_id) .at(extrapolated_submap_transforms.size() - 1) - .submap->local_pose() - .inverse(); + .submap->local_pose.inverse(); } std::vector> SparsePoseGraph::GetConnectedTrajectories() { @@ -482,14 +495,13 @@ std::vector SparsePoseGraph::ExtrapolateSubmapTransforms( } else if (result.empty()) { result.push_back(transform::Rigid3d::Identity()); } else { - // Extrapolate to the remaining submaps. Accessing local_pose() in Submaps + // Extrapolate to the remaining submaps. Accessing 'local_pose' in Submaps // is okay, since the member is const. result.push_back(result.back() * submap_states_.at(trajectory_id) .at(result.size() - 1) - .submap->local_pose() - .inverse() * - state.submap->local_pose()); + .submap->local_pose.inverse() * + state.submap->local_pose); } } diff --git a/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc b/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc index ef2b4c4..079b6f6 100644 --- a/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc +++ b/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc @@ -88,6 +88,7 @@ void ConstraintBuilder::MaybeAddGlobalConstraint( const mapping::NodeId& node_id, const sensor::CompressedPointCloud* const compressed_point_cloud, const std::vector& submap_nodes, + const Eigen::Quaterniond& gravity_alignment, mapping::TrajectoryConnectivity* const trajectory_connectivity) { common::MutexLocker locker(&mutex_); constraints_.emplace_back(); @@ -97,10 +98,10 @@ void ConstraintBuilder::MaybeAddGlobalConstraint( ScheduleSubmapScanMatcherConstructionAndQueueWorkItem( submap_id, submap_nodes, &submap->high_resolution_hybrid_grid, [=]() EXCLUDES(mutex_) { - ComputeConstraint(submap_id, submap, node_id, - true, /* match_full_submap */ - trajectory_connectivity, compressed_point_cloud, - transform::Rigid3d::Identity(), constraint); + ComputeConstraint( + submap_id, submap, node_id, true, /* match_full_submap */ + trajectory_connectivity, compressed_point_cloud, + transform::Rigid3d::Rotation(gravity_alignment), constraint); FinishComputation(current_computation); }); } diff --git a/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.h b/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.h index 9778b48..91916f3 100644 --- a/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.h +++ b/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.h @@ -84,7 +84,9 @@ class ConstraintBuilder { // 'submap_id' and the 'compressed_point_cloud' for 'node_id'. // This performs full-submap matching. // - // The 'trajectory_connectivity' is updated if the full-submap match succeeds. + // The 'gravity_alignment' is the rotation to apply to the point cloud data + // to make it approximately gravity aligned. The 'trajectory_connectivity' is + // updated if the full-submap match succeeds. // // The pointees of 'submap' and 'compressed_point_cloud' must stay valid until // all computations are finished. @@ -93,6 +95,7 @@ class ConstraintBuilder { const mapping::NodeId& node_id, const sensor::CompressedPointCloud* compressed_point_cloud, const std::vector& submap_nodes, + const Eigen::Quaterniond& gravity_alignment, mapping::TrajectoryConnectivity* trajectory_connectivity); // Must be called after all computations related to one node have been added. diff --git a/cartographer/mapping_3d/submaps.cc b/cartographer/mapping_3d/submaps.cc index f919315..d781acd 100644 --- a/cartographer/mapping_3d/submaps.cc +++ b/cartographer/mapping_3d/submaps.cc @@ -217,17 +217,20 @@ proto::SubmapsOptions CreateSubmapsOptions( } Submap::Submap(const float high_resolution, const float low_resolution, - const Eigen::Vector3f& origin) - : mapping::Submap(origin), + const transform::Rigid3d& local_pose) + : mapping::Submap(local_pose), high_resolution_hybrid_grid(high_resolution), low_resolution_hybrid_grid(low_resolution) {} Submaps::Submaps(const proto::SubmapsOptions& options) : options_(options), range_data_inserter_(options.range_data_inserter_options()) { - // We always want to have at least one likelihood field which we can return, - // and will create it at the origin in absence of a better choice. - AddSubmap(Eigen::Vector3f::Zero()); + // We always want to have at least one submap which we can return and will + // create it at the origin in absence of a better choice. + // + // TODO(whess): Start with no submaps, so that all of them can be + // approximately gravity aligned. + AddSubmap(transform::Rigid3d::Identity()); } const Submap* Submaps::Get(int index) const { @@ -271,11 +274,12 @@ void Submaps::SubmapToProto( global_submap_pose.translation().z()))); } -void Submaps::InsertRangeData(const sensor::RangeData& range_data) { +void Submaps::InsertRangeData(const sensor::RangeData& range_data, + const Eigen::Quaterniond& gravity_alignment) { for (const int index : insertion_indices()) { Submap* submap = submaps_[index].get(); const sensor::RangeData transformed_range_data = sensor::TransformRangeData( - range_data, submap->local_pose().inverse().cast()); + range_data, submap->local_pose.inverse().cast()); range_data_inserter_.Insert( FilterRangeDataByMaxRange(transformed_range_data, options_.high_resolution_max_range()), @@ -286,18 +290,19 @@ void Submaps::InsertRangeData(const sensor::RangeData& range_data) { } const Submap* const last_submap = Get(size() - 1); if (last_submap->num_range_data == options_.num_range_data()) { - AddSubmap(range_data.origin); + AddSubmap(transform::Rigid3d(range_data.origin.cast(), + gravity_alignment)); } } -void Submaps::AddSubmap(const Eigen::Vector3f& origin) { +void Submaps::AddSubmap(const transform::Rigid3d& local_pose) { if (size() > 1) { Submap* submap = submaps_[size() - 2].get(); CHECK(!submap->finished); submap->finished = true; } submaps_.emplace_back(new Submap(options_.high_resolution(), - options_.low_resolution(), origin)); + options_.low_resolution(), local_pose)); LOG(INFO) << "Added submap " << size(); } diff --git a/cartographer/mapping_3d/submaps.h b/cartographer/mapping_3d/submaps.h index 08caa5f..8c9a67a 100644 --- a/cartographer/mapping_3d/submaps.h +++ b/cartographer/mapping_3d/submaps.h @@ -32,6 +32,7 @@ #include "cartographer/mapping_3d/proto/submaps_options.pb.h" #include "cartographer/mapping_3d/range_data_inserter.h" #include "cartographer/sensor/range_data.h" +#include "cartographer/transform/rigid_transform.h" #include "cartographer/transform/transform.h" namespace cartographer { @@ -48,7 +49,7 @@ proto::SubmapsOptions CreateSubmapsOptions( struct Submap : public mapping::Submap { Submap(float high_resolution, float low_resolution, - const Eigen::Vector3f& origin); + const transform::Rigid3d& local_pose); HybridGrid high_resolution_hybrid_grid; HybridGrid low_resolution_hybrid_grid; @@ -69,8 +70,11 @@ class Submaps : public mapping::Submaps { int index, const transform::Rigid3d& global_submap_pose, mapping::proto::SubmapQuery::Response* response) const override; - // Inserts 'range_data' into the Submap collection. - void InsertRangeData(const sensor::RangeData& range_data); + // Inserts 'range_data' into the Submap collection. 'gravity_alignment' is + // used for the orientation of new submaps so that the z axis approximately + // aligns with gravity. + void InsertRangeData(const sensor::RangeData& range_data, + const Eigen::Quaterniond& gravity_alignment); private: struct PixelData { @@ -81,7 +85,7 @@ class Submaps : public mapping::Submaps { float max_probability = 0.5f; }; - void AddSubmap(const Eigen::Vector3f& origin); + void AddSubmap(const transform::Rigid3d& local_pose); std::vector AccumulatePixelData( const int width, const int height, const Eigen::Array2i& min_index,