diff --git a/cartographer/mapping/internal/2d/pose_graph/optimization_problem_2d.cc b/cartographer/mapping/internal/2d/pose_graph/optimization_problem_2d.cc index 5e31729..e3b3402 100644 --- a/cartographer/mapping/internal/2d/pose_graph/optimization_problem_2d.cc +++ b/cartographer/mapping/internal/2d/pose_graph/optimization_problem_2d.cc @@ -74,10 +74,15 @@ transform::Rigid3d GetInitialLandmarkPose( void AddLandmarkCostFunctions( const std::map& landmark_nodes, - const MapById& node_data, + bool freeze_landmarks, const MapById& node_data, MapById>* C_nodes, std::map* C_landmarks, ceres::Problem* problem) { for (const auto& landmark_node : landmark_nodes) { + // Do not use landmarks that were not optimized for localization. + if (!landmark_node.second.global_landmark_pose.has_value() && + freeze_landmarks) { + continue; + } for (const auto& observation : landmark_node.second.landmark_observations) { const std::string& landmark_id = landmark_node.first; const auto& begin_of_trajectory = @@ -109,6 +114,12 @@ void AddLandmarkCostFunctions( CeresPose(starting_point, nullptr /* translation_parametrization */, common::make_unique(), problem)); + if (freeze_landmarks) { + problem->SetParameterBlockConstant( + C_landmarks->at(landmark_id).translation()); + problem->SetParameterBlockConstant( + C_landmarks->at(landmark_id).rotation()); + } } problem->AddResidualBlock( LandmarkCostFunction2D::CreateAutoDiffCostFunction( @@ -198,6 +209,7 @@ void OptimizationProblem2D::Solve( MapById> C_nodes; std::map C_landmarks; bool first_submap = true; + bool freeze_landmarks = !frozen_trajectories.empty(); for (const auto& submap_id_data : submap_data_) { const bool frozen = frozen_trajectories.count(submap_id_data.id.trajectory_id) != 0; @@ -231,9 +243,9 @@ void OptimizationProblem2D::Solve( 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 cost functions for landmarks. + AddLandmarkCostFunctions(landmark_nodes, freeze_landmarks, 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();) { diff --git a/cartographer/mapping/internal/2d/pose_graph_2d.cc b/cartographer/mapping/internal/2d/pose_graph_2d.cc index 79862a6..61175b7 100644 --- a/cartographer/mapping/internal/2d/pose_graph_2d.cc +++ b/cartographer/mapping/internal/2d/pose_graph_2d.cc @@ -550,8 +550,8 @@ void PoseGraph2D::RunOptimization() { } // No other thread is accessing the optimization_problem_, constraints_, - // frozen_trajectories_ and landmark_nodes_ when executing the Solve. Solve - // is time consuming, so not taking the mutex before Solve to avoid blocking + // frozen_trajectories_ and landmark_nodes_ 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_, landmark_nodes_); diff --git a/cartographer/mapping/internal/2d/pose_graph_2d.h b/cartographer/mapping/internal/2d/pose_graph_2d.h index ab2d561..ec33e57 100644 --- a/cartographer/mapping/internal/2d/pose_graph_2d.h +++ b/cartographer/mapping/internal/2d/pose_graph_2d.h @@ -257,6 +257,9 @@ class PoseGraph2D : public PoseGraph { // Set of all frozen trajectories not being optimized. std::set frozen_trajectories_ GUARDED_BY(mutex_); + // Whether or not optimize landmark poses. + bool freeze_landmarks_ GUARDED_BY(mutex_) = false; + // Set of all finished trajectories. std::set finished_trajectories_ GUARDED_BY(mutex_); diff --git a/cartographer/mapping/internal/3d/pose_graph/optimization_problem_3d.cc b/cartographer/mapping/internal/3d/pose_graph/optimization_problem_3d.cc index 198ce8a..c33123e 100644 --- a/cartographer/mapping/internal/3d/pose_graph/optimization_problem_3d.cc +++ b/cartographer/mapping/internal/3d/pose_graph/optimization_problem_3d.cc @@ -117,10 +117,15 @@ transform::Rigid3d GetInitialLandmarkPose( void AddLandmarkCostFunctions( const std::map& landmark_nodes, - const MapById& node_data, + bool freeze_landmarks, const MapById& node_data, MapById* C_nodes, std::map* C_landmarks, ceres::Problem* problem) { for (const auto& landmark_node : landmark_nodes) { + // Do not use landmarks that were not optimized for localization. + if (!landmark_node.second.global_landmark_pose.has_value() && + freeze_landmarks) { + continue; + } for (const auto& observation : landmark_node.second.landmark_observations) { const std::string& landmark_id = landmark_node.first; const auto& begin_of_trajectory = @@ -152,6 +157,12 @@ void AddLandmarkCostFunctions( CeresPose(starting_point, nullptr /* translation_parametrization */, common::make_unique(), problem)); + if (freeze_landmarks) { + problem->SetParameterBlockConstant( + C_landmarks->at(landmark_id).translation()); + problem->SetParameterBlockConstant( + C_landmarks->at(landmark_id).rotation()); + } } problem->AddResidualBlock( LandmarkCostFunction3D::CreateAutoDiffCostFunction( @@ -267,6 +278,7 @@ void OptimizationProblem3D::Solve( MapById C_nodes; std::map C_landmarks; bool first_submap = true; + bool freeze_landmarks = !frozen_trajectories.empty(); for (const auto& submap_id_data : submap_data_) { const bool frozen = frozen_trajectories.count(submap_id_data.id.trajectory_id) != 0; @@ -325,9 +337,9 @@ void OptimizationProblem3D::Solve( 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 cost functions for landmarks. + AddLandmarkCostFunctions(landmark_nodes, freeze_landmarks, node_data_, + &C_nodes, &C_landmarks, &problem); // Add constraints based on IMU observations of angular velocities and // linear acceleration. if (fix_z_ == FixZ::kNo) { diff --git a/cartographer/mapping/internal/3d/pose_graph_3d.cc b/cartographer/mapping/internal/3d/pose_graph_3d.cc index 7c81fd8..78abc2a 100644 --- a/cartographer/mapping/internal/3d/pose_graph_3d.cc +++ b/cartographer/mapping/internal/3d/pose_graph_3d.cc @@ -593,8 +593,8 @@ void PoseGraph3D::RunOptimization() { } // No other thread is accessing the optimization_problem_, constraints_, - // frozen_trajectories_ and landmark_nodes_ when executing the Solve. Solve - // is time consuming, so not taking the mutex before Solve to avoid blocking + // frozen_trajectories_ and landmark_nodes_ 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_, landmark_nodes_); diff --git a/cartographer/mapping/internal/3d/pose_graph_3d.h b/cartographer/mapping/internal/3d/pose_graph_3d.h index f4d1553..f3e2dcb 100644 --- a/cartographer/mapping/internal/3d/pose_graph_3d.h +++ b/cartographer/mapping/internal/3d/pose_graph_3d.h @@ -261,6 +261,9 @@ class PoseGraph3D : public PoseGraph { // Set of all frozen trajectories not being optimized. std::set frozen_trajectories_ GUARDED_BY(mutex_); + // Whether or not optimize landmark poses. + bool freeze_landmarks_ GUARDED_BY(mutex_) = false; + // Set of all finished trajectories. std::set finished_trajectories_ GUARDED_BY(mutex_); diff --git a/cartographer/mapping/map_builder.cc b/cartographer/mapping/map_builder.cc index 2eb78f1..2208510 100644 --- a/cartographer/mapping/map_builder.cc +++ b/cartographer/mapping/map_builder.cc @@ -369,7 +369,11 @@ void MapBuilder::LoadState(io::ProtoStreamReaderInterface* const reader, sensor::FromProto( proto.fixed_frame_pose_data().fixed_frame_pose_data())); } - // TODO(pifon2a, ojura): deserialize landmarks + if (proto.has_landmark_data()) { + pose_graph_->AddLandmarkData( + trajectory_remapping.at(proto.landmark_data().trajectory_id()), + sensor::FromProto(proto.landmark_data().landmark_data())); + } } } diff --git a/cartographer/mapping/proto/serialization.proto b/cartographer/mapping/proto/serialization.proto index 62f20e0..4a020c8 100644 --- a/cartographer/mapping/proto/serialization.proto +++ b/cartographer/mapping/proto/serialization.proto @@ -48,6 +48,11 @@ message FixedFramePoseData { sensor.proto.FixedFramePoseData fixed_frame_pose_data = 2; } +message LandmarkData { + int32 trajectory_id = 1; + sensor.proto.LandmarkData landmark_data = 2; +} + message TrajectoryData { int32 trajectory_id = 1; double gravity_constant = 2; @@ -62,6 +67,7 @@ message SerializedData { OdometryData odometry_data = 4; FixedFramePoseData fixed_frame_pose_data = 5; TrajectoryData trajectory_data = 6; + LandmarkData landmark_data = 7; } message LocalSlamResultData {