diff --git a/cartographer/cloud/internal/client/pose_graph_stub.cc b/cartographer/cloud/internal/client/pose_graph_stub.cc index f3f2138..76f1c88 100644 --- a/cartographer/cloud/internal/client/pose_graph_stub.cc +++ b/cartographer/cloud/internal/client/pose_graph_stub.cc @@ -138,7 +138,8 @@ std::map PoseGraphStub::GetLandmarkPoses() } void PoseGraphStub::SetLandmarkPose(const std::string& landmark_id, - const transform::Rigid3d& global_pose) { + const transform::Rigid3d& global_pose, + const bool frozen) { proto::SetLandmarkPoseRequest request; request.mutable_landmark_pose()->set_landmark_id(landmark_id); *request.mutable_landmark_pose()->mutable_global_pose() = diff --git a/cartographer/cloud/internal/client/pose_graph_stub.h b/cartographer/cloud/internal/client/pose_graph_stub.h index 4401f46..b2e6550 100644 --- a/cartographer/cloud/internal/client/pose_graph_stub.h +++ b/cartographer/cloud/internal/client/pose_graph_stub.h @@ -45,7 +45,8 @@ class PoseGraphStub : public ::cartographer::mapping::PoseGraphInterface { std::map GetTrajectoryStates() const override; std::map GetLandmarkPoses() const override; void SetLandmarkPose(const std::string& landmark_id, - const transform::Rigid3d& global_pose) override; + const transform::Rigid3d& global_pose, + const bool frozen = false) override; void DeleteTrajectory(int trajectory_id) override; bool IsTrajectoryFinished(int trajectory_id) const override; bool IsTrajectoryFrozen(int trajectory_id) const override; diff --git a/cartographer/cloud/internal/handlers/set_landmark_pose_handler_test.cc b/cartographer/cloud/internal/handlers/set_landmark_pose_handler_test.cc index c1f1094..015b517 100644 --- a/cartographer/cloud/internal/handlers/set_landmark_pose_handler_test.cc +++ b/cartographer/cloud/internal/handlers/set_landmark_pose_handler_test.cc @@ -53,7 +53,8 @@ TEST_F(SetLandmarkPoseHandlerTest, SetLandmarkPose) { transform::IsNearly( transform::Rigid3d(Eigen::Vector3d(1, 2, 3), Eigen::Quaterniond(4, 5, 6, 7)), - kEps))); + kEps), + false)); test_server_->SendWrite(request); } diff --git a/cartographer/mapping/internal/2d/pose_graph_2d.cc b/cartographer/mapping/internal/2d/pose_graph_2d.cc index b812dd1..ca40340 100644 --- a/cartographer/mapping/internal/2d/pose_graph_2d.cc +++ b/cartographer/mapping/internal/2d/pose_graph_2d.cc @@ -927,10 +927,12 @@ std::map PoseGraph2D::GetLandmarkPoses() } void PoseGraph2D::SetLandmarkPose(const std::string& landmark_id, - const transform::Rigid3d& global_pose) { + const transform::Rigid3d& global_pose, + const bool frozen) { AddWorkItem([=]() LOCKS_EXCLUDED(mutex_) { absl::MutexLock locker(&mutex_); data_.landmark_nodes[landmark_id].global_landmark_pose = global_pose; + data_.landmark_nodes[landmark_id].frozen = frozen; return WorkItem::Result::kDoNotRunOptimization; }); } diff --git a/cartographer/mapping/internal/2d/pose_graph_2d.h b/cartographer/mapping/internal/2d/pose_graph_2d.h index 31267e8..bf20f42 100644 --- a/cartographer/mapping/internal/2d/pose_graph_2d.h +++ b/cartographer/mapping/internal/2d/pose_graph_2d.h @@ -133,7 +133,8 @@ class PoseGraph2D : public PoseGraph { std::map GetLandmarkPoses() const override LOCKS_EXCLUDED(mutex_); void SetLandmarkPose(const std::string& landmark_id, - const transform::Rigid3d& global_pose) override + const transform::Rigid3d& global_pose, + const bool frozen = false) override LOCKS_EXCLUDED(mutex_); sensor::MapByTime GetImuData() const override LOCKS_EXCLUDED(mutex_); diff --git a/cartographer/mapping/internal/3d/pose_graph_3d.cc b/cartographer/mapping/internal/3d/pose_graph_3d.cc index d79c186..ddee1af 100644 --- a/cartographer/mapping/internal/3d/pose_graph_3d.cc +++ b/cartographer/mapping/internal/3d/pose_graph_3d.cc @@ -948,10 +948,12 @@ std::map PoseGraph3D::GetLandmarkPoses() } void PoseGraph3D::SetLandmarkPose(const std::string& landmark_id, - const transform::Rigid3d& global_pose) { + const transform::Rigid3d& global_pose, + const bool frozen) { AddWorkItem([=]() LOCKS_EXCLUDED(mutex_) { absl::MutexLock locker(&mutex_); data_.landmark_nodes[landmark_id].global_landmark_pose = global_pose; + data_.landmark_nodes[landmark_id].frozen = frozen; return WorkItem::Result::kDoNotRunOptimization; }); } diff --git a/cartographer/mapping/internal/3d/pose_graph_3d.h b/cartographer/mapping/internal/3d/pose_graph_3d.h index 2ce6413..f8467aa 100644 --- a/cartographer/mapping/internal/3d/pose_graph_3d.h +++ b/cartographer/mapping/internal/3d/pose_graph_3d.h @@ -131,7 +131,8 @@ class PoseGraph3D : public PoseGraph { std::map GetLandmarkPoses() const override LOCKS_EXCLUDED(mutex_); void SetLandmarkPose(const std::string& landmark_id, - const transform::Rigid3d& global_pose) override + const transform::Rigid3d& global_pose, + const bool frozen = false) override LOCKS_EXCLUDED(mutex_); sensor::MapByTime GetImuData() const override LOCKS_EXCLUDED(mutex_); diff --git a/cartographer/mapping/internal/optimization/optimization_problem_2d.cc b/cartographer/mapping/internal/optimization/optimization_problem_2d.cc index e7d7b6a..cd0eafc 100644 --- a/cartographer/mapping/internal/optimization/optimization_problem_2d.cc +++ b/cartographer/mapping/internal/optimization/optimization_problem_2d.cc @@ -79,16 +79,11 @@ transform::Rigid3d GetInitialLandmarkPose( void AddLandmarkCostFunctions( const std::map& landmark_nodes, - bool freeze_landmarks, const MapById& node_data, + const MapById& node_data, MapById>* C_nodes, std::map* C_landmarks, ceres::Problem* problem, double huber_scale) { 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 = @@ -123,7 +118,8 @@ void AddLandmarkCostFunctions( CeresPose(starting_point, nullptr /* translation_parametrization */, absl::make_unique(), problem)); - if (freeze_landmarks) { + // Set landmark constant if it is frozen. + if (landmark_node.second.frozen) { problem->SetParameterBlockConstant( C_landmarks->at(landmark_id).translation()); problem->SetParameterBlockConstant( @@ -220,7 +216,6 @@ 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; @@ -255,9 +250,8 @@ void OptimizationProblem2D::Solve( C_nodes.at(constraint.node_id).data()); } // Add cost functions for landmarks. - AddLandmarkCostFunctions(landmark_nodes, freeze_landmarks, node_data_, - &C_nodes, &C_landmarks, &problem, - options_.huber_scale()); + AddLandmarkCostFunctions(landmark_nodes, node_data_, &C_nodes, &C_landmarks, + &problem, options_.huber_scale()); // 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/optimization/optimization_problem_3d.cc b/cartographer/mapping/internal/optimization/optimization_problem_3d.cc index d7e1ee0..1b8f632 100644 --- a/cartographer/mapping/internal/optimization/optimization_problem_3d.cc +++ b/cartographer/mapping/internal/optimization/optimization_problem_3d.cc @@ -123,16 +123,12 @@ transform::Rigid3d GetInitialLandmarkPose( void AddLandmarkCostFunctions( const std::map& landmark_nodes, - bool freeze_landmarks, const MapById& node_data, + const MapById& node_data, MapById* C_nodes, std::map* C_landmarks, ceres::Problem* problem, double huber_scale) { 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 = @@ -167,7 +163,8 @@ void AddLandmarkCostFunctions( CeresPose(starting_point, nullptr /* translation_parametrization */, absl::make_unique(), problem)); - if (freeze_landmarks) { + // Set landmark constant if it is frozen. + if (landmark_node.second.frozen) { problem->SetParameterBlockConstant( C_landmarks->at(landmark_id).translation()); problem->SetParameterBlockConstant( @@ -291,7 +288,6 @@ 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; @@ -351,9 +347,8 @@ void OptimizationProblem3D::Solve( C_nodes.at(constraint.node_id).translation()); } // Add cost functions for landmarks. - AddLandmarkCostFunctions(landmark_nodes, freeze_landmarks, node_data_, - &C_nodes, &C_landmarks, &problem, - options_.huber_scale()); + AddLandmarkCostFunctions(landmark_nodes, node_data_, &C_nodes, &C_landmarks, + &problem, options_.huber_scale()); // Add constraints based on IMU observations of angular velocities and // linear acceleration. if (!options_.fix_z_in_3d()) { diff --git a/cartographer/mapping/internal/testing/mock_pose_graph.h b/cartographer/mapping/internal/testing/mock_pose_graph.h index 675d89f..074ac09 100644 --- a/cartographer/mapping/internal/testing/mock_pose_graph.h +++ b/cartographer/mapping/internal/testing/mock_pose_graph.h @@ -48,8 +48,8 @@ class MockPoseGraph : public mapping::PoseGraphInterface { std::map()); MOCK_CONST_METHOD0(GetLandmarkPoses, std::map()); - MOCK_METHOD2(SetLandmarkPose, - void(const std::string&, const transform::Rigid3d&)); + MOCK_METHOD3(SetLandmarkPose, + void(const std::string&, const transform::Rigid3d&, const bool)); MOCK_METHOD1(DeleteTrajectory, void(int)); MOCK_CONST_METHOD1(IsTrajectoryFinished, bool(int)); MOCK_CONST_METHOD1(IsTrajectoryFrozen, bool(int)); diff --git a/cartographer/mapping/map_builder.cc b/cartographer/mapping/map_builder.cc index 4c77c8e..895b453 100644 --- a/cartographer/mapping/map_builder.cc +++ b/cartographer/mapping/map_builder.cc @@ -279,7 +279,8 @@ std::map MapBuilder::LoadState( // Set global poses of landmarks. for (const auto& landmark : pose_graph_proto.landmark_poses()) { pose_graph_->SetLandmarkPose(landmark.landmark_id(), - transform::ToRigid3(landmark.global_pose())); + transform::ToRigid3(landmark.global_pose()), + true); } MapById submap_id_to_submap; diff --git a/cartographer/mapping/pose_graph_interface.h b/cartographer/mapping/pose_graph_interface.h index e950401..2701e93 100644 --- a/cartographer/mapping/pose_graph_interface.h +++ b/cartographer/mapping/pose_graph_interface.h @@ -62,6 +62,7 @@ class PoseGraphInterface { }; std::vector landmark_observations; absl::optional global_landmark_pose; + bool frozen = false; }; struct SubmapPose { @@ -123,7 +124,8 @@ class PoseGraphInterface { // Sets global pose of landmark 'landmark_id' to given 'global_pose'. virtual void SetLandmarkPose(const std::string& landmark_id, - const transform::Rigid3d& global_pose) = 0; + const transform::Rigid3d& global_pose, + const bool frozen = false) = 0; // Deletes a trajectory asynchronously. virtual void DeleteTrajectory(int trajectory_id) = 0;