diff --git a/cartographer/mapping_2d/local_trajectory_builder.cc b/cartographer/mapping_2d/local_trajectory_builder.cc index 116e37f..efee7dd 100644 --- a/cartographer/mapping_2d/local_trajectory_builder.cc +++ b/cartographer/mapping_2d/local_trajectory_builder.cc @@ -168,7 +168,7 @@ LocalTrajectoryBuilder::AddHorizontalLaserFan( pose_estimate_ * odometry_correction_; const transform::Rigid3d model_prediction = pose_estimate_; // TODO(whess): Prefer IMU over odom orientation if configured? - const transform::Rigid3d pose_prediction = odometry_prediction; + const transform::Rigid3d& pose_prediction = odometry_prediction; // Computes the rotation without yaw, as defined by GetYaw(). const transform::Rigid3d tracking_to_tracking_2d = diff --git a/cartographer/mapping_3d/global_trajectory_builder.cc b/cartographer/mapping_3d/global_trajectory_builder.cc index 5cb8ecc..74c5862 100644 --- a/cartographer/mapping_3d/global_trajectory_builder.cc +++ b/cartographer/mapping_3d/global_trajectory_builder.cc @@ -38,7 +38,8 @@ void GlobalTrajectoryBuilder::AddImuData( const Eigen::Vector3d& angular_velocity) { local_trajectory_builder_->AddImuData(time, linear_acceleration, angular_velocity); - sparse_pose_graph_->AddImuData(time, linear_acceleration, angular_velocity); + sparse_pose_graph_->AddImuData(local_trajectory_builder_->submaps(), time, + linear_acceleration, angular_velocity); } void GlobalTrajectoryBuilder::AddRangefinderData( diff --git a/cartographer/mapping_3d/sparse_pose_graph.cc b/cartographer/mapping_3d/sparse_pose_graph.cc index a5e52e6..2c8fd7a 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.cc +++ b/cartographer/mapping_3d/sparse_pose_graph.cc @@ -136,12 +136,13 @@ void SparsePoseGraph::AddWorkItem(std::function work_item) { } } -void SparsePoseGraph::AddImuData(common::Time time, +void SparsePoseGraph::AddImuData(const mapping::Submaps* trajectory, + common::Time time, const Eigen::Vector3d& linear_acceleration, const Eigen::Vector3d& angular_velocity) { common::MutexLocker locker(&mutex_); AddWorkItem([=]() REQUIRES(mutex_) { - optimization_problem_.AddImuData(time, linear_acceleration, + optimization_problem_.AddImuData(trajectory, time, linear_acceleration, angular_velocity); }); } @@ -202,7 +203,10 @@ void SparsePoseGraph::ComputeConstraintsForScan( submap_transforms_[matching_index] * matching_submap->local_pose().inverse() * pose; CHECK_EQ(scan_index, optimization_problem_.node_data().size()); - optimization_problem_.AddTrajectoryNode(time, optimized_pose); + const mapping::Submaps* const scan_trajectory = + trajectory_nodes_[scan_index].constant_data->trajectory; + optimization_problem_.AddTrajectoryNode(scan_trajectory, time, + optimized_pose); for (const Submap* submap : insertion_submaps) { const int submap_index = GetSubmapIndex(submap); CHECK(!submap_states_[submap_index].finished); @@ -319,17 +323,13 @@ void SparsePoseGraph::RunFinalOptimization() { void SparsePoseGraph::RunOptimization() { if (!submap_transforms_.empty()) { transform::Rigid3d submap_0_pose; - std::vector trajectories; { common::MutexLocker locker(&mutex_); CHECK(!submap_states_.empty()); submap_0_pose = submap_states_.front().submap->local_pose(); - for (const auto& trajectory_node : trajectory_nodes_) { - trajectories.push_back(trajectory_node.constant_data->trajectory); - } } - optimization_problem_.Solve(constraints_, submap_0_pose, trajectories, + optimization_problem_.Solve(constraints_, submap_0_pose, &submap_transforms_); common::MutexLocker locker(&mutex_); has_new_optimized_poses_ = true; diff --git a/cartographer/mapping_3d/sparse_pose_graph.h b/cartographer/mapping_3d/sparse_pose_graph.h index c30d7f3..4c58c2b 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.h +++ b/cartographer/mapping_3d/sparse_pose_graph.h @@ -79,7 +79,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { EXCLUDES(mutex_); // Adds new IMU data to be used in the optimization. - void AddImuData(common::Time time, const Eigen::Vector3d& linear_acceleration, + void AddImuData(const mapping::Submaps* trajectory, common::Time time, + const Eigen::Vector3d& linear_acceleration, const Eigen::Vector3d& angular_velocity); void RunFinalOptimization() override; diff --git a/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc b/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc index 3f4206c..24aca59 100644 --- a/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc +++ b/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc @@ -222,15 +222,24 @@ void ConstraintBuilder::ComputeConstraint( float score = 0.; transform::Rigid3d pose_estimate = transform::Rigid3d::Identity(); - CHECK(!match_full_submap) << "match_full_submap not supported for 3D."; - - if (submap_scan_matcher->fast_correlative_scan_matcher->Match( - initial_pose, filtered_point_cloud, point_cloud, options_.min_score(), - &score, &pose_estimate)) { - // We've reported a successful local match. - CHECK_GT(score, options_.min_score()); + if (match_full_submap) { + if (submap_scan_matcher->fast_correlative_scan_matcher->MatchFullSubmap( + initial_pose.rotation(), filtered_point_cloud, point_cloud, + options_.global_localization_min_score(), &score, &pose_estimate)) { + CHECK_GT(score, options_.global_localization_min_score()); + trajectory_connectivity->Connect(scan_trajectory, submap_trajectory); + } else { + return; + } } else { - return; + if (submap_scan_matcher->fast_correlative_scan_matcher->Match( + initial_pose, filtered_point_cloud, point_cloud, + options_.min_score(), &score, &pose_estimate)) { + // We've reported a successful local match. + CHECK_GT(score, options_.min_score()); + } else { + return; + } } { common::MutexLocker locker(&mutex_); diff --git a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc index cef16d0..5519fcc 100644 --- a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc +++ b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc @@ -77,15 +77,18 @@ OptimizationProblem::OptimizationProblem( OptimizationProblem::~OptimizationProblem() {} -void OptimizationProblem::AddImuData(common::Time time, +void OptimizationProblem::AddImuData(const mapping::Submaps* const trajectory, + const common::Time time, const Eigen::Vector3d& linear_acceleration, const Eigen::Vector3d& angular_velocity) { - imu_data_.push_back(ImuData{time, linear_acceleration, angular_velocity}); + imu_data_[trajectory].push_back( + ImuData{time, linear_acceleration, angular_velocity}); } void OptimizationProblem::AddTrajectoryNode( - common::Time time, const transform::Rigid3d& point_cloud_pose) { - node_data_.push_back(NodeData{time, point_cloud_pose}); + const mapping::Submaps* const trajectory, const common::Time time, + const transform::Rigid3d& point_cloud_pose) { + node_data_.push_back(NodeData{trajectory, time, point_cloud_pose}); } void OptimizationProblem::SetMaxNumIterations(const int32 max_num_iterations) { @@ -96,13 +99,17 @@ void OptimizationProblem::SetMaxNumIterations(const int32 max_num_iterations) { void OptimizationProblem::Solve( const std::vector& constraints, const transform::Rigid3d& submap_0_transform, - const std::vector& trajectories, - std::vector* submap_transforms) { + std::vector* const submap_transforms) { if (node_data_.empty()) { // Nothing to optimize. return; } - CHECK(!imu_data_.empty()); + + // Compute the indices of consecutive nodes for each trajectory. + std::map> nodes_per_trajectory; + for (size_t j = 0; j != node_data_.size(); ++j) { + nodes_per_trajectory[node_data_[j].trajectory].push_back(j); + } ceres::Problem::Options problem_options; ceres::Problem problem(problem_options); @@ -146,58 +153,68 @@ void OptimizationProblem::Solve( C_point_clouds[constraint.j].translation()); } - CHECK(!node_data_.empty()); - CHECK_GE(trajectories.size(), node_data_.size()); + // Add constraints based on IMU observations of angular velocities and + // linear acceleration. + for (const auto& trajectory_nodes_pair : nodes_per_trajectory) { + const mapping::Submaps* const trajectory = trajectory_nodes_pair.first; + const std::vector& node_indices = trajectory_nodes_pair.second; + const std::deque& imu_data = imu_data_.at(trajectory); + CHECK(!node_indices.empty()); + CHECK(!imu_data.empty()); - // Add constraints for IMU observed data: angular velocities and - // accelerations. - auto it = imu_data_.cbegin(); - while ((it + 1) != imu_data_.cend() && (it + 1)->time <= node_data_[0].time) { - ++it; - } - - for (size_t j = 1; j < node_data_.size(); ++j) { - auto it2 = it; - const IntegrateImuResult result = IntegrateImu( - imu_data_, node_data_[j - 1].time, node_data_[j].time, &it); - if (j + 1 < node_data_.size()) { - const common::Duration first_delta_time = - node_data_[j].time - node_data_[j - 1].time; - const common::Duration second_delta_time = - node_data_[j + 1].time - node_data_[j].time; - const common::Time first_center = - node_data_[j - 1].time + first_delta_time / 2; - const common::Time second_center = - node_data_[j].time + second_delta_time / 2; - const IntegrateImuResult result_to_first_center = - IntegrateImu(imu_data_, node_data_[j - 1].time, first_center, &it2); - const IntegrateImuResult result_center_to_center = - IntegrateImu(imu_data_, first_center, second_center, &it2); - // 'delta_velocity' is the change in velocity from the point in time - // halfway between the first and second poses to halfway between second - // and third pose. It is computed from IMU data and still contains a - // delta due to gravity. The orientation of this vector is in the IMU - // frame at the second pose. - const Eigen::Vector3d delta_velocity = - (result.delta_rotation.inverse() * - result_to_first_center.delta_rotation) * - result_center_to_center.delta_velocity; - problem.AddResidualBlock( - new ceres::AutoDiffCostFunction(new AccelerationCostFunction( - options_.acceleration_weight(), delta_velocity, - common::ToSeconds(first_delta_time), - common::ToSeconds(second_delta_time))), - nullptr, C_point_clouds[j].rotation(), - C_point_clouds[j - 1].translation(), C_point_clouds[j].translation(), - C_point_clouds[j + 1].translation(), &gravity_constant_); + // Skip IMU data before the first node of this trajectory. + auto it = imu_data.cbegin(); + while ((it + 1) != imu_data.cend() && + (it + 1)->time <= node_data_[node_indices[0]].time) { + ++it; + } + + for (size_t j = 1; j < node_indices.size(); ++j) { + auto it2 = it; + const IntegrateImuResult result = + IntegrateImu(imu_data, node_data_[node_indices[j - 1]].time, + node_data_[node_indices[j]].time, &it); + if (j + 1 < node_indices.size()) { + const common::Time first_time = node_data_[node_indices[j - 1]].time; + const common::Time second_time = node_data_[node_indices[j]].time; + const common::Time third_time = node_data_[node_indices[j + 1]].time; + const common::Duration first_duration = second_time - first_time; + const common::Duration second_duration = third_time - second_time; + const common::Time first_center = first_time + first_duration / 2; + const common::Time second_center = second_time + second_duration / 2; + const IntegrateImuResult result_to_first_center = + IntegrateImu(imu_data, first_time, first_center, &it2); + const IntegrateImuResult result_center_to_center = + IntegrateImu(imu_data, first_center, second_center, &it2); + // 'delta_velocity' is the change in velocity from the point in time + // halfway between the first and second poses to halfway between second + // and third pose. It is computed from IMU data and still contains a + // delta due to gravity. The orientation of this vector is in the IMU + // frame at the second pose. + const Eigen::Vector3d delta_velocity = + (result.delta_rotation.inverse() * + result_to_first_center.delta_rotation) * + result_center_to_center.delta_velocity; + problem.AddResidualBlock( + new ceres::AutoDiffCostFunction( + new AccelerationCostFunction( + options_.acceleration_weight(), delta_velocity, + common::ToSeconds(first_duration), + common::ToSeconds(second_duration))), + nullptr, C_point_clouds[node_indices[j]].rotation(), + C_point_clouds[node_indices[j - 1]].translation(), + C_point_clouds[node_indices[j]].translation(), + C_point_clouds[node_indices[j + 1]].translation(), + &gravity_constant_); + } + problem.AddResidualBlock( + new ceres::AutoDiffCostFunction( + new RotationCostFunction(options_.rotation_weight(), + result.delta_rotation)), + nullptr, C_point_clouds[node_indices[j - 1]].rotation(), + C_point_clouds[node_indices[j]].rotation()); } - problem.AddResidualBlock( - new ceres::AutoDiffCostFunction( - new RotationCostFunction(options_.rotation_weight(), - result.delta_rotation)), - nullptr, C_point_clouds[j - 1].rotation(), - C_point_clouds[j].rotation()); } // Solve. diff --git a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h index b97d67d..4625b49 100644 --- a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h +++ b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h @@ -36,6 +36,8 @@ namespace mapping_3d { namespace sparse_pose_graph { struct NodeData { + // TODO(whess): Keep nodes per trajectory instead. + const mapping::Submaps* trajectory; common::Time time; transform::Rigid3d point_cloud_pose; }; @@ -53,26 +55,24 @@ class OptimizationProblem { OptimizationProblem(const OptimizationProblem&) = delete; OptimizationProblem& operator=(const OptimizationProblem&) = delete; - void AddImuData(common::Time time, const Eigen::Vector3d& linear_acceleration, + void AddImuData(const mapping::Submaps* trajectory, common::Time time, + const Eigen::Vector3d& linear_acceleration, const Eigen::Vector3d& angular_velocity); - void AddTrajectoryNode(common::Time time, + void AddTrajectoryNode(const mapping::Submaps* trajectory, common::Time time, const transform::Rigid3d& point_cloud_pose); void SetMaxNumIterations(int32 max_num_iterations); - // Computes the optimized poses. The point cloud at 'point_cloud_poses[i]' - // belongs to 'trajectories[i]'. Within a given trajectory, scans are expected - // to be contiguous. + // Computes the optimized poses. void Solve(const std::vector& constraints, const transform::Rigid3d& submap_0_transform, - const std::vector& trajectories, std::vector* submap_transforms); const std::vector& node_data() const; private: mapping::sparse_pose_graph::proto::OptimizationProblemOptions options_; - std::deque imu_data_; + std::map> imu_data_; std::vector node_data_; double gravity_constant_ = 9.8; }; 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 fd476b6..05afc9c 100644 --- a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem_test.cc +++ b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem_test.cc @@ -118,15 +118,14 @@ TEST_F(OptimizationProblemTest, ReducesNoise) { NoisyNode{RandomTransform(10., 3.), RandomYawOnlyTransform(0.2, 0.3)}); } - std::vector trajectories; common::Time now = common::FromUniversal(0); for (const NoisyNode& node : test_data) { - trajectories.push_back(kTrajectory); const transform::Rigid3d pose = AddNoise(node.ground_truth_pose, node.noise); - optimization_problem_.AddImuData(now, Eigen::Vector3d::UnitZ() * 9.81, + optimization_problem_.AddImuData(kTrajectory, now, + Eigen::Vector3d::UnitZ() * 9.81, Eigen::Vector3d::Zero()); - optimization_problem_.AddTrajectoryNode(now, pose); + optimization_problem_.AddTrajectoryNode(kTrajectory, now, pose); now += common::FromSeconds(0.01); } @@ -166,7 +165,7 @@ TEST_F(OptimizationProblemTest, ReducesNoise) { node_data[j].point_cloud_pose); } - optimization_problem_.Solve(constraints, kSubmap0Transform, trajectories, + optimization_problem_.Solve(constraints, kSubmap0Transform, &submap_transforms); double translation_error_after = 0.;