From d91afa44967f53c57908d1b5e2c29b20709d6e4c Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Mon, 16 Oct 2017 13:37:28 +0200 Subject: [PATCH] Introduces mapping::MapById for nodes in 3D. (#587) --- cartographer/mapping_3d/sparse_pose_graph.cc | 57 ++--- cartographer/mapping_3d/sparse_pose_graph.h | 2 +- .../sparse_pose_graph/optimization_problem.cc | 200 ++++++++---------- .../sparse_pose_graph/optimization_problem.h | 5 +- .../optimization_problem_test.cc | 28 +-- 5 files changed, 123 insertions(+), 169 deletions(-) diff --git a/cartographer/mapping_3d/sparse_pose_graph.cc b/cartographer/mapping_3d/sparse_pose_graph.cc index 29f628d..f4849e6 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.cc +++ b/cartographer/mapping_3d/sparse_pose_graph.cc @@ -99,7 +99,7 @@ void SparsePoseGraph::AddScan( common::MutexLocker locker(&mutex_); AddTrajectoryIfNeeded(trajectory_id); - trajectory_nodes_.Append( + const mapping::NodeId node_id = trajectory_nodes_.Append( trajectory_id, mapping::TrajectoryNode{constant_data, optimized_pose}); ++num_trajectory_nodes_; @@ -121,7 +121,7 @@ void SparsePoseGraph::AddScan( // execute the lambda. const bool newly_finished_submap = insertion_submaps.front()->finished(); AddWorkItem([=]() REQUIRES(mutex_) { - ComputeConstraintsForScan(trajectory_id, insertion_submaps, + ComputeConstraintsForScan(node_id, insertion_submaps, newly_finished_submap); }); } @@ -178,10 +178,7 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id, optimization_problem_.submap_data().at(submap_id).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) - .pose; + inverse_submap_pose * optimization_problem_.node_data().at(node_id).pose; std::vector submap_nodes; for (const mapping::NodeId& submap_node_id : @@ -231,40 +228,23 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id, void SparsePoseGraph::ComputeConstraintsForOldScans( const mapping::SubmapId& submap_id) { const auto& submap_data = submap_data_.at(submap_id); - const auto& node_data = optimization_problem_.node_data(); - for (size_t trajectory_id = 0; trajectory_id != node_data.size(); - ++trajectory_id) { - for (const auto& index_node_data : node_data[trajectory_id]) { - const mapping::NodeId node_id{static_cast(trajectory_id), - index_node_data.first}; - CHECK(!trajectory_nodes_.at(node_id).trimmed()); - if (submap_data.node_ids.count(node_id) == 0) { - ComputeConstraint(node_id, submap_id); - } + for (const auto& node_id_data : optimization_problem_.node_data()) { + const mapping::NodeId& node_id = node_id_data.id; + CHECK(!trajectory_nodes_.at(node_id).trimmed()); + if (submap_data.node_ids.count(node_id) == 0) { + ComputeConstraint(node_id, submap_id); } } } void SparsePoseGraph::ComputeConstraintsForScan( - const int trajectory_id, + const mapping::NodeId& node_id, std::vector> insertion_submaps, const bool newly_finished_submap) { const std::vector submap_ids = - GrowSubmapTransformsAsNeeded(trajectory_id, insertion_submaps); + GrowSubmapTransformsAsNeeded(node_id.trajectory_id, insertion_submaps); CHECK_EQ(submap_ids.size(), insertion_submaps.size()); const mapping::SubmapId matching_id = submap_ids.front(); - const mapping::NodeId node_id{ - matching_id.trajectory_id, - static_cast(matching_id.trajectory_id) < - optimization_problem_.node_data().size() && - !optimization_problem_.node_data()[matching_id.trajectory_id] - .empty() - ? static_cast(optimization_problem_.node_data() - .at(matching_id.trajectory_id) - .rbegin() - ->first + - 1) - : 0}; const auto& constant_data = trajectory_nodes_.at(node_id).constant_data; const transform::Rigid3d& pose = constant_data->initial_pose; const transform::Rigid3d optimized_pose = @@ -528,12 +508,14 @@ void SparsePoseGraph::RunOptimization() { const auto& submap_data = optimization_problem_.submap_data(); const auto& node_data = optimization_problem_.node_data(); - for (int trajectory_id = 0; - trajectory_id != static_cast(node_data.size()); ++trajectory_id) { + for (auto node_it = node_data.begin(); node_it != node_data.end();) { + const int trajectory_id = node_it->id.trajectory_id; + const auto trajectory_end = node_data.EndOfTrajectory(trajectory_id); const int num_nodes = trajectory_nodes_.num_indices(trajectory_id); - for (const auto& node_data_index : node_data.at(trajectory_id)) { - const mapping::NodeId node_id{trajectory_id, node_data_index.first}; - trajectory_nodes_.at(node_id).pose = node_data_index.second.pose; + for (; node_it != trajectory_end; ++node_it) { + const mapping::NodeId node_id = node_it->id; + auto& node = trajectory_nodes_.at(node_id); + node.pose = node_it->data.pose; } // Extrapolate all point cloud poses that were added later. const auto local_to_new_global = @@ -542,10 +524,7 @@ void SparsePoseGraph::RunOptimization() { optimized_submap_transforms_, trajectory_id); const transform::Rigid3d old_global_to_new_global = local_to_new_global * local_to_old_global.inverse(); - int last_optimized_node_index = - node_data.at(trajectory_id).empty() - ? 0 - : node_data.at(trajectory_id).rbegin()->first; + const int last_optimized_node_index = std::prev(node_it)->id.node_index; for (int node_index = last_optimized_node_index + 1; node_index < num_nodes; ++node_index) { const mapping::NodeId node_id{trajectory_id, node_index}; diff --git a/cartographer/mapping_3d/sparse_pose_graph.h b/cartographer/mapping_3d/sparse_pose_graph.h index 6c529da..a495919 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.h +++ b/cartographer/mapping_3d/sparse_pose_graph.h @@ -134,7 +134,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { // Adds constraints for a scan, and starts scan matching in the background. void ComputeConstraintsForScan( - int trajectory_id, + const mapping::NodeId& node_id, std::vector> insertion_submaps, bool newly_finished_submap) REQUIRES(mutex_); diff --git a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc index 993f620..5b9e7a5 100644 --- a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc +++ b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc @@ -84,23 +84,20 @@ void OptimizationProblem::AddTrajectoryNode( const int trajectory_id, const common::Time time, const transform::Rigid3d& initial_pose, const transform::Rigid3d& pose) { CHECK_GE(trajectory_id, 0); - node_data_.resize( - std::max(node_data_.size(), static_cast(trajectory_id) + 1)); - trajectory_data_.resize(std::max(trajectory_data_.size(), node_data_.size())); - auto& trajectory_data = trajectory_data_[trajectory_id]; - node_data_[trajectory_id].emplace(trajectory_data.next_node_index, - NodeData{time, initial_pose, pose}); - ++trajectory_data.next_node_index; + trajectory_data_.resize(std::max(trajectory_data_.size(), + static_cast(trajectory_id) + 1)); + node_data_.Append(trajectory_id, NodeData{time, initial_pose, pose}); } void OptimizationProblem::TrimTrajectoryNode(const mapping::NodeId& node_id) { - auto& node_data = node_data_.at(node_id.trajectory_id); - CHECK(node_data.erase(node_id.node_index)); + node_data_.Trim(node_id); - if (!node_data.empty() && - node_id.trajectory_id < static_cast(imu_data_.size())) { - const common::Time node_time = node_data.begin()->second.time; - auto& imu_data = imu_data_.at(node_id.trajectory_id); + const int trajectory_id = node_id.trajectory_id; + if (node_data_.SizeOfTrajectoryOrZero(trajectory_id) == 0 && + trajectory_id < static_cast(imu_data_.size())) { + const common::Time node_time = + node_data_.BeginOfTrajectory(trajectory_id)->data.time; + auto& imu_data = imu_data_.at(trajectory_id); while (imu_data.size() > 1 && imu_data[1].time <= node_time) { imu_data.pop_front(); } @@ -146,7 +143,7 @@ void OptimizationProblem::Solve(const std::vector& constraints, CHECK(!submap_data_.empty()); CHECK(submap_data_.Contains(mapping::SubmapId{0, 0})); mapping::MapById C_submaps; - std::vector> C_nodes(node_data_.size()); + mapping::MapById C_nodes; bool first_submap = true; for (const auto& submap_id_data : submap_data_) { const bool frozen = @@ -177,23 +174,18 @@ void OptimizationProblem::Solve(const std::vector& constraints, C_submaps.at(submap_id_data.id).translation()); } } - for (size_t trajectory_id = 0; trajectory_id != node_data_.size(); - ++trajectory_id) { - const bool frozen = frozen_trajectories.count(trajectory_id) != 0; - for (const auto& index_node_data : node_data_[trajectory_id]) { - const int node_index = index_node_data.first; - C_nodes[trajectory_id].emplace( - std::piecewise_construct, std::forward_as_tuple(node_index), - std::forward_as_tuple( - index_node_data.second.pose, translation_parameterization(), - common::make_unique(), - &problem)); - if (frozen) { - problem.SetParameterBlockConstant( - C_nodes[trajectory_id].at(node_index).rotation()); - problem.SetParameterBlockConstant( - C_nodes[trajectory_id].at(node_index).translation()); - } + for (const auto& node_id_data : node_data_) { + const bool frozen = + frozen_trajectories.count(node_id_data.id.trajectory_id) != 0; + C_nodes.Insert( + node_id_data.id, + CeresPose(node_id_data.data.pose, translation_parameterization(), + common::make_unique(), + &problem)); + if (frozen) { + problem.SetParameterBlockConstant(C_nodes.at(node_id_data.id).rotation()); + problem.SetParameterBlockConstant( + C_nodes.at(node_id_data.id).translation()); } } // Add cost functions for intra- and inter-submap constraints. @@ -207,44 +199,33 @@ void OptimizationProblem::Solve(const std::vector& constraints, : nullptr, C_submaps.at(constraint.submap_id).rotation(), C_submaps.at(constraint.submap_id).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()); + C_nodes.at(constraint.node_id).rotation(), + C_nodes.at(constraint.node_id).translation()); } // Add constraints based on IMU observations of angular velocities and // linear acceleration. if (fix_z_ == FixZ::kNo) { - trajectory_data_.resize(imu_data_.size()); - CHECK_GE(trajectory_data_.size(), node_data_.size()); - for (size_t trajectory_id = 0; trajectory_id != node_data_.size(); - ++trajectory_id) { - if (node_data_[trajectory_id].empty()) { - // We skip empty trajectories which might not have any IMU data. - continue; - } + for (auto node_it = node_data_.begin(); node_it != node_data_.end();) { + const int trajectory_id = node_it->id.trajectory_id; + const auto trajectory_end = node_data_.EndOfTrajectory(trajectory_id); TrajectoryData& trajectory_data = trajectory_data_.at(trajectory_id); + problem.AddParameterBlock(trajectory_data.imu_calibration.data(), 4, new ceres::QuaternionParameterization()); const std::deque& imu_data = imu_data_.at(trajectory_id); CHECK(!imu_data.empty()); auto imu_it = imu_data.cbegin(); - for (auto node_it = node_data_[trajectory_id].begin();;) { - const int first_node_index = node_it->first; - const NodeData& first_node_data = node_it->second; - ++node_it; - if (node_it == node_data_[trajectory_id].end()) { - break; - } + auto prev_node_it = node_it; + for (++node_it; node_it != trajectory_end; ++node_it) { + const mapping::NodeId first_node_id = prev_node_it->id; + const NodeData& first_node_data = prev_node_it->data; + prev_node_it = node_it; + const mapping::NodeId second_node_id = node_it->id; + const NodeData& second_node_data = node_it->data; - const int second_node_index = node_it->first; - const NodeData& second_node_data = node_it->second; - - if (second_node_index != first_node_index + 1) { + if (second_node_id.node_index != first_node_id.node_index + 1) { continue; } @@ -258,10 +239,10 @@ void OptimizationProblem::Solve(const std::vector& constraints, const IntegrateImuResult result = IntegrateImu( imu_data, first_node_data.time, second_node_data.time, &imu_it); const auto next_node_it = std::next(node_it); - if (next_node_it != node_data_[trajectory_id].end() && - next_node_it->first == second_node_index + 1) { - const int third_node_index = next_node_it->first; - const NodeData& third_node_data = next_node_it->second; + if (next_node_it != trajectory_end && + next_node_it->id.node_index == second_node_id.node_index + 1) { + const mapping::NodeId third_node_id = next_node_it->id; + const NodeData& third_node_data = next_node_it->data; const common::Time first_time = first_node_data.time; const common::Time second_time = second_node_data.time; const common::Time third_time = third_node_data.time; @@ -289,10 +270,10 @@ void OptimizationProblem::Solve(const std::vector& constraints, options_.acceleration_weight(), delta_velocity, common::ToSeconds(first_duration), common::ToSeconds(second_duration))), - nullptr, C_nodes[trajectory_id].at(second_node_index).rotation(), - C_nodes[trajectory_id].at(first_node_index).translation(), - C_nodes[trajectory_id].at(second_node_index).translation(), - C_nodes[trajectory_id].at(third_node_index).translation(), + nullptr, C_nodes.at(second_node_id).rotation(), + C_nodes.at(first_node_id).translation(), + C_nodes.at(second_node_id).translation(), + C_nodes.at(third_node_id).translation(), &trajectory_data.gravity_constant, trajectory_data.imu_calibration.data()); } @@ -300,8 +281,8 @@ void OptimizationProblem::Solve(const std::vector& constraints, new ceres::AutoDiffCostFunction( new RotationCostFunction(options_.rotation_weight(), result.delta_rotation)), - nullptr, C_nodes[trajectory_id].at(first_node_index).rotation(), - C_nodes[trajectory_id].at(second_node_index).rotation(), + nullptr, C_nodes.at(first_node_id).rotation(), + C_nodes.at(second_node_id).rotation(), trajectory_data.imu_calibration.data()); } } @@ -310,66 +291,62 @@ void OptimizationProblem::Solve(const std::vector& constraints, if (fix_z_ == FixZ::kYes) { // Add penalties for violating odometry or changes between consecutive scans // if odometry is not available. - for (size_t trajectory_id = 0; trajectory_id != node_data_.size(); - ++trajectory_id) { - if (node_data_[trajectory_id].empty()) { - continue; - } - for (auto node_it = node_data_[trajectory_id].begin();;) { - const int node_index = node_it->first; - const NodeData& node_data = node_it->second; - ++node_it; - if (node_it == node_data_[trajectory_id].end()) { - break; - } + for (auto node_it = node_data_.begin(); node_it != node_data_.end();) { + const int trajectory_id = node_it->id.trajectory_id; + const auto trajectory_end = node_data_.EndOfTrajectory(trajectory_id); - const int next_node_index = node_it->first; - const NodeData& next_node_data = node_it->second; + auto prev_node_it = node_it; + for (++node_it; node_it != trajectory_end; ++node_it) { + const mapping::NodeId first_node_id = prev_node_it->id; + const NodeData& first_node_data = prev_node_it->data; + prev_node_it = node_it; + const mapping::NodeId second_node_id = node_it->id; + const NodeData& second_node_data = node_it->data; - if (next_node_index != node_index + 1) { + if (second_node_id.node_index != first_node_id.node_index + 1) { continue; } const bool odometry_available = - trajectory_id < odometry_data_.size() && - odometry_data_[trajectory_id].Has(next_node_data.time) && - odometry_data_[trajectory_id].Has(node_data.time); + trajectory_id < static_cast(odometry_data_.size()) && + odometry_data_[trajectory_id].Has(second_node_data.time) && + odometry_data_[trajectory_id].Has(first_node_data.time); const transform::Rigid3d relative_pose = - odometry_available - ? odometry_data_[trajectory_id] - .Lookup(node_data.time) - .inverse() * - odometry_data_[trajectory_id].Lookup(next_node_data.time) - : node_data.initial_pose.inverse() * - next_node_data.initial_pose; + odometry_available ? odometry_data_[trajectory_id] + .Lookup(first_node_data.time) + .inverse() * + odometry_data_[trajectory_id].Lookup( + second_node_data.time) + : first_node_data.initial_pose.inverse() * + second_node_data.initial_pose; problem.AddResidualBlock( new ceres::AutoDiffCostFunction( new SpaCostFunction(Constraint::Pose{ relative_pose, options_.consecutive_scan_translation_penalty_factor(), options_.consecutive_scan_rotation_penalty_factor()})), - nullptr /* loss function */, - C_nodes[trajectory_id].at(node_index).rotation(), - C_nodes[trajectory_id].at(node_index).translation(), - C_nodes[trajectory_id].at(next_node_index).rotation(), - C_nodes[trajectory_id].at(next_node_index).translation()); + nullptr /* loss function */, C_nodes.at(first_node_id).rotation(), + C_nodes.at(first_node_id).translation(), + C_nodes.at(second_node_id).rotation(), + C_nodes.at(second_node_id).translation()); } } } // Add fixed frame pose constraints. std::deque C_fixed_frames; - for (size_t trajectory_id = 0; trajectory_id != node_data_.size(); - ++trajectory_id) { - if (trajectory_id >= fixed_frame_pose_data_.size()) { + for (auto node_it = node_data_.begin(); node_it != node_data_.end();) { + const int trajectory_id = node_it->id.trajectory_id; + if (trajectory_id >= static_cast(fixed_frame_pose_data_.size())) { break; } - bool fixed_frame_pose_initialized = false; - for (auto& index_node_data : node_data_[trajectory_id]) { - const int node_index = index_node_data.first; - const NodeData& node_data = index_node_data.second; + const auto trajectory_end = node_data_.EndOfTrajectory(trajectory_id); + for (; node_it != trajectory_end; ++node_it) { + const mapping::NodeId node_id = node_it->id; + const NodeData& node_data = node_it->data; + if (!fixed_frame_pose_data_.at(trajectory_id).Has(node_data.time)) { continue; } @@ -399,9 +376,8 @@ void OptimizationProblem::Solve(const std::vector& constraints, new ceres::AutoDiffCostFunction( new SpaCostFunction(constraint_pose)), nullptr, C_fixed_frames.back().rotation(), - C_fixed_frames.back().translation(), - C_nodes.at(trajectory_id).at(node_index).rotation(), - C_nodes.at(trajectory_id).at(node_index).translation()); + C_fixed_frames.back().translation(), C_nodes.at(node_id).rotation(), + C_nodes.at(node_id).translation()); } } @@ -433,17 +409,13 @@ void OptimizationProblem::Solve(const std::vector& constraints, for (const auto& C_submap_id_data : C_submaps) { submap_data_.at(C_submap_id_data.id).pose = C_submap_id_data.data.ToRigid(); } - for (size_t trajectory_id = 0; trajectory_id != node_data_.size(); - ++trajectory_id) { - for (auto& index_node_data : node_data_[trajectory_id]) { - index_node_data.second.pose = - C_nodes[trajectory_id].at(index_node_data.first).ToRigid(); - } + for (const auto& C_node_id_data : C_nodes) { + node_data_.at(C_node_id_data.id).pose = C_node_id_data.data.ToRigid(); } } -const std::vector>& OptimizationProblem::node_data() - const { +const mapping::MapById& +OptimizationProblem::node_data() const { return node_data_; } diff --git a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h index bddf617..15ad265 100644 --- a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h +++ b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h @@ -84,20 +84,19 @@ class OptimizationProblem { void Solve(const std::vector& constraints, const std::set& frozen_trajectories); - const std::vector>& node_data() const; + const mapping::MapById& node_data() const; const mapping::MapById& submap_data() const; private: struct TrajectoryData { double gravity_constant = 9.8; std::array imu_calibration{{1., 0., 0., 0.}}; - int next_node_index = 0; }; mapping::sparse_pose_graph::proto::OptimizationProblemOptions options_; FixZ fix_z_; std::vector> imu_data_; - std::vector> node_data_; + mapping::MapById node_data_; std::vector odometry_data_; mapping::MapById submap_data_; std::vector trajectory_data_; 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 603ccca..9685c02 100644 --- a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem_test.cc +++ b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem_test.cc @@ -134,20 +134,20 @@ TEST_F(OptimizationProblemTest, ReducesNoise) { std::vector constraints; for (int j = 0; j != kNumNodes; ++j) { constraints.push_back(OptimizationProblem::Constraint{ - mapping::SubmapId{0, 0}, mapping::NodeId{0, j}, + mapping::SubmapId{kTrajectoryId, 0}, mapping::NodeId{kTrajectoryId, j}, OptimizationProblem::Constraint::Pose{ AddNoise(test_data[j].ground_truth_pose, test_data[j].noise), 1., 1.}}); // We add an additional independent, but equally noisy observation. constraints.push_back(OptimizationProblem::Constraint{ - mapping::SubmapId{0, 1}, mapping::NodeId{0, j}, + mapping::SubmapId{kTrajectoryId, 1}, mapping::NodeId{kTrajectoryId, j}, OptimizationProblem::Constraint::Pose{ AddNoise(test_data[j].ground_truth_pose, RandomYawOnlyTransform(0.2, 0.3)), 1., 1.}}); // We add very noisy data with a low weight to verify it is mostly ignored. constraints.push_back(OptimizationProblem::Constraint{ - mapping::SubmapId{0, 2}, mapping::NodeId{0, j}, + mapping::SubmapId{kTrajectoryId, 2}, mapping::NodeId{kTrajectoryId, j}, OptimizationProblem::Constraint::Pose{ kSubmap2Transform.inverse() * test_data[j].ground_truth_pose * RandomTransform(1e3, 3.), @@ -156,13 +156,15 @@ TEST_F(OptimizationProblemTest, ReducesNoise) { double translation_error_before = 0.; double rotation_error_before = 0.; - const auto& node_data = optimization_problem_.node_data().at(0); + const auto& node_data = optimization_problem_.node_data(); for (int j = 0; j != kNumNodes; ++j) { - translation_error_before += (test_data[j].ground_truth_pose.translation() - - node_data.at(j).pose.translation()) - .norm(); + translation_error_before += + (test_data[j].ground_truth_pose.translation() - + node_data.at(mapping::NodeId{kTrajectoryId, j}).pose.translation()) + .norm(); rotation_error_before += transform::GetAngle( - test_data[j].ground_truth_pose.inverse() * node_data.at(j).pose); + test_data[j].ground_truth_pose.inverse() * + node_data.at(mapping::NodeId{kTrajectoryId, j}).pose); } optimization_problem_.AddSubmap(kTrajectoryId, kSubmap0Transform); @@ -174,11 +176,13 @@ TEST_F(OptimizationProblemTest, ReducesNoise) { double translation_error_after = 0.; double rotation_error_after = 0.; for (int j = 0; j != kNumNodes; ++j) { - translation_error_after += (test_data[j].ground_truth_pose.translation() - - node_data.at(j).pose.translation()) - .norm(); + translation_error_after += + (test_data[j].ground_truth_pose.translation() - + node_data.at(mapping::NodeId{kTrajectoryId, j}).pose.translation()) + .norm(); rotation_error_after += transform::GetAngle( - test_data[j].ground_truth_pose.inverse() * node_data.at(j).pose); + test_data[j].ground_truth_pose.inverse() * + node_data.at(mapping::NodeId{kTrajectoryId, j}).pose); } EXPECT_GT(0.8 * translation_error_before, translation_error_after);