diff --git a/cartographer/mapping/trajectory_node.h b/cartographer/mapping/trajectory_node.h index e12c81c..9b0445c 100644 --- a/cartographer/mapping/trajectory_node.h +++ b/cartographer/mapping/trajectory_node.h @@ -35,16 +35,17 @@ struct TrajectoryNode { // Range data in 'tracking' frame. Only used in 3D. sensor::CompressedRangeData range_data; - // Used for loop closure in 2D: voxel filtered returns in 'pose' frame. - sensor::PointCloud filtered_point_cloud; + // Transform to approximately gravity align the tracking frame as + // determined by local SLAM. + Eigen::Quaterniond gravity_alignment; + + // Used for loop closure in 2D: voxel filtered returns in the + // 'gravity_alignment' frame. + sensor::PointCloud filtered_gravity_aligned_point_cloud; // Used for loop closure in 3D. sensor::PointCloud high_resolution_point_cloud; sensor::PointCloud low_resolution_point_cloud; - - // Transforms the 'tracking' frame into a gravity-aligned 'tracking_2d' - // frame. Only used in 2D. - transform::Rigid3d tracking_to_tracking_2d; }; common::Time time() const { return constant_data->time; } diff --git a/cartographer/mapping_2d/local_trajectory_builder.cc b/cartographer/mapping_2d/local_trajectory_builder.cc index 220f74d..02dbe40 100644 --- a/cartographer/mapping_2d/local_trajectory_builder.cc +++ b/cartographer/mapping_2d/local_trajectory_builder.cc @@ -181,11 +181,12 @@ LocalTrajectoryBuilder::AddAccumulatedRangeData( mapping::TrajectoryNode::Data{ time, {}, // 'range_data' is only used in 3D. + gravity_alignment.rotation(), filtered_gravity_aligned_point_cloud, {}, // 'high_resolution_point_cloud' is only used in 3D. {}, // 'low_resolution_point_cloud' is only used in 3D. - gravity_alignment}), - pose_estimate_2d, std::move(insertion_submaps)}); + }), + pose_estimate, std::move(insertion_submaps)}); } const mapping::PoseEstimate& LocalTrajectoryBuilder::pose_estimate() const { diff --git a/cartographer/mapping_2d/local_trajectory_builder.h b/cartographer/mapping_2d/local_trajectory_builder.h index a684380..447c6a2 100644 --- a/cartographer/mapping_2d/local_trajectory_builder.h +++ b/cartographer/mapping_2d/local_trajectory_builder.h @@ -42,7 +42,7 @@ class LocalTrajectoryBuilder { public: struct InsertionResult { std::shared_ptr constant_data; - transform::Rigid2d pose_observation; + transform::Rigid3d pose_observation; std::vector> insertion_submaps; }; diff --git a/cartographer/mapping_2d/sparse_pose_graph.cc b/cartographer/mapping_2d/sparse_pose_graph.cc index fb7f9f1..57d3724 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.cc +++ b/cartographer/mapping_2d/sparse_pose_graph.cc @@ -95,11 +95,10 @@ std::vector SparsePoseGraph::GrowSubmapTransformsAsNeeded( void SparsePoseGraph::AddScan( std::shared_ptr constant_data, - const transform::Rigid2d& pose, const int trajectory_id, + const transform::Rigid3d& pose, const int trajectory_id, const std::vector>& insertion_submaps) { const transform::Rigid3d optimized_pose( - GetLocalToGlobalTransform(trajectory_id) * transform::Embed3D(pose) * - constant_data->tracking_to_tracking_2d); + GetLocalToGlobalTransform(trajectory_id) * pose); common::MutexLocker locker(&mutex_); trajectory_nodes_.Append( @@ -132,8 +131,11 @@ void SparsePoseGraph::AddScan( // execute the lambda. const bool newly_finished_submap = insertion_submaps.front()->finished(); AddWorkItem([=]() REQUIRES(mutex_) { - ComputeConstraintsForScan(trajectory_id, insertion_submaps, - newly_finished_submap, pose); + ComputeConstraintsForScan( + trajectory_id, insertion_submaps, newly_finished_submap, + transform::Project2D(pose * + transform::Rigid3d::Rotation( + constant_data->gravity_alignment.inverse()))); }); } @@ -445,9 +447,10 @@ void SparsePoseGraph::RunOptimization() { for (; node_data_index != static_cast(node_data[trajectory_id].size()); ++node_data_index, ++node_index) { const mapping::NodeId node_id{trajectory_id, node_index}; - trajectory_nodes_.at(node_id).pose = + auto& node = trajectory_nodes_.at(node_id); + node.pose = transform::Embed3D(node_data[trajectory_id][node_data_index].pose) * - trajectory_nodes_.at(node_id).constant_data->tracking_to_tracking_2d; + transform::Rigid3d::Rotation(node.constant_data->gravity_alignment); } // Extrapolate all point cloud poses that were added later. const auto local_to_new_global = @@ -458,8 +461,8 @@ void SparsePoseGraph::RunOptimization() { local_to_new_global * local_to_old_global.inverse(); for (; node_index < num_nodes; ++node_index) { const mapping::NodeId node_id{trajectory_id, node_index}; - trajectory_nodes_.at(node_id).pose = - old_global_to_new_global * trajectory_nodes_.at(node_id).pose; + auto& node_pose = trajectory_nodes_.at(node_id).pose; + node_pose = old_global_to_new_global * node_pose; } } optimized_submap_transforms_ = submap_data; @@ -490,8 +493,9 @@ std::vector SparsePoseGraph::constraints() { result.push_back(Constraint{ constraint.submap_id, constraint.node_id, Constraint::Pose{constraint.pose.zbar_ij * - trajectory_nodes_.at(constraint.node_id) - .constant_data->tracking_to_tracking_2d, + transform::Rigid3d::Rotation( + trajectory_nodes_.at(constraint.node_id) + .constant_data->gravity_alignment), constraint.pose.translation_weight, constraint.pose.rotation_weight}, constraint.tag}); diff --git a/cartographer/mapping_2d/sparse_pose_graph.h b/cartographer/mapping_2d/sparse_pose_graph.h index eaa9968..7f3c38a 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.h +++ b/cartographer/mapping_2d/sparse_pose_graph.h @@ -71,7 +71,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { // 'true', this submap was inserted into for the last time. void AddScan( std::shared_ptr constant_data, - const transform::Rigid2d& pose, int trajectory_id, + const transform::Rigid3d& pose, int trajectory_id, const std::vector>& insertion_submaps) EXCLUDES(mutex_); diff --git a/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc b/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc index 3175e85..2a89761 100644 --- a/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc +++ b/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc @@ -172,7 +172,7 @@ void ConstraintBuilder::ComputeConstraint( GetSubmapScanMatcher(submap_id); // The 'constraint_transform' (submap i <- scan j) is computed from: - // - a 'filtered_point_cloud' in scan j, + // - a 'filtered_gravity_aligned_point_cloud' in scan j, // - the initial guess 'initial_pose' for (map <- scan j), // - the result 'pose_estimate' of Match() (map <- scan j). // - the ComputeSubmapPose() (map <- submap i) @@ -185,7 +185,7 @@ void ConstraintBuilder::ComputeConstraint( // 3. Refine. if (match_full_submap) { if (submap_scan_matcher->fast_correlative_scan_matcher->MatchFullSubmap( - constant_data->filtered_point_cloud, + constant_data->filtered_gravity_aligned_point_cloud, options_.global_localization_min_score(), &score, &pose_estimate)) { CHECK_GT(score, options_.global_localization_min_score()); CHECK_GE(node_id.trajectory_id, 0); @@ -197,7 +197,7 @@ void ConstraintBuilder::ComputeConstraint( } } else { if (submap_scan_matcher->fast_correlative_scan_matcher->Match( - initial_pose, constant_data->filtered_point_cloud, + initial_pose, constant_data->filtered_gravity_aligned_point_cloud, options_.min_score(), &score, &pose_estimate)) { // We've reported a successful local match. CHECK_GT(score, options_.min_score()); @@ -214,9 +214,10 @@ void ConstraintBuilder::ComputeConstraint( // effect that, in the absence of better information, we prefer the original // CSM estimate. ceres::Solver::Summary unused_summary; - ceres_scan_matcher_.Match( - pose_estimate, pose_estimate, constant_data->filtered_point_cloud, - *submap_scan_matcher->probability_grid, &pose_estimate, &unused_summary); + ceres_scan_matcher_.Match(pose_estimate, pose_estimate, + constant_data->filtered_gravity_aligned_point_cloud, + *submap_scan_matcher->probability_grid, + &pose_estimate, &unused_summary); const transform::Rigid2d constraint_transform = ComputeSubmapPose(*submap).inverse() * pose_estimate; @@ -230,8 +231,8 @@ void ConstraintBuilder::ComputeConstraint( if (options_.log_matches()) { std::ostringstream info; info << "Node " << node_id << " with " - << constant_data->filtered_point_cloud.size() << " points on submap " - << submap_id << std::fixed; + << constant_data->filtered_gravity_aligned_point_cloud.size() + << " points on submap " << submap_id << std::fixed; if (match_full_submap) { info << " matches"; } else { diff --git a/cartographer/mapping_2d/sparse_pose_graph_test.cc b/cartographer/mapping_2d/sparse_pose_graph_test.cc index f7216e4..f641491 100644 --- a/cartographer/mapping_2d/sparse_pose_graph_test.cc +++ b/cartographer/mapping_2d/sparse_pose_graph_test.cc @@ -161,11 +161,11 @@ class SparsePoseGraphTest : public ::testing::Test { std::make_shared( mapping::TrajectoryNode::Data{common::FromUniversal(0), Compress(range_data), + Eigen::Quaterniond::Identity(), range_data.returns, {}, - {}, - transform::Rigid3d::Identity()}), - pose_estimate, kTrajectoryId, insertion_submaps); + {}}), + transform::Embed3D(pose_estimate), kTrajectoryId, insertion_submaps); } void MoveRelative(const transform::Rigid2d& movement) { diff --git a/cartographer/mapping_3d/local_trajectory_builder.cc b/cartographer/mapping_3d/local_trajectory_builder.cc index f61e4a5..fcce9f4 100644 --- a/cartographer/mapping_3d/local_trajectory_builder.cc +++ b/cartographer/mapping_3d/local_trajectory_builder.cc @@ -210,10 +210,10 @@ LocalTrajectoryBuilder::InsertIntoSubmap( mapping::TrajectoryNode::Data{ time, sensor::Compress(range_data_in_tracking), + gravity_alignment, {}, // 'filtered_point_cloud' is only used in 2D. high_resolution_point_cloud, - low_resolution_point_cloud, - {}}), // 'tracking_to_tracking_2d' in only used in 2D. + low_resolution_point_cloud}), pose_observation, std::move(insertion_submaps)}); } diff --git a/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher_test.cc b/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher_test.cc index 5994925..f7c3dce 100644 --- a/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher_test.cc +++ b/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher_test.cc @@ -114,10 +114,10 @@ class FastCorrelativeScanMatcherTest : public ::testing::Test { return mapping::TrajectoryNode::Data{ common::FromUniversal(0), Compress(sensor::RangeData{Eigen::Vector3f::Zero(), point_cloud_, {}}), + Eigen::Quaterniond::Identity(), {}, point_cloud_, - low_resolution_point_cloud, - transform::Rigid3d::Identity()}; + low_resolution_point_cloud}; } std::mt19937 prng_ = std::mt19937(42);