diff --git a/cartographer/mapping/id.h b/cartographer/mapping/id.h new file mode 100644 index 0000000..d550694 --- /dev/null +++ b/cartographer/mapping/id.h @@ -0,0 +1,61 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_MAPPING_ID_H_ +#define CARTOGRAPHER_MAPPING_ID_H_ + +#include +#include + +namespace cartographer { +namespace mapping { + +// Uniquely identifies a trajectory node using a combination of a unique +// trajectory ID and a zero-based index of the node inside that trajectory. +struct NodeId { + int trajectory_id; + int node_index; + + bool operator<(const NodeId& other) const { + return std::forward_as_tuple(trajectory_id, node_index) < + std::forward_as_tuple(other.trajectory_id, other.node_index); + } +}; + +inline std::ostream& operator<<(std::ostream& os, const NodeId& v) { + return os << "(" << v.trajectory_id << ", " << v.node_index << ")"; +} + +// Uniquely identifies a submap using a combination of a unique trajectory ID +// and a zero-based index of the submap inside that trajectory. +struct SubmapId { + int trajectory_id; + int submap_index; + + bool operator<(const SubmapId& other) const { + return std::forward_as_tuple(trajectory_id, submap_index) < + std::forward_as_tuple(other.trajectory_id, other.submap_index); + } +}; + +inline std::ostream& operator<<(std::ostream& os, const SubmapId& v) { + return os << "(" << v.trajectory_id << ", " << v.submap_index << ")"; +} + +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_ID_H_ diff --git a/cartographer/mapping/sparse_pose_graph.cc b/cartographer/mapping/sparse_pose_graph.cc index 6bb0474..61b5cba 100644 --- a/cartographer/mapping/sparse_pose_graph.cc +++ b/cartographer/mapping/sparse_pose_graph.cc @@ -98,9 +98,9 @@ proto::SparsePoseGraph SparsePoseGraph::ToProto() { constraint.submap_id.submap_index); constraint_proto->mutable_scan_id()->set_trajectory_id( - grouped_node_indices[constraint.j].first); + constraint.node_id.trajectory_id); constraint_proto->mutable_scan_id()->set_scan_index( - grouped_node_indices[constraint.j].second); + constraint.node_id.node_index); constraint_proto->set_tag(mapping::ToProto(constraint.tag)); } diff --git a/cartographer/mapping/sparse_pose_graph.h b/cartographer/mapping/sparse_pose_graph.h index 352e5ce..a82eac8 100644 --- a/cartographer/mapping/sparse_pose_graph.h +++ b/cartographer/mapping/sparse_pose_graph.h @@ -23,6 +23,7 @@ #include #include "cartographer/common/lua_parameter_dictionary.h" +#include "cartographer/mapping/id.h" #include "cartographer/mapping/proto/sparse_pose_graph.pb.h" #include "cartographer/mapping/proto/sparse_pose_graph_options.pb.h" #include "cartographer/mapping/submaps.h" @@ -59,9 +60,7 @@ class SparsePoseGraph { }; mapping::SubmapId submap_id; // 'i' in the paper. - - // Scan index. - int j; + mapping::NodeId node_id; // 'j' in the paper. // Pose of the scan 'j' relative to submap 'i'. Pose pose; diff --git a/cartographer/mapping/submaps.h b/cartographer/mapping/submaps.h index d7a2b6c..17609a6 100644 --- a/cartographer/mapping/submaps.h +++ b/cartographer/mapping/submaps.h @@ -14,18 +14,6 @@ * limitations under the License. */ -// Submaps is a sequence of maps to which scans are matched and into which scans -// are inserted. -// -// Except during initialization when only a single submap exists, there are -// always two submaps into which scans are inserted: an old submap that is used -// for matching, and a new one, which will be used for matching next, that is -// being initialized. -// -// Once a certain number of scans have been inserted, the new submap is -// considered initialized: the old submap is no longer changed, the "new" submap -// is now the "old" submap and is used for scan-to-map matching. Moreover, -// a "new" submap gets inserted. #ifndef CARTOGRAPHER_MAPPING_SUBMAPS_H_ #define CARTOGRAPHER_MAPPING_SUBMAPS_H_ @@ -35,6 +23,7 @@ #include "Eigen/Geometry" #include "cartographer/common/math.h" #include "cartographer/common/port.h" +#include "cartographer/mapping/id.h" #include "cartographer/mapping/probability_values.h" #include "cartographer/mapping/proto/submap_visualization.pb.h" #include "cartographer/mapping/trajectory_node.h" @@ -63,22 +52,6 @@ inline uint8 ProbabilityToLogOddsInteger(const float probability) { return value; } -// Uniquely identifies a submap using a combination of a unique trajectory ID -// and a zero-based index of the submap inside that trajectory. -struct SubmapId { - int trajectory_id; - int submap_index; - - bool operator<(const SubmapId& other) const { - return std::forward_as_tuple(trajectory_id, submap_index) < - std::forward_as_tuple(other.trajectory_id, other.submap_index); - } -}; - -inline std::ostream& operator<<(std::ostream& os, const SubmapId& v) { - return os << "(" << v.trajectory_id << ", " << v.submap_index << ")"; -} - // An individual submap, which has an initial position 'origin', 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 @@ -102,7 +75,18 @@ struct Submap { const mapping_2d::ProbabilityGrid* finished_probability_grid = nullptr; }; -// A container of Submaps. +// Submaps is a sequence of maps to which scans are matched and into which scans +// are inserted. +// +// Except during initialization when only a single submap exists, there are +// always two submaps into which scans are inserted: an old submap that is used +// for matching, and a new one, which will be used for matching next, that is +// being initialized. +// +// Once a certain number of scans have been inserted, the new submap is +// considered initialized: the old submap is no longer changed, the "new" submap +// is now the "old" submap and is used for scan-to-map matching. Moreover, +// a "new" submap gets inserted. class Submaps { public: static constexpr uint8 kUnknownLogOdds = 0; diff --git a/cartographer/mapping_2d/sparse_pose_graph.cc b/cartographer/mapping_2d/sparse_pose_graph.cc index 5c05aec..5bb43c0 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.cc +++ b/cartographer/mapping_2d/sparse_pose_graph.cc @@ -100,8 +100,11 @@ void SparsePoseGraph::AddScan( common::MutexLocker locker(&mutex_); trajectory_ids_.emplace(trajectory, trajectory_ids_.size()); const int trajectory_id = trajectory_ids_.at(trajectory); - const int j = trajectory_nodes_.size(); - CHECK_LT(j, std::numeric_limits::max()); + const int flat_scan_index = trajectory_nodes_.size(); + CHECK_LT(flat_scan_index, std::numeric_limits::max()); + scan_index_to_node_id_.push_back( + mapping::NodeId{trajectory_id, num_nodes_in_trajectory_[trajectory_id]}); + ++num_nodes_in_trajectory_[trajectory_id]; constant_node_data_.push_back(mapping::TrajectoryNode::ConstantData{ time, range_data_in_pose, @@ -135,7 +138,7 @@ void SparsePoseGraph::AddScan( } AddWorkItem([=]() REQUIRES(mutex_) { - ComputeConstraintsForScan(j, matching_submap, insertion_submaps, + ComputeConstraintsForScan(flat_scan_index, matching_submap, insertion_submaps, finished_submap, pose, covariance); }); } @@ -180,8 +183,9 @@ void SparsePoseGraph::ComputeConstraint(const int scan_index, if (scan_trajectory_id != submap_trajectory_id && global_localization_samplers_[scan_trajectory_id]->Pulse()) { constraint_builder_.MaybeAddGlobalConstraint( - submap_id, submap_states_[submap_index].submap, scan_index, - scan_trajectory_id, &trajectory_connectivity_, + submap_id, submap_states_[submap_index].submap, + scan_index_to_node_id_.at(scan_index), scan_index, + &trajectory_connectivity_, &trajectory_nodes_[scan_index].constant_data->range_data_2d.returns); } else { const bool scan_and_submap_trajectories_connected = @@ -192,7 +196,8 @@ void SparsePoseGraph::ComputeConstraint(const int scan_index, if (scan_trajectory_id == submap_trajectory_id || scan_and_submap_trajectories_connected) { constraint_builder_.MaybeAddConstraint( - submap_id, submap_states_[submap_index].submap, scan_index, + submap_id, submap_states_[submap_index].submap, + scan_index_to_node_id_.at(scan_index), scan_index, &trajectory_nodes_[scan_index].constant_data->range_data_2d.returns, relative_pose); } @@ -243,7 +248,7 @@ void SparsePoseGraph::ComputeConstraintsForScan( constexpr double kFakeOrientationCovariance = 1e-6; constraints_.push_back(Constraint{ submap_states_[submap_index].id, - scan_index, + scan_index_to_node_id_.at(scan_index), {transform::Embed3D(constraint_transform), common::ComputeSpdMatrixSqrtInverse( kalman_filter::Embed3D(covariance, kFakePositionCovariance, diff --git a/cartographer/mapping_2d/sparse_pose_graph.h b/cartographer/mapping_2d/sparse_pose_graph.h index b3b52a5..96da50c 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.h +++ b/cartographer/mapping_2d/sparse_pose_graph.h @@ -14,16 +14,6 @@ * limitations under the License. */ -// Implements the loop closure method called Sparse Pose Adjustment (SPA) from -// Konolige, Kurt, et al. "Efficient sparse pose adjustment for 2d mapping." -// Intelligent Robots and Systems (IROS), 2010 IEEE/RSJ International Conference -// on (pp. 22--29). IEEE, 2010. -// -// It is extended for submapping: -// Each scan has been matched against one or more submaps (adding a constraint -// for each match), both poses of scans and of submaps are to be optimized. -// All constraints are between a submap i and a scan j. - #ifndef CARTOGRAPHER_MAPPING_2D_SPARSE_POSE_GRAPH_H_ #define CARTOGRAPHER_MAPPING_2D_SPARSE_POSE_GRAPH_H_ @@ -53,7 +43,15 @@ namespace cartographer { namespace mapping_2d { -// Implements the SPA loop closure method. +// Implements the loop closure method called Sparse Pose Adjustment (SPA) from +// Konolige, Kurt, et al. "Efficient sparse pose adjustment for 2d mapping." +// Intelligent Robots and Systems (IROS), 2010 IEEE/RSJ International Conference +// on (pp. 22--29). IEEE, 2010. +// +// It is extended for submapping: +// Each scan has been matched against one or more submaps (adding a constraint +// for each match), both poses of scans and of submaps are to be optimized. +// All constraints are between a submap i and a scan j. class SparsePoseGraph : public mapping::SparsePoseGraph { public: SparsePoseGraph(const mapping::proto::SparsePoseGraphOptions& options, @@ -199,6 +197,11 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { std::vector submap_states_ GUARDED_BY(mutex_); std::map num_submaps_in_trajectory_ GUARDED_BY(mutex_); + // Mapping to flat indices to aid the transition to per-trajectory data + // structures. + std::map num_nodes_in_trajectory_ GUARDED_BY(mutex_); + std::vector scan_index_to_node_id_ GUARDED_BY(mutex_); + // Connectivity structure of our trajectories by IDs. std::vector> connected_components_; // Trajectory ID to connected component ID. diff --git a/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc b/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc index 462085c..6cb88e8 100644 --- a/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc +++ b/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc @@ -62,7 +62,8 @@ ConstraintBuilder::~ConstraintBuilder() { void ConstraintBuilder::MaybeAddConstraint( const mapping::SubmapId& submap_id, const mapping::Submap* const submap, - const int scan_index, const sensor::PointCloud* const point_cloud, + const mapping::NodeId& node_id, const int flat_scan_index, + const sensor::PointCloud* const point_cloud, const transform::Rigid2d& initial_relative_pose) { if (initial_relative_pose.translation().norm() > options_.max_constraint_distance()) { @@ -70,15 +71,14 @@ void ConstraintBuilder::MaybeAddConstraint( } if (sampler_.Pulse()) { common::MutexLocker locker(&mutex_); - CHECK_LE(scan_index, current_computation_); + CHECK_LE(flat_scan_index, current_computation_); constraints_.emplace_back(); auto* const constraint = &constraints_.back(); ++pending_computations_[current_computation_]; const int current_computation = current_computation_; ScheduleSubmapScanMatcherConstructionAndQueueWorkItem( submap_id, submap->finished_probability_grid, [=]() EXCLUDES(mutex_) { - ComputeConstraint(submap_id, submap, scan_index, - -1, /* scan_trajectory_id */ + ComputeConstraint(submap_id, submap, node_id, false, /* match_full_submap */ nullptr, /* trajectory_connectivity */ point_cloud, initial_relative_pose, constraint); @@ -89,18 +89,18 @@ void ConstraintBuilder::MaybeAddConstraint( void ConstraintBuilder::MaybeAddGlobalConstraint( const mapping::SubmapId& submap_id, const mapping::Submap* const submap, - const int scan_index, const int scan_trajectory_id, + const mapping::NodeId& node_id, const int flat_scan_index, mapping::TrajectoryConnectivity* trajectory_connectivity, const sensor::PointCloud* const point_cloud) { common::MutexLocker locker(&mutex_); - CHECK_LE(scan_index, current_computation_); + CHECK_LE(flat_scan_index, current_computation_); constraints_.emplace_back(); auto* const constraint = &constraints_.back(); ++pending_computations_[current_computation_]; const int current_computation = current_computation_; ScheduleSubmapScanMatcherConstructionAndQueueWorkItem( submap_id, submap->finished_probability_grid, [=]() EXCLUDES(mutex_) { - ComputeConstraint(submap_id, submap, scan_index, scan_trajectory_id, + ComputeConstraint(submap_id, submap, node_id, true, /* match_full_submap */ trajectory_connectivity, point_cloud, transform::Rigid2d::Identity(), constraint); @@ -108,9 +108,9 @@ void ConstraintBuilder::MaybeAddGlobalConstraint( }); } -void ConstraintBuilder::NotifyEndOfScan(const int scan_index) { +void ConstraintBuilder::NotifyEndOfScan(const int flat_scan_index) { common::MutexLocker locker(&mutex_); - CHECK_EQ(current_computation_, scan_index); + CHECK_EQ(current_computation_, flat_scan_index); ++current_computation_; } @@ -166,7 +166,7 @@ ConstraintBuilder::GetSubmapScanMatcher(const mapping::SubmapId& submap_id) { void ConstraintBuilder::ComputeConstraint( const mapping::SubmapId& submap_id, const mapping::Submap* const submap, - const int scan_index, const int scan_trajectory_id, bool match_full_submap, + const mapping::NodeId& node_id, bool match_full_submap, mapping::TrajectoryConnectivity* trajectory_connectivity, const sensor::PointCloud* const point_cloud, const transform::Rigid2d& initial_relative_pose, @@ -191,9 +191,9 @@ void ConstraintBuilder::ComputeConstraint( filtered_point_cloud, options_.global_localization_min_score(), &score, &pose_estimate)) { CHECK_GT(score, options_.global_localization_min_score()); - CHECK_GE(scan_trajectory_id, 0); + CHECK_GE(node_id.trajectory_id, 0); CHECK_GE(submap_id.trajectory_id, 0); - trajectory_connectivity->Connect(scan_trajectory_id, + trajectory_connectivity->Connect(node_id.trajectory_id, submap_id.trajectory_id); } else { return; @@ -229,7 +229,7 @@ void ConstraintBuilder::ComputeConstraint( constexpr double kFakeOrientationCovariance = 1e-6; constraint->reset(new Constraint{ submap_id, - scan_index, + node_id, {transform::Embed3D(constraint_transform), common::ComputeSpdMatrixSqrtInverse( kalman_filter::Embed3D(covariance, kFakePositionCovariance, @@ -241,9 +241,9 @@ void ConstraintBuilder::ComputeConstraint( const transform::Rigid2d difference = initial_pose.inverse() * pose_estimate; std::ostringstream info; - info << "Scan index " << scan_index << " with " - << filtered_point_cloud.size() << " points on submap " << submap_id - << " differs by translation " << std::fixed << std::setprecision(2) + info << "Node " << node_id << " with " << filtered_point_cloud.size() + << " points on submap " << submap_id << " differs by translation " + << std::fixed << std::setprecision(2) << difference.translation().norm() << " rotation " << std::setprecision(3) << std::abs(difference.normalized_angle()) << " with score " << std::setprecision(1) << 100. * score diff --git a/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.h b/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.h index 1a8c64d..d04120d 100644 --- a/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.h +++ b/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.h @@ -72,33 +72,37 @@ class ConstraintBuilder { ConstraintBuilder& operator=(const ConstraintBuilder&) = delete; // Schedules exploring a new constraint between 'submap' identified by - // 'submap_id', and the 'point_cloud' for 'scan_index'. + // 'submap_id', and the 'point_cloud' for 'flat_scan_index'. // The 'initial_pose' is relative to the 'submap'. // // The pointees of 'submap' and 'point_cloud' must stay valid until all // computations are finished. void MaybeAddConstraint(const mapping::SubmapId& submap_id, - const mapping::Submap* submap, int scan_index, + const mapping::Submap* submap, + const mapping::NodeId& node_id, + const int flat_scan_index, const sensor::PointCloud* point_cloud, const transform::Rigid2d& initial_relative_pose); // Schedules exploring a new constraint between 'submap' identified by - // 'submap_id' and the 'point_cloud' for 'scan_index'. This performs + // 'submap_id' and the 'point_cloud' for 'flat_scan_index'. This performs // full-submap matching. // - // The scan at 'scan_index' should be from trajectory 'scan_trajectory_id'. - // The 'trajectory_connectivity' is updated if the full-submap match succeeds. + // The scan at 'flat_scan_index' should be from trajectory + // 'node_id.trajectory_id'. The 'trajectory_connectivity' is updated if the + // full-submap match succeeds. // // The pointees of 'submap' and 'point_cloud' must stay valid until all // computations are finished. void MaybeAddGlobalConstraint( const mapping::SubmapId& submap_id, const mapping::Submap* submap, - int scan_index, int scan_trajectory_id, + const mapping::NodeId& node_id, const int flat_scan_index, mapping::TrajectoryConnectivity* trajectory_connectivity, const sensor::PointCloud* point_cloud); - // Must be called after all computations related to 'scan_index' are added. - void NotifyEndOfScan(const int scan_index); + // Must be called after all computations related to 'flat_scan_index' are + // added. + void NotifyEndOfScan(const int flat_scan_index); // Registers the 'callback' to be called with the results, after all // computations triggered by MaybeAddConstraint() have finished. @@ -132,12 +136,12 @@ class ConstraintBuilder { // Runs in a background thread and does computations for an additional // constraint, assuming 'submap' and 'point_cloud' do not change anymore. // If 'match_full_submap' is true, and global localization succeeds, will - // connect 'scan_trajectory_id' and 'submap_id.trajectory_id' in + // connect 'node_id.trajectory_id' and 'submap_id.trajectory_id' in // 'trajectory_connectivity'. // As output, it may create a new Constraint in 'constraint'. void ComputeConstraint( const mapping::SubmapId& submap_id, const mapping::Submap* submap, - int scan_index, int scan_trajectory_id, bool match_full_submap, + const mapping::NodeId& node_id, bool match_full_submap, mapping::TrajectoryConnectivity* trajectory_connectivity, const sensor::PointCloud* point_cloud, const transform::Rigid2d& initial_relative_pose, diff --git a/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc b/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc index 6df19c4..0c409dc 100644 --- a/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc +++ b/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc @@ -98,13 +98,20 @@ void OptimizationProblem::Solve(const std::vector& constraints) { return; } + // Compute the indices of consecutive nodes for each trajectory. + std::vector> nodes_per_trajectory(submap_data_.size()); + for (size_t j = 0; j != node_data_.size(); ++j) { + nodes_per_trajectory.at(node_data_[j].trajectory_id).push_back(j); + } + ceres::Problem::Options problem_options; ceres::Problem problem(problem_options); // Set the starting point. // TODO(hrapp): Move ceres data into SubmapData. std::vector>> C_submaps(submap_data_.size()); - std::vector> C_point_clouds(node_data_.size()); + std::vector>> C_nodes( + nodes_per_trajectory.size()); for (size_t trajectory_id = 0; trajectory_id != submap_data_.size(); ++trajectory_id) { for (size_t submap_index = 0; @@ -123,15 +130,21 @@ void OptimizationProblem::Solve(const std::vector& constraints) { } } } - for (size_t j = 0; j != node_data_.size(); ++j) { - C_point_clouds[j] = FromPose(node_data_[j].point_cloud_pose); - problem.AddParameterBlock(C_point_clouds[j].data(), 3); + for (size_t trajectory_id = 0; trajectory_id != nodes_per_trajectory.size(); + ++trajectory_id) { + C_nodes[trajectory_id].resize(nodes_per_trajectory[trajectory_id].size()); + for (size_t node_index = 0; + node_index != nodes_per_trajectory[trajectory_id].size(); + ++node_index) { + C_nodes[trajectory_id][node_index] = + FromPose(node_data_[nodes_per_trajectory[trajectory_id][node_index]] + .point_cloud_pose); + problem.AddParameterBlock(C_nodes[trajectory_id][node_index].data(), 3); + } } // Add cost functions for intra- and inter-submap constraints. for (const Constraint& constraint : constraints) { - CHECK_GE(constraint.j, 0); - CHECK_LT(constraint.j, node_data_.size()); problem.AddResidualBlock( new ceres::AutoDiffCostFunction( new SpaCostFunction(constraint.pose)), @@ -142,7 +155,9 @@ void OptimizationProblem::Solve(const std::vector& constraints) { C_submaps.at(constraint.submap_id.trajectory_id) .at(constraint.submap_id.submap_index) .data(), - C_point_clouds[constraint.j].data()); + C_nodes.at(constraint.node_id.trajectory_id) + .at(constraint.node_id.node_index) + .data()); } // Add penalties for changes between consecutive scans. @@ -151,32 +166,32 @@ void OptimizationProblem::Solve(const std::vector& constraints) { options_.consecutive_scan_translation_penalty_factor(), options_.consecutive_scan_rotation_penalty_factor()); - // The poses in 'node_data_' are interleaved from multiple trajectories - // (although the points from a given trajectory are in time order). - // 'last_pose_indices[trajectory_id]' is the index of the most-recent pose on - // 'trajectory_id'. - std::map last_pose_indices; - - for (size_t j = 0; j != node_data_.size(); ++j) { - const int trajectory_id = node_data_[j].trajectory_id; - // This pose has a predecessor. - if (last_pose_indices.count(trajectory_id) != 0) { - const int last_pose_index = last_pose_indices[trajectory_id]; + for (size_t trajectory_id = 0; trajectory_id != nodes_per_trajectory.size(); + ++trajectory_id) { + if (nodes_per_trajectory[trajectory_id].empty()) { + continue; + } + for (size_t node_index = 1; + node_index != nodes_per_trajectory[trajectory_id].size(); + ++node_index) { constexpr double kUnusedPositionPenalty = 1.; constexpr double kUnusedOrientationPenalty = 1.; problem.AddResidualBlock( - new ceres::AutoDiffCostFunction( - new SpaCostFunction(Constraint::Pose{ - transform::Embed3D(node_data_[last_pose_index] - .initial_point_cloud_pose.inverse() * - node_data_[j].initial_point_cloud_pose), - kalman_filter::Embed3D(consecutive_pose_change_penalty_matrix, - kUnusedPositionPenalty, - kUnusedOrientationPenalty)})), - nullptr /* loss function */, C_point_clouds[last_pose_index].data(), - C_point_clouds[j].data()); + new ceres::AutoDiffCostFunction< + SpaCostFunction, 3, 3, 3>(new SpaCostFunction(Constraint::Pose{ + transform::Embed3D( + node_data_[nodes_per_trajectory[trajectory_id] + [node_index - 1]] + .initial_point_cloud_pose.inverse() * + node_data_[nodes_per_trajectory[trajectory_id][node_index]] + .initial_point_cloud_pose), + kalman_filter::Embed3D(consecutive_pose_change_penalty_matrix, + kUnusedPositionPenalty, + kUnusedOrientationPenalty)})), + nullptr /* loss function */, + C_nodes[trajectory_id][node_index - 1].data(), + C_nodes[trajectory_id][node_index].data()); } - last_pose_indices[trajectory_id] = j; } // Solve. @@ -198,9 +213,14 @@ void OptimizationProblem::Solve(const std::vector& constraints) { ToPose(C_submaps[trajectory_id][submap_index]); } } - - for (size_t j = 0; j != node_data_.size(); ++j) { - node_data_[j].point_cloud_pose = ToPose(C_point_clouds[j]); + for (size_t trajectory_id = 0; trajectory_id != nodes_per_trajectory.size(); + ++trajectory_id) { + for (size_t node_index = 0; + node_index != nodes_per_trajectory[trajectory_id].size(); + ++node_index) { + node_data_[nodes_per_trajectory[trajectory_id][node_index]] + .point_cloud_pose = ToPose(C_nodes[trajectory_id][node_index]); + } } } diff --git a/cartographer/mapping_3d/sparse_pose_graph.cc b/cartographer/mapping_3d/sparse_pose_graph.cc index 59bc756..1025f19 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.cc +++ b/cartographer/mapping_3d/sparse_pose_graph.cc @@ -98,8 +98,11 @@ void SparsePoseGraph::AddScan( common::MutexLocker locker(&mutex_); trajectory_ids_.emplace(trajectory, trajectory_ids_.size()); const int trajectory_id = trajectory_ids_.at(trajectory); - const int j = trajectory_nodes_.size(); - CHECK_LT(j, std::numeric_limits::max()); + const int flat_scan_index = trajectory_nodes_.size(); + CHECK_LT(flat_scan_index, std::numeric_limits::max()); + scan_index_to_node_id_.push_back( + mapping::NodeId{trajectory_id, num_nodes_in_trajectory_[trajectory_id]}); + ++num_nodes_in_trajectory_[trajectory_id]; constant_node_data_.push_back(mapping::TrajectoryNode::ConstantData{ time, sensor::RangeData{Eigen::Vector3f::Zero(), {}, {}}, @@ -130,7 +133,7 @@ void SparsePoseGraph::AddScan( } AddWorkItem([=]() REQUIRES(mutex_) { - ComputeConstraintsForScan(j, matching_submap, insertion_submaps, + ComputeConstraintsForScan(flat_scan_index, matching_submap, insertion_submaps, finished_submap, pose, covariance); }); } @@ -180,8 +183,9 @@ void SparsePoseGraph::ComputeConstraint(const int scan_index, if (scan_trajectory_id != submap_trajectory_id && global_localization_samplers_[scan_trajectory_id]->Pulse()) { constraint_builder_.MaybeAddGlobalConstraint( - submap_id, submap_states_[submap_index].submap, scan_index, - scan_trajectory_id, &trajectory_connectivity_, trajectory_nodes_); + submap_id, submap_states_[submap_index].submap, + scan_index_to_node_id_.at(scan_index), scan_index, + &trajectory_connectivity_, trajectory_nodes_); } else { const bool scan_and_submap_trajectories_connected = reverse_connected_components_.count(scan_trajectory_id) > 0 && @@ -191,8 +195,9 @@ void SparsePoseGraph::ComputeConstraint(const int scan_index, if (scan_trajectory_id == submap_trajectory_id || scan_and_submap_trajectories_connected) { constraint_builder_.MaybeAddConstraint( - submap_id, submap_states_[submap_index].submap, scan_index, - trajectory_nodes_, relative_pose); + submap_id, submap_states_[submap_index].submap, + scan_index_to_node_id_.at(scan_index), scan_index, trajectory_nodes_, + relative_pose); } } } @@ -238,7 +243,7 @@ void SparsePoseGraph::ComputeConstraintsForScan( submap->local_pose().inverse() * pose; constraints_.push_back( Constraint{submap_states_[submap_index].id, - scan_index, + scan_index_to_node_id_.at(scan_index), {constraint_transform, common::ComputeSpdMatrixSqrtInverse( covariance, options_.constraint_builder_options() diff --git a/cartographer/mapping_3d/sparse_pose_graph.h b/cartographer/mapping_3d/sparse_pose_graph.h index db0efc9..fcedc93 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.h +++ b/cartographer/mapping_3d/sparse_pose_graph.h @@ -14,16 +14,6 @@ * limitations under the License. */ -// Implements the loop closure method called Sparse Pose Adjustment (SPA) from -// Konolige, Kurt, et al. "Efficient sparse pose adjustment for 2d mapping." -// Intelligent Robots and Systems (IROS), 2010 IEEE/RSJ International Conference -// on (pp. 22--29). IEEE, 2010. -// -// It is extended for submapping in 3D: -// Each scan has been matched against one or more submaps (adding a constraint -// for each match), both poses of scans and of submaps are to be optimized. -// All constraints are between a submap i and a scan j. - #ifndef CARTOGRAPHER_MAPPING_3D_SPARSE_POSE_GRAPH_H_ #define CARTOGRAPHER_MAPPING_3D_SPARSE_POSE_GRAPH_H_ @@ -53,7 +43,15 @@ namespace cartographer { namespace mapping_3d { -// Implements the SPA loop closure method. +// Implements the loop closure method called Sparse Pose Adjustment (SPA) from +// Konolige, Kurt, et al. "Efficient sparse pose adjustment for 2d mapping." +// Intelligent Robots and Systems (IROS), 2010 IEEE/RSJ International Conference +// on (pp. 22--29). IEEE, 2010. +// +// It is extended for submapping in 3D: +// Each scan has been matched against one or more submaps (adding a constraint +// for each match), both poses of scans and of submaps are to be optimized. +// All constraints are between a submap i and a scan j. class SparsePoseGraph : public mapping::SparsePoseGraph { public: SparsePoseGraph(const mapping::proto::SparsePoseGraphOptions& options, @@ -199,6 +197,11 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { std::vector submap_states_ GUARDED_BY(mutex_); std::map num_submaps_in_trajectory_ GUARDED_BY(mutex_); + // Mapping to flat indices to aid the transition to per-trajectory data + // structures. + std::map num_nodes_in_trajectory_ GUARDED_BY(mutex_); + std::vector scan_index_to_node_id_ GUARDED_BY(mutex_); + // Connectivity structure of our trajectories by IDs. std::vector> connected_components_; // Trajectory ID to connected component ID. diff --git a/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc b/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc index 0d52c7a..6a0318f 100644 --- a/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc +++ b/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc @@ -43,14 +43,14 @@ namespace { std::vector ComputeSubmapNodes( const std::vector& trajectory_nodes, - const Submap* const submap, int scan_index, + const Submap* const submap, const int flat_scan_index, const transform::Rigid3d& initial_relative_pose) { std::vector submap_nodes; for (const int node_index : submap->trajectory_node_indices) { submap_nodes.push_back(mapping::TrajectoryNode{ trajectory_nodes[node_index].constant_data, transform::Rigid3d(initial_relative_pose * - trajectory_nodes[scan_index].pose.inverse() * + trajectory_nodes[flat_scan_index].pose.inverse() * trajectory_nodes[node_index].pose)}); } return submap_nodes; @@ -77,7 +77,7 @@ ConstraintBuilder::~ConstraintBuilder() { void ConstraintBuilder::MaybeAddConstraint( const mapping::SubmapId& submap_id, const Submap* const submap, - const int scan_index, + const mapping::NodeId& node_id, const int flat_scan_index, const std::vector& trajectory_nodes, const transform::Rigid3d& initial_relative_pose) { if (initial_relative_pose.translation().norm() > @@ -86,20 +86,19 @@ void ConstraintBuilder::MaybeAddConstraint( } if (sampler_.Pulse()) { const auto submap_nodes = ComputeSubmapNodes( - trajectory_nodes, submap, scan_index, initial_relative_pose); + trajectory_nodes, submap, flat_scan_index, initial_relative_pose); common::MutexLocker locker(&mutex_); - CHECK_LE(scan_index, current_computation_); + CHECK_LE(flat_scan_index, current_computation_); constraints_.emplace_back(); auto* const constraint = &constraints_.back(); ++pending_computations_[current_computation_]; const int current_computation = current_computation_; const auto* const point_cloud = - &trajectory_nodes[scan_index].constant_data->range_data_3d.returns; + &trajectory_nodes[flat_scan_index].constant_data->range_data_3d.returns; ScheduleSubmapScanMatcherConstructionAndQueueWorkItem( submap_id, submap_nodes, &submap->high_resolution_hybrid_grid, [=]() EXCLUDES(mutex_) { - ComputeConstraint(submap_id, submap, scan_index, - -1, /* scan_trajectory_id */ + ComputeConstraint(submap_id, submap, node_id, false, /* match_full_submap */ nullptr, /* trajectory_connectivity */ point_cloud, initial_relative_pose, constraint); @@ -110,23 +109,24 @@ void ConstraintBuilder::MaybeAddConstraint( void ConstraintBuilder::MaybeAddGlobalConstraint( const mapping::SubmapId& submap_id, const Submap* const submap, - const int scan_index, const int scan_trajectory_id, + const mapping::NodeId& node_id, const int flat_scan_index, mapping::TrajectoryConnectivity* trajectory_connectivity, const std::vector& trajectory_nodes) { - const auto submap_nodes = ComputeSubmapNodes( - trajectory_nodes, submap, scan_index, transform::Rigid3d::Identity()); + const auto submap_nodes = + ComputeSubmapNodes(trajectory_nodes, submap, flat_scan_index, + transform::Rigid3d::Identity()); common::MutexLocker locker(&mutex_); - CHECK_LE(scan_index, current_computation_); + CHECK_LE(flat_scan_index, current_computation_); constraints_.emplace_back(); auto* const constraint = &constraints_.back(); ++pending_computations_[current_computation_]; const int current_computation = current_computation_; const auto* const point_cloud = - &trajectory_nodes[scan_index].constant_data->range_data_3d.returns; + &trajectory_nodes[flat_scan_index].constant_data->range_data_3d.returns; ScheduleSubmapScanMatcherConstructionAndQueueWorkItem( submap_id, submap_nodes, &submap->high_resolution_hybrid_grid, [=]() EXCLUDES(mutex_) { - ComputeConstraint(submap_id, submap, scan_index, scan_trajectory_id, + ComputeConstraint(submap_id, submap, node_id, true, /* match_full_submap */ trajectory_connectivity, point_cloud, transform::Rigid3d::Identity(), constraint); @@ -134,9 +134,9 @@ void ConstraintBuilder::MaybeAddGlobalConstraint( }); } -void ConstraintBuilder::NotifyEndOfScan(const int scan_index) { +void ConstraintBuilder::NotifyEndOfScan(const int flat_scan_index) { common::MutexLocker locker(&mutex_); - CHECK_EQ(current_computation_, scan_index); + CHECK_EQ(current_computation_, flat_scan_index); ++current_computation_; } @@ -197,7 +197,7 @@ ConstraintBuilder::GetSubmapScanMatcher(const mapping::SubmapId& submap_id) { void ConstraintBuilder::ComputeConstraint( const mapping::SubmapId& submap_id, const Submap* const submap, - const int scan_index, const int scan_trajectory_id, bool match_full_submap, + const mapping::NodeId& node_id, bool match_full_submap, mapping::TrajectoryConnectivity* trajectory_connectivity, const sensor::CompressedPointCloud* const compressed_point_cloud, const transform::Rigid3d& initial_relative_pose, @@ -223,9 +223,9 @@ void ConstraintBuilder::ComputeConstraint( initial_pose.rotation(), filtered_point_cloud, point_cloud, options_.global_localization_min_score(), &score, &pose_estimate)) { CHECK_GT(score, options_.global_localization_min_score()); - CHECK_GE(scan_trajectory_id, 0); + CHECK_GE(node_id.trajectory_id, 0); CHECK_GE(submap_id.trajectory_id, 0); - trajectory_connectivity->Connect(scan_trajectory_id, + trajectory_connectivity->Connect(node_id.trajectory_id, submap_id.trajectory_id); } else { return; @@ -258,7 +258,7 @@ void ConstraintBuilder::ComputeConstraint( submap->local_pose().inverse() * pose_estimate; constraint->reset(new OptimizationProblem::Constraint{ submap_id, - scan_index, + node_id, {constraint_transform, 1. / std::sqrt(options_.lower_covariance_eigenvalue_bound()) * kalman_filter::PoseCovariance::Identity()}, @@ -268,9 +268,9 @@ void ConstraintBuilder::ComputeConstraint( const transform::Rigid3d difference = initial_pose.inverse() * pose_estimate; std::ostringstream info; - info << "Scan index " << scan_index << " with " - << filtered_point_cloud.size() << " points on submap " << submap_id - << " differs by translation " << std::fixed << std::setprecision(2) + info << "Node " << node_id << " with " << filtered_point_cloud.size() + << " points on submap " << submap_id << " differs by translation " + << std::fixed << std::setprecision(2) << difference.translation().norm() << " rotation " << std::setprecision(3) << transform::GetAngle(difference) << " with score " << std::setprecision(1) << 100. * score << "%."; diff --git a/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.h b/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.h index 742aba4..4633154 100644 --- a/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.h +++ b/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.h @@ -69,32 +69,34 @@ class ConstraintBuilder { // Schedules exploring a new constraint between 'submap' identified by // 'submap_id', and the 'range_data_3d.returns' in 'trajectory_nodes' for - // 'scan_index'. The 'initial_relative_pose' is relative to the 'submap'. + // 'flat_scan_index'. The 'initial_relative_pose' is relative to the 'submap'. // // The pointees of 'submap' and 'range_data_3d.returns' must stay valid until // all computations are finished. void MaybeAddConstraint( - const mapping::SubmapId& submap_id, const Submap* submap, int scan_index, + const mapping::SubmapId& submap_id, const Submap* submap, + const mapping::NodeId& node_id, int flat_scan_index, const std::vector& trajectory_nodes, const transform::Rigid3d& initial_relative_pose); // Schedules exploring a new constraint between 'submap' identified by // 'submap_id' and the 'range_data_3d.returns' in 'trajectory_nodes' for - // 'scan_index'. This performs full-submap matching. + // 'flat_scan_index'. This performs full-submap matching. // - // The scan at 'scan_index' should be from trajectory 'scan_trajectory_id'. + // The scan at 'flat_scan_index' should be from 'node_id.trajectory_id'. // The 'trajectory_connectivity' is updated if the full-submap match succeeds. // // The pointees of 'submap' and 'range_data_3d.returns' must stay valid until // all computations are finished. void MaybeAddGlobalConstraint( - const mapping::SubmapId& submap_id, const Submap* submap, int scan_index, - int scan_trajectory_id, + const mapping::SubmapId& submap_id, const Submap* submap, + const mapping::NodeId& node_id, int flat_scan_index, mapping::TrajectoryConnectivity* trajectory_connectivity, const std::vector& trajectory_nodes); - // Must be called after all computations related to 'scan_index' are added. - void NotifyEndOfScan(int scan_index); + // Must be called after all computations related to 'flat_scan_index' are + // added. + void NotifyEndOfScan(int flat_scan_index); // Registers the 'callback' to be called with the results, after all // computations triggered by MaybeAddConstraint() have finished. @@ -135,8 +137,8 @@ class ConstraintBuilder { // 'trajectory_connectivity'. // As output, it may create a new Constraint in 'constraint'. void ComputeConstraint( - const mapping::SubmapId& submap_id, const Submap* submap, int scan_index, - int scan_trajectory_id, bool match_full_submap, + const mapping::SubmapId& submap_id, const Submap* submap, + const mapping::NodeId& node_id, bool match_full_submap, mapping::TrajectoryConnectivity* trajectory_connectivity, const sensor::CompressedPointCloud* const compressed_point_cloud, const transform::Rigid3d& initial_relative_pose, diff --git a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc index 63b685d..cd970c8 100644 --- a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc +++ b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc @@ -136,7 +136,7 @@ void OptimizationProblem::Solve(const std::vector& constraints) { CHECK(!submap_data_[0].empty()); // TODO(hrapp): Move ceres data into SubmapData. std::vector> C_submaps(submap_data_.size()); - std::deque C_point_clouds; + std::vector> C_nodes(nodes_per_trajectory.size()); for (size_t trajectory_id = 0; trajectory_id != submap_data_.size(); ++trajectory_id) { for (size_t submap_index = 0; @@ -158,16 +158,21 @@ void OptimizationProblem::Solve(const std::vector& constraints) { } } } - for (size_t j = 0; j != node_data_.size(); ++j) { - C_point_clouds.emplace_back( - node_data_[j].point_cloud_pose, translation_parameterization(), - common::make_unique(), &problem); + for (size_t trajectory_id = 0; trajectory_id != nodes_per_trajectory.size(); + ++trajectory_id) { + for (size_t node_index = 0; + node_index != nodes_per_trajectory[trajectory_id].size(); + ++node_index) { + C_nodes[trajectory_id].emplace_back( + node_data_[nodes_per_trajectory[trajectory_id][node_index]] + .point_cloud_pose, + translation_parameterization(), + common::make_unique(), &problem); + } } // Add cost functions for intra- and inter-submap constraints. for (const Constraint& constraint : constraints) { - CHECK_GE(constraint.j, 0); - CHECK_LT(constraint.j, node_data_.size()); problem.AddResidualBlock( new ceres::AutoDiffCostFunction( new SpaCostFunction(constraint.pose)), @@ -181,36 +186,44 @@ void OptimizationProblem::Solve(const std::vector& constraints) { C_submaps.at(constraint.submap_id.trajectory_id) .at(constraint.submap_id.submap_index) .translation(), - C_point_clouds[constraint.j].rotation(), - C_point_clouds[constraint.j].translation()); + C_nodes.at(constraint.node_id.trajectory_id) + .at(constraint.node_id.node_index) + .rotation(), + C_nodes.at(constraint.node_id.trajectory_id) + .at(constraint.node_id.node_index) + .translation()); } // Add constraints based on IMU observations of angular velocities and // linear acceleration. for (size_t trajectory_id = 0; trajectory_id != nodes_per_trajectory.size(); ++trajectory_id) { - const std::vector& node_indices = + const std::vector& flat_indices = nodes_per_trajectory[trajectory_id]; const std::deque& imu_data = imu_data_.at(trajectory_id); - CHECK(!node_indices.empty()); + CHECK(!flat_indices.empty()); CHECK(!imu_data.empty()); // 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 + 1)->time <= node_data_[flat_indices[0]].time) { ++it; } - for (size_t j = 1; j < node_indices.size(); ++j) { + for (size_t node_index = 1; node_index < flat_indices.size(); + ++node_index) { auto it2 = it; const IntegrateImuResult 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; + IntegrateImu(imu_data, node_data_[flat_indices[node_index - 1]].time, + node_data_[flat_indices[node_index]].time, &it); + if (node_index + 1 < flat_indices.size()) { + const common::Time first_time = + node_data_[flat_indices[node_index - 1]].time; + const common::Time second_time = + node_data_[flat_indices[node_index]].time; + const common::Time third_time = + node_data_[flat_indices[node_index + 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; @@ -235,18 +248,18 @@ void OptimizationProblem::Solve(const std::vector& constraints) { 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(), + nullptr, C_nodes[trajectory_id].at(node_index).rotation(), + C_nodes[trajectory_id].at(node_index - 1).translation(), + C_nodes[trajectory_id].at(node_index).translation(), + C_nodes[trajectory_id].at(node_index + 1).translation(), &gravity_constant_); } problem.AddResidualBlock( new ceres::AutoDiffCostFunction( new RotationCostFunction(options_.rotation_weight(), result.delta_rotation)), - nullptr, C_point_clouds[node_indices[j - 1]].rotation(), - C_point_clouds[node_indices[j]].rotation()); + nullptr, C_nodes[trajectory_id].at(node_index - 1).rotation(), + C_nodes[trajectory_id].at(node_index).rotation()); } } @@ -270,9 +283,14 @@ void OptimizationProblem::Solve(const std::vector& constraints) { C_submaps[trajectory_id][submap_index].ToRigid(); } } - - for (size_t j = 0; j != node_data_.size(); ++j) { - node_data_[j].point_cloud_pose = C_point_clouds[j].ToRigid(); + for (size_t trajectory_id = 0; trajectory_id != nodes_per_trajectory.size(); + ++trajectory_id) { + for (size_t node_index = 0; + node_index != nodes_per_trajectory[trajectory_id].size(); + ++node_index) { + node_data_[nodes_per_trajectory[trajectory_id][node_index]] + .point_cloud_pose = C_nodes[trajectory_id][node_index].ToRigid(); + } } } diff --git a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem_test.cc b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem_test.cc index 593e135..1dc681b 100644 --- a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem_test.cc +++ b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem_test.cc @@ -132,13 +132,13 @@ TEST_F(OptimizationProblemTest, ReducesNoise) { std::vector constraints; for (int j = 0; j != kNumNodes; ++j) { constraints.push_back(OptimizationProblem::Constraint{ - mapping::SubmapId{0, 0}, j, + mapping::SubmapId{0, 0}, mapping::NodeId{0, j}, OptimizationProblem::Constraint::Pose{ AddNoise(test_data[j].ground_truth_pose, test_data[j].noise), Eigen::Matrix::Identity()}}); // We add an additional independent, but equally noisy observation. constraints.push_back(OptimizationProblem::Constraint{ - mapping::SubmapId{0, 1}, j, + mapping::SubmapId{0, 1}, mapping::NodeId{0, j}, OptimizationProblem::Constraint::Pose{ AddNoise(test_data[j].ground_truth_pose, RandomYawOnlyTransform(0.2, 0.3)), @@ -146,7 +146,7 @@ TEST_F(OptimizationProblemTest, ReducesNoise) { // We add very noisy data with high covariance (i.e. small Lambda) to verify // it is mostly ignored. constraints.push_back(OptimizationProblem::Constraint{ - mapping::SubmapId{0, 2}, j, + mapping::SubmapId{0, 2}, mapping::NodeId{0, j}, OptimizationProblem::Constraint::Pose{ kSubmap2Transform.inverse() * test_data[j].ground_truth_pose * RandomTransform(1e3, 3.),