diff --git a/cartographer/mapping/pose_graph_interface.h b/cartographer/mapping/pose_graph_interface.h index 4e56d13..9bb8649 100644 --- a/cartographer/mapping/pose_graph_interface.h +++ b/cartographer/mapping/pose_graph_interface.h @@ -19,6 +19,7 @@ #include +#include "cartographer/common/optional.h" #include "cartographer/mapping/id.h" #include "cartographer/mapping/submaps.h" @@ -58,7 +59,7 @@ class PoseGraphInterface { double rotation_weight; }; std::vector landmark_observations; - transform::Rigid3d global_landmark_pose; + common::optional global_landmark_pose; }; struct SubmapPose { diff --git a/cartographer/mapping_2d/pose_graph.cc b/cartographer/mapping_2d/pose_graph.cc index 44912bc..4db95d0 100644 --- a/cartographer/mapping_2d/pose_graph.cc +++ b/cartographer/mapping_2d/pose_graph.cc @@ -550,7 +550,8 @@ void PoseGraph::RunOptimization() { // frozen_trajectories_ when executing the Solve. Solve is time consuming, // so not taking the mutex before Solve to avoid blocking foreground // processing. - optimization_problem_.Solve(constraints_, frozen_trajectories_); + optimization_problem_.Solve(constraints_, frozen_trajectories_, + landmark_nodes_); common::MutexLocker locker(&mutex_); const auto& submap_data = optimization_problem_.submap_data(); diff --git a/cartographer/mapping_2d/pose_graph/optimization_problem.cc b/cartographer/mapping_2d/pose_graph/optimization_problem.cc index 5833ae2..47bd561 100644 --- a/cartographer/mapping_2d/pose_graph/optimization_problem.cc +++ b/cartographer/mapping_2d/pose_graph/optimization_problem.cc @@ -27,6 +27,8 @@ #include "cartographer/common/ceres_solver_options.h" #include "cartographer/common/histogram.h" #include "cartographer/common/math.h" +#include "cartographer/mapping/pose_graph/ceres_pose.h" +#include "cartographer/mapping_2d/pose_graph/landmark_cost_function.h" #include "cartographer/mapping_2d/pose_graph/spa_cost_function.h" #include "cartographer/sensor/odometry_data.h" #include "cartographer/transform/transform.h" @@ -39,6 +41,9 @@ namespace pose_graph { namespace { +using ::cartographer::mapping::pose_graph::CeresPose; +using LandmarkNode = ::cartographer::mapping::PoseGraphInterface::LandmarkNode; + // Converts a pose into the 3 optimization variable format used for Ceres: // translation in x and y, followed by the rotation angle representing the // orientation. @@ -52,6 +57,63 @@ transform::Rigid2d ToPose(const std::array& values) { return transform::Rigid2d({values[0], values[1]}, values[2]); } +// Selects a trajectory node closest in time to the landmark observation and +// applies a relative transform from it. +transform::Rigid3d GetInitialLandmarkPose( + const LandmarkNode::LandmarkObservation& observation, + const NodeData& prev_node, const NodeData& next_node) { + const NodeData& closest_node = + observation.time - prev_node.time < next_node.time - observation.time + ? prev_node + : next_node; + return transform::Embed3D(closest_node.pose) * + transform::Rigid3d::Rotation(closest_node.gravity_alignment) * + observation.landmark_to_tracking_transform; +} + +void AddLandmarkCostFunctions( + const std::map& landmark_nodes, + const mapping::MapById& node_data, + mapping::MapById>* C_nodes, + std::map* C_landmarks, ceres::Problem* problem) { + for (const auto& landmark_node : landmark_nodes) { + for (const auto& observation : landmark_node.second.landmark_observations) { + const std::string& landmark_id = landmark_node.first; + // Find the trajectory nodes before and after the landmark observation. + auto next = + node_data.lower_bound(observation.trajectory_id, observation.time); + // The landmark observation was made before the trajectory was created. + if (next == node_data.BeginOfTrajectory(observation.trajectory_id)) { + continue; + } + // The landmark observation was made, but the next trajectory node has + // not been added yet. + if (next == node_data.EndOfTrajectory(observation.trajectory_id)) { + continue; + } + auto prev = std::prev(next); + // Add parameter blocks for the landmark ID if they were not added before. + if (!C_landmarks->count(landmark_id)) { + const transform::Rigid3d starting_point = + landmark_node.second.global_landmark_pose.has_value() + ? landmark_node.second.global_landmark_pose.value() + : GetInitialLandmarkPose(observation, prev->data, next->data); + C_landmarks->emplace( + landmark_id, + CeresPose(starting_point, nullptr /* translation_parametrization */, + common::make_unique(), + problem)); + } + problem->AddResidualBlock( + LandmarkCostFunction::CreateAutoDiffCostFunction( + observation, prev->data, next->data), + nullptr /* loss function */, C_nodes->at(prev->id).data(), + C_nodes->at(next->id).data(), C_landmarks->at(landmark_id).rotation(), + C_landmarks->at(landmark_id).translation()); + } + } +} + } // namespace OptimizationProblem::OptimizationProblem( @@ -112,8 +174,10 @@ void OptimizationProblem::SetMaxNumIterations(const int32 max_num_iterations) { max_num_iterations); } -void OptimizationProblem::Solve(const std::vector& constraints, - const std::set& frozen_trajectories) { +void OptimizationProblem::Solve( + const std::vector& constraints, + const std::set& frozen_trajectories, + const std::map& landmark_nodes) { if (node_data_.empty()) { // Nothing to optimize. return; @@ -126,6 +190,7 @@ void OptimizationProblem::Solve(const std::vector& constraints, // TODO(hrapp): Move ceres data into SubmapData. mapping::MapById> C_submaps; mapping::MapById> C_nodes; + std::map C_landmarks; bool first_submap = true; for (const auto& submap_id_data : submap_data_) { const bool frozen = @@ -160,6 +225,9 @@ void OptimizationProblem::Solve(const std::vector& constraints, C_submaps.at(constraint.submap_id).data(), C_nodes.at(constraint.node_id).data()); } + // Add cost functions for landmarks. + AddLandmarkCostFunctions(landmark_nodes, node_data_, &C_nodes, &C_landmarks, + &problem); // Add penalties for violating odometry or changes between consecutive nodes // if odometry is not available. for (auto node_it = node_data_.begin(); node_it != node_data_.end();) { @@ -222,6 +290,11 @@ OptimizationProblem::submap_data() const { return submap_data_; } +const std::map& +OptimizationProblem::landmark_data() const { + return landmark_data_; +} + const sensor::MapByTime& OptimizationProblem::imu_data() const { return imu_data_; diff --git a/cartographer/mapping_2d/pose_graph/optimization_problem.h b/cartographer/mapping_2d/pose_graph/optimization_problem.h index 3fdbb66..98ebe6a 100644 --- a/cartographer/mapping_2d/pose_graph/optimization_problem.h +++ b/cartographer/mapping_2d/pose_graph/optimization_problem.h @@ -54,6 +54,7 @@ struct SubmapData { class OptimizationProblem { public: using Constraint = mapping::PoseGraphInterface::Constraint; + using LandmarkNode = mapping::PoseGraphInterface::LandmarkNode; explicit OptimizationProblem( const mapping::pose_graph::proto::OptimizationProblemOptions& options); @@ -84,10 +85,12 @@ class OptimizationProblem { // Optimizes the global poses. void Solve(const std::vector& constraints, - const std::set& frozen_trajectories); + const std::set& frozen_trajectories, + const std::map& landmark_nodes); const mapping::MapById& node_data() const; const mapping::MapById& submap_data() const; + const std::map& landmark_data() const; const sensor::MapByTime& imu_data() const; const sensor::MapByTime& odometry_data() const; @@ -102,6 +105,7 @@ class OptimizationProblem { mapping::pose_graph::proto::OptimizationProblemOptions options_; mapping::MapById node_data_; mapping::MapById submap_data_; + std::map landmark_data_; sensor::MapByTime imu_data_; sensor::MapByTime odometry_data_; }; diff --git a/cartographer/mapping_3d/pose_graph.cc b/cartographer/mapping_3d/pose_graph.cc index fdac718..0c63ac1 100644 --- a/cartographer/mapping_3d/pose_graph.cc +++ b/cartographer/mapping_3d/pose_graph.cc @@ -578,7 +578,8 @@ void PoseGraph::RunOptimization() { // No other thread is accessing the optimization_problem_, constraints_ and // frozen_trajectories_ when executing the Solve. Solve is time consuming, so // not taking the mutex before Solve to avoid blocking foreground processing. - optimization_problem_.Solve(constraints_, frozen_trajectories_); + optimization_problem_.Solve(constraints_, frozen_trajectories_, + landmark_nodes_); common::MutexLocker locker(&mutex_); const auto& submap_data = optimization_problem_.submap_data(); diff --git a/cartographer/mapping_3d/pose_graph/optimization_problem.cc b/cartographer/mapping_3d/pose_graph/optimization_problem.cc index 21378f0..211a333 100644 --- a/cartographer/mapping_3d/pose_graph/optimization_problem.cc +++ b/cartographer/mapping_3d/pose_graph/optimization_problem.cc @@ -34,6 +34,7 @@ #include "cartographer/internal/mapping_3d/rotation_cost_function.h" #include "cartographer/mapping/pose_graph/ceres_pose.h" #include "cartographer/mapping_3d/imu_integration.h" +#include "cartographer/mapping_3d/pose_graph/landmark_cost_function.h" #include "cartographer/mapping_3d/pose_graph/spa_cost_function.h" #include "cartographer/mapping_3d/rotation_parameterization.h" #include "cartographer/transform/timestamped_transform.h" @@ -49,6 +50,7 @@ namespace pose_graph { namespace { using ::cartographer::mapping::pose_graph::CeresPose; +using LandmarkNode = ::cartographer::mapping::PoseGraphInterface::LandmarkNode; // For odometry. std::unique_ptr Interpolate( @@ -98,6 +100,63 @@ std::unique_ptr Interpolate( return nullptr; } +// Selects a trajectory node closest in time to the landmark observation and +// applies a relative transform from it. +transform::Rigid3d GetInitialLandmarkPose( + const LandmarkNode::LandmarkObservation& observation, + const NodeData& prev_node, const NodeData& next_node) { + const NodeData& closest_node = + observation.time - prev_node.time < next_node.time - observation.time + ? prev_node + : next_node; + return closest_node.global_pose * observation.landmark_to_tracking_transform; +} + +void AddLandmarkCostFunctions( + const std::map& landmark_nodes, + const mapping::MapById& node_data, + mapping::MapById* C_nodes, + std::map* C_landmarks, ceres::Problem* problem) { + for (const auto& landmark_node : landmark_nodes) { + for (const auto& observation : landmark_node.second.landmark_observations) { + const std::string& landmark_id = landmark_node.first; + // Find the trajectory nodes before and after the landmark observation. + auto next = + node_data.lower_bound(observation.trajectory_id, observation.time); + // The landmark observation was made before the trajectory was created. + if (next == node_data.BeginOfTrajectory(observation.trajectory_id)) { + continue; + } + // The landmark observation was made, but the next trajectory node has + // not been added yet. + if (next == node_data.EndOfTrajectory(observation.trajectory_id)) { + continue; + } + auto prev = std::prev(next); + // Add parameter blocks for the landmark ID if they were not added before. + if (!C_landmarks->count(landmark_id)) { + const transform::Rigid3d starting_point = + landmark_node.second.global_landmark_pose.has_value() + ? landmark_node.second.global_landmark_pose.value() + : GetInitialLandmarkPose(observation, prev->data, next->data); + C_landmarks->emplace( + landmark_id, + CeresPose(starting_point, nullptr /* translation_parametrization */, + common::make_unique(), + problem)); + } + problem->AddResidualBlock( + LandmarkCostFunction::CreateAutoDiffCostFunction( + observation, prev->data, next->data), + nullptr /* loss function */, C_nodes->at(prev->id).rotation(), + C_nodes->at(prev->id).translation(), C_nodes->at(next->id).rotation(), + C_nodes->at(next->id).translation(), + C_landmarks->at(landmark_id).rotation(), + C_landmarks->at(landmark_id).translation()); + } + } +} + } // namespace OptimizationProblem::OptimizationProblem( @@ -169,8 +228,10 @@ void OptimizationProblem::SetMaxNumIterations(const int32 max_num_iterations) { max_num_iterations); } -void OptimizationProblem::Solve(const std::vector& constraints, - const std::set& frozen_trajectories) { +void OptimizationProblem::Solve( + const std::vector& constraints, + const std::set& frozen_trajectories, + const std::map& landmark_nodes) { if (node_data_.empty()) { // Nothing to optimize. return; @@ -192,6 +253,7 @@ void OptimizationProblem::Solve(const std::vector& constraints, CHECK(submap_data_.Contains(mapping::SubmapId{0, 0})); mapping::MapById C_submaps; mapping::MapById C_nodes; + std::map C_landmarks; bool first_submap = true; for (const auto& submap_id_data : submap_data_) { const bool frozen = @@ -251,6 +313,9 @@ void OptimizationProblem::Solve(const std::vector& constraints, C_nodes.at(constraint.node_id).rotation(), C_nodes.at(constraint.node_id).translation()); } + // Add cost functions for landmarks. + AddLandmarkCostFunctions(landmark_nodes, node_data_, &C_nodes, &C_landmarks, + &problem); // Add constraints based on IMU observations of angular velocities and // linear acceleration. if (fix_z_ == FixZ::kNo) { @@ -482,6 +547,11 @@ OptimizationProblem::submap_data() const { return submap_data_; } +const std::map& +OptimizationProblem::landmark_data() const { + return landmark_data_; +} + const sensor::MapByTime& OptimizationProblem::imu_data() const { return imu_data_; diff --git a/cartographer/mapping_3d/pose_graph/optimization_problem.h b/cartographer/mapping_3d/pose_graph/optimization_problem.h index 7a02621..0445921 100644 --- a/cartographer/mapping_3d/pose_graph/optimization_problem.h +++ b/cartographer/mapping_3d/pose_graph/optimization_problem.h @@ -54,6 +54,7 @@ struct SubmapData { class OptimizationProblem { public: using Constraint = mapping::PoseGraphInterface::Constraint; + using LandmarkNode = mapping::PoseGraphInterface::LandmarkNode; enum class FixZ { kYes, kNo }; @@ -88,10 +89,12 @@ class OptimizationProblem { // Optimizes the global poses. void Solve(const std::vector& constraints, - const std::set& frozen_trajectories); + const std::set& frozen_trajectories, + const std::map& landmark_nodes); const mapping::MapById& node_data() const; const mapping::MapById& submap_data() const; + const std::map& landmark_data() const; const sensor::MapByTime& imu_data() const; const sensor::MapByTime& odometry_data() const; const sensor::MapByTime& fixed_frame_pose_data() @@ -113,6 +116,7 @@ class OptimizationProblem { FixZ fix_z_; mapping::MapById node_data_; mapping::MapById submap_data_; + std::map landmark_data_; sensor::MapByTime imu_data_; sensor::MapByTime odometry_data_; sensor::MapByTime fixed_frame_pose_data_; diff --git a/cartographer/mapping_3d/pose_graph/optimization_problem_test.cc b/cartographer/mapping_3d/pose_graph/optimization_problem_test.cc index eb14648..a0695d5 100644 --- a/cartographer/mapping_3d/pose_graph/optimization_problem_test.cc +++ b/cartographer/mapping_3d/pose_graph/optimization_problem_test.cc @@ -170,7 +170,7 @@ TEST_F(OptimizationProblemTest, ReducesNoise) { optimization_problem_.AddSubmap(kTrajectoryId, kSubmap0Transform); optimization_problem_.AddSubmap(kTrajectoryId, kSubmap2Transform); const std::set kFrozen; - optimization_problem_.Solve(constraints, kFrozen); + optimization_problem_.Solve(constraints, kFrozen, {}); double translation_error_after = 0.; double rotation_error_after = 0.;