12345qiupeng
/
cartographer
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Deserialize landmark poses. (#1064)

master
Alexander Belyaev 2018-04-13 16:38:45 +02:00 committed by Wally B. Feed
parent 46d3a9443a
commit 74f74c4b61
13 changed files with 89 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
cartographer/cloud/internal/client/pose_graph_stub.cc
View File

@ -109,6 +109,11 @@ std::map<std::string, transform::Rigid3d> 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);

2
cartographer/cloud/internal/client/pose_graph_stub.h
View File

@ -39,6 +39,8 @@ class PoseGraphStub : public ::cartographer::mapping::PoseGraphInterface {
mapping::MapById<mapping::NodeId, mapping::TrajectoryNodePose>
GetTrajectoryNodePoses() override;
std::map<std::string, transform::Rigid3d> 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<int, mapping::PoseGraphInterface::TrajectoryData> GetTrajectoryData()

8
cartographer/mapping/internal/2d/pose_graph_2d.cc
View File

@ -627,6 +627,14 @@ std::map<std::string, transform::Rigid3d> 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<sensor::ImuData> PoseGraph2D::GetImuData() {
common::MutexLocker locker(&mutex_);
return optimization_problem_->imu_data();

3
cartographer/mapping/internal/2d/pose_graph_2d.h
View File

@ -121,6 +121,9 @@ class PoseGraph2D : public PoseGraph {
EXCLUDES(mutex_);
std::map<std::string, transform::Rigid3d> GetLandmarkPoses() override
EXCLUDES(mutex_);
void SetLandmarkPose(const std::string& landmark_id,
const transform::Rigid3d& global_pose) override
EXCLUDES(mutex_);
sensor::MapByTime<sensor::ImuData> GetImuData() override EXCLUDES(mutex_);
sensor::MapByTime<sensor::OdometryData> GetOdometryData() override
EXCLUDES(mutex_);

8
cartographer/mapping/internal/3d/pose_graph_3d.cc
View File

@ -669,6 +669,14 @@ std::map<std::string, transform::Rigid3d> 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<sensor::ImuData> PoseGraph3D::GetImuData() {
common::MutexLocker locker(&mutex_);
return optimization_problem_->imu_data();

3
cartographer/mapping/internal/3d/pose_graph_3d.h
View File

@ -120,6 +120,9 @@ class PoseGraph3D : public PoseGraph {
EXCLUDES(mutex_);
std::map<std::string, transform::Rigid3d> GetLandmarkPoses() override
EXCLUDES(mutex_);
void SetLandmarkPose(const std::string& landmark_id,
const transform::Rigid3d& global_pose) override
EXCLUDES(mutex_);
sensor::MapByTime<sensor::ImuData> GetImuData() override EXCLUDES(mutex_);
sensor::MapByTime<sensor::OdometryData> GetOdometryData() override
EXCLUDES(mutex_);

49
cartographer/mapping/internal/3d/pose_graph_3d_test.cc
View File

@ -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<Constraint>& constraints,
const std::set<int>& frozen_trajectories,
const std::map<std::string, LandmarkNode>& 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<Constraint>&, const std::set<int>&,
const std::map<std::string, LandmarkNode>&));
};
class PoseGraph3DForTesting : public PoseGraph3D {
public:
PoseGraph3DForTesting(
@ -78,7 +77,7 @@ class PoseGraph3DTest : public ::testing::Test {
void BuildPoseGraphWithFakeOptimization() {
auto optimization_problem =
common::make_unique<pose_graph::FakeOptimizationProblem3D>();
common::make_unique<MockOptimizationProblem3D>();
pose_graph_ = common::make_unique<PoseGraph3DForTesting>(
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);

13
cartographer/mapping/internal/test_helpers.cc
View File

@ -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

5
cartographer/mapping/internal/test_helpers.h
View File

@ -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

2
cartographer/mapping/internal/testing/mock_pose_graph.h
View File

@ -43,6 +43,8 @@ class MockPoseGraph : public mapping::PoseGraphInterface {
GetTrajectoryNodePoses,
mapping::MapById<mapping::NodeId, mapping::TrajectoryNodePose>());
MOCK_METHOD0(GetLandmarkPoses, std::map<std::string, transform::Rigid3d>());
MOCK_METHOD2(SetLandmarkPose,
void(const std::string&, const transform::Rigid3d&));
MOCK_METHOD1(IsTrajectoryFinished, bool(int));
MOCK_METHOD1(IsTrajectoryFrozen, bool(int));
MOCK_METHOD0(GetTrajectoryData,

6
cartographer/mapping/map_builder.cc
View File

@ -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)) {

4
cartographer/mapping/pose_graph.h
View File

@ -133,6 +133,10 @@ class PoseGraph : public PoseGraphInterface {
virtual std::map<std::string /* landmark ID */, PoseGraph::LandmarkNode>
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,

4
cartographer/mapping/pose_graph_interface.h
View File

@ -108,6 +108,10 @@ class PoseGraphInterface {
// Returns the current optimized landmark poses.
virtual std::map<std::string, transform::Rigid3d> 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;