diff --git a/cartographer/cloud/internal/client/pose_graph_stub.cc b/cartographer/cloud/internal/client/pose_graph_stub.cc index 6356473..fc9b577 100644 --- a/cartographer/cloud/internal/client/pose_graph_stub.cc +++ b/cartographer/cloud/internal/client/pose_graph_stub.cc @@ -109,6 +109,11 @@ std::map PoseGraphStub::GetLandmarkPoses() { return landmark_poses; } +void PoseGraphStub::SetLandmarkPose(const std::string& landmark_id, + const transform::Rigid3d& global_pose) { + LOG(FATAL) << "Not implemented"; +} + bool PoseGraphStub::IsTrajectoryFinished(int trajectory_id) { proto::IsTrajectoryFinishedRequest request; request.set_trajectory_id(trajectory_id); diff --git a/cartographer/cloud/internal/client/pose_graph_stub.h b/cartographer/cloud/internal/client/pose_graph_stub.h index 9c7e90d..4b41fe6 100644 --- a/cartographer/cloud/internal/client/pose_graph_stub.h +++ b/cartographer/cloud/internal/client/pose_graph_stub.h @@ -39,6 +39,8 @@ class PoseGraphStub : public ::cartographer::mapping::PoseGraphInterface { mapping::MapById GetTrajectoryNodePoses() override; std::map GetLandmarkPoses() override; + void SetLandmarkPose(const std::string& landmark_id, + const transform::Rigid3d& global_pose) override; bool IsTrajectoryFinished(int trajectory_id) override; bool IsTrajectoryFrozen(int trajectory_id) override; std::map GetTrajectoryData() diff --git a/cartographer/mapping/internal/2d/pose_graph_2d.cc b/cartographer/mapping/internal/2d/pose_graph_2d.cc index 55cb8f2..e9fbfa0 100644 --- a/cartographer/mapping/internal/2d/pose_graph_2d.cc +++ b/cartographer/mapping/internal/2d/pose_graph_2d.cc @@ -627,6 +627,14 @@ std::map PoseGraph2D::GetLandmarkPoses() { return landmark_poses; } +void PoseGraph2D::SetLandmarkPose(const std::string& landmark_id, + const transform::Rigid3d& global_pose) { + common::MutexLocker locker(&mutex_); + AddWorkItem([=]() REQUIRES(mutex_) { + landmark_nodes_[landmark_id].global_landmark_pose = global_pose; + }); +} + sensor::MapByTime PoseGraph2D::GetImuData() { common::MutexLocker locker(&mutex_); return optimization_problem_->imu_data(); diff --git a/cartographer/mapping/internal/2d/pose_graph_2d.h b/cartographer/mapping/internal/2d/pose_graph_2d.h index 3276f42..49ad011 100644 --- a/cartographer/mapping/internal/2d/pose_graph_2d.h +++ b/cartographer/mapping/internal/2d/pose_graph_2d.h @@ -121,6 +121,9 @@ class PoseGraph2D : public PoseGraph { EXCLUDES(mutex_); std::map GetLandmarkPoses() override EXCLUDES(mutex_); + void SetLandmarkPose(const std::string& landmark_id, + const transform::Rigid3d& global_pose) override + EXCLUDES(mutex_); sensor::MapByTime GetImuData() override EXCLUDES(mutex_); sensor::MapByTime GetOdometryData() override EXCLUDES(mutex_); diff --git a/cartographer/mapping/internal/3d/pose_graph_3d.cc b/cartographer/mapping/internal/3d/pose_graph_3d.cc index e2d1df7..c572135 100644 --- a/cartographer/mapping/internal/3d/pose_graph_3d.cc +++ b/cartographer/mapping/internal/3d/pose_graph_3d.cc @@ -669,6 +669,14 @@ std::map PoseGraph3D::GetLandmarkPoses() { return landmark_poses; } +void PoseGraph3D::SetLandmarkPose(const std::string& landmark_id, + const transform::Rigid3d& global_pose) { + common::MutexLocker locker(&mutex_); + AddWorkItem([=]() REQUIRES(mutex_) { + landmark_nodes_[landmark_id].global_landmark_pose = global_pose; + }); +} + sensor::MapByTime PoseGraph3D::GetImuData() { common::MutexLocker locker(&mutex_); return optimization_problem_->imu_data(); diff --git a/cartographer/mapping/internal/3d/pose_graph_3d.h b/cartographer/mapping/internal/3d/pose_graph_3d.h index ec09f94..debc8fa 100644 --- a/cartographer/mapping/internal/3d/pose_graph_3d.h +++ b/cartographer/mapping/internal/3d/pose_graph_3d.h @@ -120,6 +120,9 @@ class PoseGraph3D : public PoseGraph { EXCLUDES(mutex_); std::map GetLandmarkPoses() override EXCLUDES(mutex_); + void SetLandmarkPose(const std::string& landmark_id, + const transform::Rigid3d& global_pose) override + EXCLUDES(mutex_); sensor::MapByTime GetImuData() override EXCLUDES(mutex_); sensor::MapByTime GetOdometryData() override EXCLUDES(mutex_); diff --git a/cartographer/mapping/internal/3d/pose_graph_3d_test.cc b/cartographer/mapping/internal/3d/pose_graph_3d_test.cc index c5c01d9..4b89900 100644 --- a/cartographer/mapping/internal/3d/pose_graph_3d_test.cc +++ b/cartographer/mapping/internal/3d/pose_graph_3d_test.cc @@ -26,23 +26,22 @@ namespace cartographer { namespace mapping { -namespace pose_graph { - -class FakeOptimizationProblem3D : public OptimizationProblem3D { - public: - FakeOptimizationProblem3D() - : OptimizationProblem3D(proto::OptimizationProblemOptions{}) {} - ~FakeOptimizationProblem3D() override = default; - void Solve( - const std::vector& constraints, - const std::set& frozen_trajectories, - const std::map& landmark_nodes) override {} -}; - -} // namespace pose_graph - namespace { +using ::cartographer::mapping::pose_graph::OptimizationProblem3D; +using ::cartographer::mapping::pose_graph::proto::OptimizationProblemOptions; +using ::cartographer::transform::Rigid3d; + +class MockOptimizationProblem3D : public OptimizationProblem3D { + public: + MockOptimizationProblem3D() + : OptimizationProblem3D(OptimizationProblemOptions{}) {} + ~MockOptimizationProblem3D() override = default; + + MOCK_METHOD3(Solve, void(const std::vector&, const std::set&, + const std::map&)); +}; + class PoseGraph3DForTesting : public PoseGraph3D { public: PoseGraph3DForTesting( @@ -78,7 +77,7 @@ class PoseGraph3DTest : public ::testing::Test { void BuildPoseGraphWithFakeOptimization() { auto optimization_problem = - common::make_unique(); + common::make_unique(); pose_graph_ = common::make_unique( pose_graph_options_, std::move(optimization_problem), thread_pool_.get()); @@ -109,14 +108,16 @@ TEST_F(PoseGraph3DTest, BasicSerialization) { proto::PoseGraph proto; auto fake_node = test::CreateFakeNode(); test::AddToProtoGraph(fake_node, &proto); - pose_graph_->AddNodeFromProto(transform::Rigid3d::Identity(), fake_node); + pose_graph_->AddNodeFromProto(Rigid3d::Identity(), fake_node); auto fake_submap = test::CreateFakeSubmap3D(); test::AddToProtoGraph(fake_submap, &proto); - pose_graph_->AddSubmapFromProto(transform::Rigid3d::Identity(), fake_submap); + pose_graph_->AddSubmapFromProto(Rigid3d::Identity(), fake_submap); test::AddToProtoGraph(test::CreateFakeConstraint(fake_node, fake_submap), &proto); pose_graph_->AddSerializedConstraints(FromProto(proto.constraint())); - + test::AddToProtoGraph( + test::CreateFakeLandmark("landmark_id", Rigid3d::Identity()), &proto); + pose_graph_->SetLandmarkPose("landmark_id", Rigid3d::Identity()); pose_graph_->WaitForAllComputations(); proto::PoseGraph actual_proto = pose_graph_->ToProto(); EXPECT_TRUE(google::protobuf::util::MessageDifferencer::Equals( @@ -127,6 +128,8 @@ TEST_F(PoseGraph3DTest, BasicSerialization) { proto.trajectory(0).submap(0), actual_proto.trajectory(0).submap(0))); EXPECT_TRUE(google::protobuf::util::MessageDifferencer::Equals( proto.trajectory(0), actual_proto.trajectory(0))); + EXPECT_TRUE(google::protobuf::util::MessageDifferencer::Equals( + proto.landmarks(0), actual_proto.landmarks(0))); EXPECT_TRUE( google::protobuf::util::MessageDifferencer::Equals(proto, actual_proto)); } @@ -140,11 +143,11 @@ TEST_F(PoseGraph3DTest, PureLocalizationTrimmer) { for (int i = 0; i < num_submaps_to_create; ++i) { int submap_index = (i < 3) ? 42 + i : 100 + i; auto submap = test::CreateFakeSubmap3D(trajectory_id, submap_index); - pose_graph_->AddSubmapFromProto(transform::Rigid3d::Identity(), submap); + pose_graph_->AddSubmapFromProto(Rigid3d::Identity(), submap); for (int j = 0; j < num_nodes_per_submap; ++j) { int node_index = 7 + num_nodes_per_submap * submap_index + j; auto node = test::CreateFakeNode(trajectory_id, node_index); - pose_graph_->AddNodeFromProto(transform::Rigid3d::Identity(), node); + pose_graph_->AddNodeFromProto(Rigid3d::Identity(), node); proto::PoseGraph proto; auto constraint = test::CreateFakeConstraint(node, submap); // TODO(gaschler): Also remove inter constraints when all references are @@ -219,11 +222,11 @@ TEST_F(PoseGraph3DTest, EvenSubmapTrimmer) { for (int i = 0; i < num_submaps_to_create; ++i) { int submap_index = 42 + i; auto submap = test::CreateFakeSubmap3D(trajectory_id, submap_index); - pose_graph_->AddSubmapFromProto(transform::Rigid3d::Identity(), submap); + pose_graph_->AddSubmapFromProto(Rigid3d::Identity(), submap); for (int j = 0; j < num_nodes_per_submap; ++j) { int node_index = 7 + num_nodes_per_submap * i + j; auto node = test::CreateFakeNode(trajectory_id, node_index); - pose_graph_->AddNodeFromProto(transform::Rigid3d::Identity(), node); + pose_graph_->AddNodeFromProto(Rigid3d::Identity(), node); proto::PoseGraph proto; auto constraint = test::CreateFakeConstraint(node, submap); constraint.set_tag(proto::PoseGraph::Constraint::INTRA_SUBMAP); diff --git a/cartographer/mapping/internal/test_helpers.cc b/cartographer/mapping/internal/test_helpers.cc index ebbfee5..3a3234c 100644 --- a/cartographer/mapping/internal/test_helpers.cc +++ b/cartographer/mapping/internal/test_helpers.cc @@ -120,6 +120,14 @@ proto::Trajectory* CreateTrajectoryIfNeeded(int trajectory_id, return trajectory; } +proto::PoseGraph::Landmark CreateFakeLandmark( + const std::string& landmark_id, const transform::Rigid3d& global_pose) { + proto::PoseGraph::Landmark landmark; + landmark.set_landmark_id(landmark_id); + *landmark.mutable_global_pose() = transform::ToProto(global_pose); + return landmark; +} + void AddToProtoGraph(const proto::Node& node_data, proto::PoseGraph* pose_graph) { auto* trajectory = @@ -148,6 +156,11 @@ void AddToProtoGraph(const proto::PoseGraph::Constraint& constraint, *pose_graph->add_constraint() = constraint; } +void AddToProtoGraph(const proto::PoseGraph::Landmark& landmark, + proto::PoseGraph* pose_graph) { + *pose_graph->add_landmarks() = landmark; +} + } // namespace test } // namespace mapping } // namespace cartographer diff --git a/cartographer/mapping/internal/test_helpers.h b/cartographer/mapping/internal/test_helpers.h index f4ea723..9f966ff 100644 --- a/cartographer/mapping/internal/test_helpers.h +++ b/cartographer/mapping/internal/test_helpers.h @@ -43,6 +43,8 @@ proto::PoseGraph::Constraint CreateFakeConstraint(const proto::Node& node, proto::Trajectory* CreateTrajectoryIfNeeded(int trajectory_id, proto::PoseGraph* pose_graph); +proto::PoseGraph::Landmark CreateFakeLandmark( + const std::string& landmark_id, const transform::Rigid3d& global_pose); void AddToProtoGraph(const proto::Node& node_data, proto::PoseGraph* pose_graph); @@ -53,6 +55,9 @@ void AddToProtoGraph(const proto::Submap& submap_data, void AddToProtoGraph(const proto::PoseGraph::Constraint& constraint, proto::PoseGraph* pose_graph); +void AddToProtoGraph(const proto::PoseGraph::Landmark& landmark_node, + proto::PoseGraph* pose_graph); + } // namespace test } // namespace mapping } // namespace cartographer diff --git a/cartographer/mapping/internal/testing/mock_pose_graph.h b/cartographer/mapping/internal/testing/mock_pose_graph.h index 780c39b..4a54a12 100644 --- a/cartographer/mapping/internal/testing/mock_pose_graph.h +++ b/cartographer/mapping/internal/testing/mock_pose_graph.h @@ -43,6 +43,8 @@ class MockPoseGraph : public mapping::PoseGraphInterface { GetTrajectoryNodePoses, mapping::MapById()); MOCK_METHOD0(GetLandmarkPoses, std::map()); + MOCK_METHOD2(SetLandmarkPose, + void(const std::string&, const transform::Rigid3d&)); MOCK_METHOD1(IsTrajectoryFinished, bool(int)); MOCK_METHOD1(IsTrajectoryFrozen, bool(int)); MOCK_METHOD0(GetTrajectoryData, diff --git a/cartographer/mapping/map_builder.cc b/cartographer/mapping/map_builder.cc index 8919f11..be3bfc5 100644 --- a/cartographer/mapping/map_builder.cc +++ b/cartographer/mapping/map_builder.cc @@ -366,6 +366,12 @@ void MapBuilder::LoadState(io::ProtoStreamReaderInterface* const reader, } } + // Set global poses of landmarks. + for (const auto& landmark : pose_graph_proto.landmarks()) { + pose_graph_->SetLandmarkPose(landmark.landmark_id(), + transform::ToRigid3(landmark.global_pose())); + } + for (;;) { proto::SerializedData proto; if (!reader->ReadProto(&proto)) { diff --git a/cartographer/mapping/pose_graph.h b/cartographer/mapping/pose_graph.h index 2c57826..1b595f4 100644 --- a/cartographer/mapping/pose_graph.h +++ b/cartographer/mapping/pose_graph.h @@ -133,6 +133,10 @@ class PoseGraph : public PoseGraphInterface { virtual std::map GetLandmarkNodes() = 0; + // 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; + // Sets a relative initial pose 'relative_pose' for 'from_trajectory_id' with // respect to 'to_trajectory_id' at time 'time'. virtual void SetInitialTrajectoryPose(int from_trajectory_id, diff --git a/cartographer/mapping/pose_graph_interface.h b/cartographer/mapping/pose_graph_interface.h index 03fe1c9..e17ef5f 100644 --- a/cartographer/mapping/pose_graph_interface.h +++ b/cartographer/mapping/pose_graph_interface.h @@ -108,6 +108,10 @@ class PoseGraphInterface { // Returns the current optimized landmark poses. virtual std::map GetLandmarkPoses() = 0; + // 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; + // Checks if the given trajectory is finished. virtual bool IsTrajectoryFinished(int trajectory_id) = 0;