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

Add IsTrajectoryFrozen to PoseGraph (#962) 

Adds IsTrajectoryFrozen to the PoseGraph interface
master
Kevin Daun 2018-03-08 11:18:08 +01:00 committed by Wally B. Feed
parent ec078c7e1c
commit 82a491181d
8 changed files with 19 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -108,6 +108,10 @@ bool PoseGraphStub::IsTrajectoryFinished(int trajectory_id) {
LOG(FATAL) << "Not implemented"; LOG(FATAL) << "Not implemented";
} }
bool PoseGraphStub::IsTrajectoryFrozen(int trajectory_id) {
LOG(FATAL) << "Not implemented";
}
std::map<int, mapping::PoseGraphInterface::TrajectoryData> std::map<int, mapping::PoseGraphInterface::TrajectoryData>
PoseGraphStub::GetTrajectoryData() { PoseGraphStub::GetTrajectoryData() {
LOG(FATAL) << "Not implemented"; LOG(FATAL) << "Not implemented";

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

@ -40,6 +40,7 @@ class PoseGraphStub : public ::cartographer::mapping::PoseGraphInterface {
GetTrajectoryNodePoses() override; GetTrajectoryNodePoses() override;
std::map<std::string, transform::Rigid3d> GetLandmarkPoses() override; std::map<std::string, transform::Rigid3d> GetLandmarkPoses() override;
bool IsTrajectoryFinished(int trajectory_id) override; bool IsTrajectoryFinished(int trajectory_id) override;
bool IsTrajectoryFrozen(int trajectory_id) override;
std::map<int, mapping::PoseGraphInterface::TrajectoryData> GetTrajectoryData() std::map<int, mapping::PoseGraphInterface::TrajectoryData> GetTrajectoryData()
override; override;
std::vector<Constraint> constraints() override; std::vector<Constraint> constraints() override;

4
cartographer/mapping/2d/pose_graph_2d.cc
View File

@ -417,6 +417,10 @@ void PoseGraph2D::FreezeTrajectory(const int trajectory_id) {
}); });
} }
bool PoseGraph2D::IsTrajectoryFrozen(const int trajectory_id) {
return frozen_trajectories_.count(trajectory_id) > 0;
}
void PoseGraph2D::AddSubmapFromProto( void PoseGraph2D::AddSubmapFromProto(
const transform::Rigid3d& global_submap_pose, const proto::Submap& submap) { const transform::Rigid3d& global_submap_pose, const proto::Submap& submap) {
if (!submap.has_submap_2d()) { if (!submap.has_submap_2d()) {

1
cartographer/mapping/2d/pose_graph_2d.h
View File

@ -93,6 +93,7 @@ class PoseGraph2D : public PoseGraph {
void FinishTrajectory(int trajectory_id) override; void FinishTrajectory(int trajectory_id) override;
bool IsTrajectoryFinished(int trajectory_id) override; bool IsTrajectoryFinished(int trajectory_id) override;
void FreezeTrajectory(int trajectory_id) override; void FreezeTrajectory(int trajectory_id) override;
bool IsTrajectoryFrozen(int trajectory_id) override;
void AddSubmapFromProto(const transform::Rigid3d& global_submap_pose, void AddSubmapFromProto(const transform::Rigid3d& global_submap_pose,
const proto::Submap& submap) override; const proto::Submap& submap) override;
void AddNodeFromProto(const transform::Rigid3d& global_pose, void AddNodeFromProto(const transform::Rigid3d& global_pose,

4
cartographer/mapping/3d/pose_graph_3d.cc
View File

@ -435,6 +435,10 @@ void PoseGraph3D::FreezeTrajectory(const int trajectory_id) {
}); });
} }
bool PoseGraph3D::IsTrajectoryFrozen(const int trajectory_id) {
return frozen_trajectories_.count(trajectory_id) > 0;
}
void PoseGraph3D::AddSubmapFromProto( void PoseGraph3D::AddSubmapFromProto(
const transform::Rigid3d& global_submap_pose, const proto::Submap& submap) { const transform::Rigid3d& global_submap_pose, const proto::Submap& submap) {
if (!submap.has_submap_3d()) { if (!submap.has_submap_3d()) {

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

@ -92,6 +92,7 @@ class PoseGraph3D : public PoseGraph {
void FinishTrajectory(int trajectory_id) override; void FinishTrajectory(int trajectory_id) override;
bool IsTrajectoryFinished(int trajectory_id) override; bool IsTrajectoryFinished(int trajectory_id) override;
void FreezeTrajectory(int trajectory_id) override; void FreezeTrajectory(int trajectory_id) override;
bool IsTrajectoryFrozen(int trajectory_id) override;
void AddSubmapFromProto(const transform::Rigid3d& global_submap_pose, void AddSubmapFromProto(const transform::Rigid3d& global_submap_pose,
const proto::Submap& submap) override; const proto::Submap& submap) override;
void AddNodeFromProto(const transform::Rigid3d& global_pose, void AddNodeFromProto(const transform::Rigid3d& global_pose,

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

@ -44,6 +44,7 @@ class MockPoseGraph : public mapping::PoseGraphInterface {
mapping::MapById<mapping::NodeId, mapping::TrajectoryNodePose>()); mapping::MapById<mapping::NodeId, mapping::TrajectoryNodePose>());
MOCK_METHOD0(GetLandmarkPoses, std::map<std::string, transform::Rigid3d>()); MOCK_METHOD0(GetLandmarkPoses, std::map<std::string, transform::Rigid3d>());
MOCK_METHOD1(IsTrajectoryFinished, bool(int)); MOCK_METHOD1(IsTrajectoryFinished, bool(int));
MOCK_METHOD1(IsTrajectoryFrozen, bool(int));
MOCK_METHOD0(GetTrajectoryData, MOCK_METHOD0(GetTrajectoryData,
std::map<int, mapping::PoseGraphInterface::TrajectoryData>()); std::map<int, mapping::PoseGraphInterface::TrajectoryData>());
MOCK_METHOD0(constraints, std::vector<Constraint>()); MOCK_METHOD0(constraints, std::vector<Constraint>());

3
cartographer/mapping/pose_graph_interface.h
View File

@ -111,6 +111,9 @@ class PoseGraphInterface {
// Checks if the given trajectory is finished. // Checks if the given trajectory is finished.
virtual bool IsTrajectoryFinished(int trajectory_id) = 0; virtual bool IsTrajectoryFinished(int trajectory_id) = 0;
// Checks if the given trajectory is frozen.
virtual bool IsTrajectoryFrozen(int trajectory_id) = 0;
// Returns the trajectory data. // Returns the trajectory data.
virtual std::map<int, TrajectoryData> GetTrajectoryData() = 0; virtual std::map<int, TrajectoryData> GetTrajectoryData() = 0;