gaschler 2018-06-19 11:48:09 +02:00 committed by GitHub
parent 5d26742bfa
commit a9045fa375
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
16 changed files with 114 additions and 24 deletions

View File

@ -105,6 +105,11 @@ PoseGraphStub::GetTrajectoryNodePoses() const {
return node_poses; return node_poses;
} }
std::map<int, mapping::PoseGraphInterface::TrajectoryState>
PoseGraphStub::GetTrajectoryStates() const {
LOG(FATAL) << "not implemented";
}
std::map<std::string, transform::Rigid3d> PoseGraphStub::GetLandmarkPoses() std::map<std::string, transform::Rigid3d> PoseGraphStub::GetLandmarkPoses()
const { const {
google::protobuf::Empty request; google::protobuf::Empty request;

View File

@ -41,6 +41,7 @@ class PoseGraphStub : public ::cartographer::mapping::PoseGraphInterface {
GetTrajectoryNodes() const override; GetTrajectoryNodes() const override;
mapping::MapById<mapping::NodeId, mapping::TrajectoryNodePose> mapping::MapById<mapping::NodeId, mapping::TrajectoryNodePose>
GetTrajectoryNodePoses() const override; GetTrajectoryNodePoses() const override;
std::map<int, TrajectoryState> GetTrajectoryStates() const override;
std::map<std::string, transform::Rigid3d> GetLandmarkPoses() const override; std::map<std::string, transform::Rigid3d> GetLandmarkPoses() const override;
void SetLandmarkPose(const std::string& landmark_id, void SetLandmarkPose(const std::string& landmark_id,
const transform::Rigid3d& global_pose) override; const transform::Rigid3d& global_pose) override;

View File

@ -147,6 +147,11 @@ void PoseGraph2D::AddWorkItem(const std::function<void()>& work_item) {
} }
void PoseGraph2D::AddTrajectoryIfNeeded(const int trajectory_id) { void PoseGraph2D::AddTrajectoryIfNeeded(const int trajectory_id) {
data_.trajectories_state[trajectory_id];
CHECK(data_.trajectories_state.at(trajectory_id).state !=
TrajectoryState::FINISHED);
CHECK(data_.trajectories_state.at(trajectory_id).state !=
TrajectoryState::DELETED);
data_.trajectory_connectivity_state.Add(trajectory_id); data_.trajectory_connectivity_state.Add(trajectory_id);
// Make sure we have a sampler for this trajectory. // Make sure we have a sampler for this trajectory.
if (!global_localization_samplers_[trajectory_id]) { if (!global_localization_samplers_[trajectory_id]) {
@ -436,8 +441,8 @@ void PoseGraph2D::WaitForAllComputations() {
void PoseGraph2D::FinishTrajectory(const int trajectory_id) { void PoseGraph2D::FinishTrajectory(const int trajectory_id) {
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
AddWorkItem([this, trajectory_id]() REQUIRES(mutex_) { AddWorkItem([this, trajectory_id]() REQUIRES(mutex_) {
CHECK_EQ(data_.finished_trajectories.count(trajectory_id), 0); CHECK(!IsTrajectoryFinished(trajectory_id));
data_.finished_trajectories.insert(trajectory_id); data_.trajectories_state[trajectory_id].state = TrajectoryState::FINISHED;
for (const auto& submap : data_.submap_data.trajectory(trajectory_id)) { for (const auto& submap : data_.submap_data.trajectory(trajectory_id)) {
data_.submap_data.at(submap.id).state = SubmapState::kFinished; data_.submap_data.at(submap.id).state = SubmapState::kFinished;
@ -448,20 +453,24 @@ void PoseGraph2D::FinishTrajectory(const int trajectory_id) {
} }
bool PoseGraph2D::IsTrajectoryFinished(const int trajectory_id) const { bool PoseGraph2D::IsTrajectoryFinished(const int trajectory_id) const {
return data_.finished_trajectories.count(trajectory_id) > 0; return data_.trajectories_state.count(trajectory_id) != 0 &&
data_.trajectories_state.at(trajectory_id).state ==
TrajectoryState::FINISHED;
} }
void PoseGraph2D::FreezeTrajectory(const int trajectory_id) { void PoseGraph2D::FreezeTrajectory(const int trajectory_id) {
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
data_.trajectory_connectivity_state.Add(trajectory_id); data_.trajectory_connectivity_state.Add(trajectory_id);
AddWorkItem([this, trajectory_id]() REQUIRES(mutex_) { AddWorkItem([this, trajectory_id]() REQUIRES(mutex_) {
CHECK_EQ(data_.frozen_trajectories.count(trajectory_id), 0); CHECK(!IsTrajectoryFrozen(trajectory_id));
data_.frozen_trajectories.insert(trajectory_id); data_.trajectories_state[trajectory_id].state = TrajectoryState::FROZEN;
}); });
} }
bool PoseGraph2D::IsTrajectoryFrozen(const int trajectory_id) const { bool PoseGraph2D::IsTrajectoryFrozen(const int trajectory_id) const {
return data_.frozen_trajectories.count(trajectory_id) > 0; return data_.trajectories_state.count(trajectory_id) != 0 &&
data_.trajectories_state.at(trajectory_id).state ==
TrajectoryState::FROZEN;
} }
void PoseGraph2D::AddSubmapFromProto( void PoseGraph2D::AddSubmapFromProto(
@ -599,7 +608,7 @@ void PoseGraph2D::RunOptimization() {
// data_.constraints, data_.frozen_trajectories and data_.landmark_nodes // data_.constraints, data_.frozen_trajectories and data_.landmark_nodes
// when executing the Solve. Solve is time consuming, so not taking the mutex // when executing the Solve. Solve is time consuming, so not taking the mutex
// before Solve to avoid blocking foreground processing. // before Solve to avoid blocking foreground processing.
optimization_problem_->Solve(data_.constraints, data_.frozen_trajectories, optimization_problem_->Solve(data_.constraints, GetTrajectoryStates(),
data_.landmark_nodes); data_.landmark_nodes);
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
@ -663,6 +672,16 @@ MapById<NodeId, TrajectoryNodePose> PoseGraph2D::GetTrajectoryNodePoses()
return node_poses; return node_poses;
} }
std::map<int, PoseGraphInterface::TrajectoryState>
PoseGraph2D::GetTrajectoryStates() const {
std::map<int, PoseGraphInterface::TrajectoryState> trajectories_state;
common::MutexLocker locker(&mutex_);
for (const auto& it : data_.trajectories_state) {
trajectories_state[it.first] = it.second.state;
}
return trajectories_state;
}
std::map<std::string, transform::Rigid3d> PoseGraph2D::GetLandmarkPoses() std::map<std::string, transform::Rigid3d> PoseGraph2D::GetLandmarkPoses()
const { const {
std::map<std::string, transform::Rigid3d> landmark_poses; std::map<std::string, transform::Rigid3d> landmark_poses;

View File

@ -121,6 +121,8 @@ class PoseGraph2D : public PoseGraph {
EXCLUDES(mutex_); EXCLUDES(mutex_);
MapById<NodeId, TrajectoryNodePose> GetTrajectoryNodePoses() const override MapById<NodeId, TrajectoryNodePose> GetTrajectoryNodePoses() const override
EXCLUDES(mutex_); EXCLUDES(mutex_);
std::map<int, TrajectoryState> GetTrajectoryStates() const override
EXCLUDES(mutex_);
std::map<std::string, transform::Rigid3d> GetLandmarkPoses() const override std::map<std::string, transform::Rigid3d> GetLandmarkPoses() const override
EXCLUDES(mutex_); EXCLUDES(mutex_);
void SetLandmarkPose(const std::string& landmark_id, void SetLandmarkPose(const std::string& landmark_id,

View File

@ -145,6 +145,11 @@ void PoseGraph3D::AddWorkItem(const std::function<void()>& work_item) {
} }
void PoseGraph3D::AddTrajectoryIfNeeded(const int trajectory_id) { void PoseGraph3D::AddTrajectoryIfNeeded(const int trajectory_id) {
data_.trajectories_state[trajectory_id];
CHECK(data_.trajectories_state.at(trajectory_id).state !=
TrajectoryState::FINISHED);
CHECK(data_.trajectories_state.at(trajectory_id).state !=
TrajectoryState::DELETED);
data_.trajectory_connectivity_state.Add(trajectory_id); data_.trajectory_connectivity_state.Add(trajectory_id);
// Make sure we have a sampler for this trajectory. // Make sure we have a sampler for this trajectory.
if (!global_localization_samplers_[trajectory_id]) { if (!global_localization_samplers_[trajectory_id]) {
@ -452,8 +457,8 @@ void PoseGraph3D::WaitForAllComputations() {
void PoseGraph3D::FinishTrajectory(const int trajectory_id) { void PoseGraph3D::FinishTrajectory(const int trajectory_id) {
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
AddWorkItem([this, trajectory_id]() REQUIRES(mutex_) { AddWorkItem([this, trajectory_id]() REQUIRES(mutex_) {
CHECK_EQ(data_.finished_trajectories.count(trajectory_id), 0); CHECK(!IsTrajectoryFinished(trajectory_id));
data_.finished_trajectories.insert(trajectory_id); data_.trajectories_state[trajectory_id].state = TrajectoryState::FINISHED;
for (const auto& submap : data_.submap_data.trajectory(trajectory_id)) { for (const auto& submap : data_.submap_data.trajectory(trajectory_id)) {
data_.submap_data.at(submap.id).state = SubmapState::kFinished; data_.submap_data.at(submap.id).state = SubmapState::kFinished;
@ -464,20 +469,24 @@ void PoseGraph3D::FinishTrajectory(const int trajectory_id) {
} }
bool PoseGraph3D::IsTrajectoryFinished(const int trajectory_id) const { bool PoseGraph3D::IsTrajectoryFinished(const int trajectory_id) const {
return data_.finished_trajectories.count(trajectory_id) > 0; return data_.trajectories_state.count(trajectory_id) != 0 &&
data_.trajectories_state.at(trajectory_id).state ==
TrajectoryState::FINISHED;
} }
void PoseGraph3D::FreezeTrajectory(const int trajectory_id) { void PoseGraph3D::FreezeTrajectory(const int trajectory_id) {
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
data_.trajectory_connectivity_state.Add(trajectory_id); data_.trajectory_connectivity_state.Add(trajectory_id);
AddWorkItem([this, trajectory_id]() REQUIRES(mutex_) { AddWorkItem([this, trajectory_id]() REQUIRES(mutex_) {
CHECK_EQ(data_.frozen_trajectories.count(trajectory_id), 0); CHECK(!IsTrajectoryFrozen(trajectory_id));
data_.frozen_trajectories.insert(trajectory_id); data_.trajectories_state[trajectory_id].state = TrajectoryState::FROZEN;
}); });
} }
bool PoseGraph3D::IsTrajectoryFrozen(const int trajectory_id) const { bool PoseGraph3D::IsTrajectoryFrozen(const int trajectory_id) const {
return data_.frozen_trajectories.count(trajectory_id) > 0; return data_.trajectories_state.count(trajectory_id) != 0 &&
data_.trajectories_state.at(trajectory_id).state ==
TrajectoryState::FROZEN;
} }
void PoseGraph3D::AddSubmapFromProto( void PoseGraph3D::AddSubmapFromProto(
@ -640,7 +649,7 @@ void PoseGraph3D::RunOptimization() {
// data_.frozen_trajectories and data_.landmark_nodes when executing the // data_.frozen_trajectories and data_.landmark_nodes when executing the
// Solve. Solve is time consuming, so not taking the mutex before Solve to // Solve. Solve is time consuming, so not taking the mutex before Solve to
// avoid blocking foreground processing. // avoid blocking foreground processing.
optimization_problem_->Solve(data_.constraints, data_.frozen_trajectories, optimization_problem_->Solve(data_.constraints, GetTrajectoryStates(),
data_.landmark_nodes); data_.landmark_nodes);
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
@ -705,6 +714,16 @@ MapById<NodeId, TrajectoryNodePose> PoseGraph3D::GetTrajectoryNodePoses()
return node_poses; return node_poses;
} }
std::map<int, PoseGraphInterface::TrajectoryState>
PoseGraph3D::GetTrajectoryStates() const {
std::map<int, PoseGraphInterface::TrajectoryState> trajectories_state;
common::MutexLocker locker(&mutex_);
for (const auto& it : data_.trajectories_state) {
trajectories_state[it.first] = it.second.state;
}
return trajectories_state;
}
std::map<std::string, transform::Rigid3d> PoseGraph3D::GetLandmarkPoses() std::map<std::string, transform::Rigid3d> PoseGraph3D::GetLandmarkPoses()
const { const {
std::map<std::string, transform::Rigid3d> landmark_poses; std::map<std::string, transform::Rigid3d> landmark_poses;

View File

@ -120,6 +120,8 @@ class PoseGraph3D : public PoseGraph {
EXCLUDES(mutex_); EXCLUDES(mutex_);
MapById<NodeId, TrajectoryNodePose> GetTrajectoryNodePoses() const override MapById<NodeId, TrajectoryNodePose> GetTrajectoryNodePoses() const override
EXCLUDES(mutex_); EXCLUDES(mutex_);
std::map<int, TrajectoryState> GetTrajectoryStates() const override
EXCLUDES(mutex_);
std::map<std::string, transform::Rigid3d> GetLandmarkPoses() const override std::map<std::string, transform::Rigid3d> GetLandmarkPoses() const override
EXCLUDES(mutex_); EXCLUDES(mutex_);
void SetLandmarkPose(const std::string& landmark_id, void SetLandmarkPose(const std::string& landmark_id,

View File

@ -39,7 +39,8 @@ class MockOptimizationProblem3D : public OptimizationProblem3D {
~MockOptimizationProblem3D() override = default; ~MockOptimizationProblem3D() override = default;
MOCK_METHOD3(Solve, MOCK_METHOD3(Solve,
void(const std::vector<Constraint> &, const std::set<int> &, void(const std::vector<Constraint> &,
const std::map<int, PoseGraphInterface::TrajectoryState> &,
const std::map<std::string, LandmarkNode> &)); const std::map<std::string, LandmarkNode> &));
}; };

View File

@ -195,13 +195,21 @@ void OptimizationProblem2D::SetMaxNumIterations(
void OptimizationProblem2D::Solve( void OptimizationProblem2D::Solve(
const std::vector<Constraint>& constraints, const std::vector<Constraint>& constraints,
const std::set<int>& frozen_trajectories, const std::map<int, PoseGraphInterface::TrajectoryState>&
trajectories_state,
const std::map<std::string, LandmarkNode>& landmark_nodes) { const std::map<std::string, LandmarkNode>& landmark_nodes) {
if (node_data_.empty()) { if (node_data_.empty()) {
// Nothing to optimize. // Nothing to optimize.
return; return;
} }
std::set<int> frozen_trajectories;
for (const auto& it : trajectories_state) {
if (it.second == PoseGraphInterface::TrajectoryState::FROZEN) {
frozen_trajectories.insert(it.first);
}
}
ceres::Problem::Options problem_options; ceres::Problem::Options problem_options;
ceres::Problem problem(problem_options); ceres::Problem problem(problem_options);

View File

@ -79,7 +79,8 @@ class OptimizationProblem2D
void Solve( void Solve(
const std::vector<Constraint>& constraints, const std::vector<Constraint>& constraints,
const std::set<int>& frozen_trajectories, const std::map<int, PoseGraphInterface::TrajectoryState>&
trajectories_state,
const std::map<std::string, LandmarkNode>& landmark_nodes) override; const std::map<std::string, LandmarkNode>& landmark_nodes) override;
const MapById<NodeId, NodeSpec2D>& node_data() const override { const MapById<NodeId, NodeSpec2D>& node_data() const override {

View File

@ -258,13 +258,21 @@ void OptimizationProblem3D::SetMaxNumIterations(
void OptimizationProblem3D::Solve( void OptimizationProblem3D::Solve(
const std::vector<Constraint>& constraints, const std::vector<Constraint>& constraints,
const std::set<int>& frozen_trajectories, const std::map<int, PoseGraphInterface::TrajectoryState>&
trajectories_state,
const std::map<std::string, LandmarkNode>& landmark_nodes) { const std::map<std::string, LandmarkNode>& landmark_nodes) {
if (node_data_.empty()) { if (node_data_.empty()) {
// Nothing to optimize. // Nothing to optimize.
return; return;
} }
std::set<int> frozen_trajectories;
for (const auto& it : trajectories_state) {
if (it.second == PoseGraphInterface::TrajectoryState::FROZEN) {
frozen_trajectories.insert(it.first);
}
}
ceres::Problem::Options problem_options; ceres::Problem::Options problem_options;
ceres::Problem problem(problem_options); ceres::Problem problem(problem_options);

View File

@ -79,7 +79,8 @@ class OptimizationProblem3D
void Solve( void Solve(
const std::vector<Constraint>& constraints, const std::vector<Constraint>& constraints,
const std::set<int>& frozen_trajectories, const std::map<int, PoseGraphInterface::TrajectoryState>&
trajectories_state,
const std::map<std::string, LandmarkNode>& landmark_nodes) override; const std::map<std::string, LandmarkNode>& landmark_nodes) override;
const MapById<NodeId, NodeSpec3D>& node_data() const override { const MapById<NodeId, NodeSpec3D>& node_data() const override {

View File

@ -171,8 +171,9 @@ TEST_F(OptimizationProblem3DTest, ReducesNoise) {
optimization_problem_.AddSubmap(kTrajectoryId, kSubmap0Transform); optimization_problem_.AddSubmap(kTrajectoryId, kSubmap0Transform);
optimization_problem_.AddSubmap(kTrajectoryId, kSubmap0Transform); optimization_problem_.AddSubmap(kTrajectoryId, kSubmap0Transform);
optimization_problem_.AddSubmap(kTrajectoryId, kSubmap2Transform); optimization_problem_.AddSubmap(kTrajectoryId, kSubmap2Transform);
const std::set<int> kFrozen = {}; const std::map<int, PoseGraphInterface::TrajectoryState> kTrajectoriesState =
optimization_problem_.Solve(constraints, kFrozen, {}); {{kTrajectoryId, PoseGraphInterface::TrajectoryState::ACTIVE}};
optimization_problem_.Solve(constraints, kTrajectoriesState, {});
double translation_error_after = 0.; double translation_error_after = 0.;
double rotation_error_after = 0.; double rotation_error_after = 0.;

View File

@ -69,7 +69,8 @@ class OptimizationProblemInterface {
// Optimizes the global poses. // Optimizes the global poses.
virtual void Solve( virtual void Solve(
const std::vector<Constraint>& constraints, const std::vector<Constraint>& constraints,
const std::set<int>& frozen_trajectories, const std::map<int, PoseGraphInterface::TrajectoryState>&
trajectories_state,
const std::map<std::string, LandmarkNode>& landmark_nodes) = 0; const std::map<std::string, LandmarkNode>& landmark_nodes) = 0;
virtual const MapById<NodeId, NodeDataType>& node_data() const = 0; virtual const MapById<NodeId, NodeDataType>& node_data() const = 0;

View File

@ -43,6 +43,9 @@ class MockPoseGraph : public mapping::PoseGraphInterface {
MOCK_CONST_METHOD0( MOCK_CONST_METHOD0(
GetTrajectoryNodePoses, GetTrajectoryNodePoses,
mapping::MapById<mapping::NodeId, mapping::TrajectoryNodePose>()); mapping::MapById<mapping::NodeId, mapping::TrajectoryNodePose>());
MOCK_CONST_METHOD0(
GetTrajectoryStates,
std::map<int, mapping::PoseGraphInterface::TrajectoryState>());
MOCK_CONST_METHOD0(GetLandmarkPoses, MOCK_CONST_METHOD0(GetLandmarkPoses,
std::map<std::string, transform::Rigid3d>()); std::map<std::string, transform::Rigid3d>());
MOCK_METHOD2(SetLandmarkPose, MOCK_METHOD2(SetLandmarkPose,

View File

@ -35,6 +35,20 @@ namespace mapping {
// transitions to kFinished, all nodes are tried to match against this submap. // transitions to kFinished, all nodes are tried to match against this submap.
// Likewise, all new nodes are matched against submaps which are finished. // Likewise, all new nodes are matched against submaps which are finished.
enum class SubmapState { kActive, kFinished }; enum class SubmapState { kActive, kFinished };
struct InternalTrajectoryState {
// TODO(gaschler): Implement PoseGraphInterface::DeleteTrajectory.
enum class DeletionState {
NORMAL,
SCHEDULED_FOR_DELETION,
WAIT_FOR_DELETION
};
PoseGraphInterface::TrajectoryState state =
PoseGraphInterface::TrajectoryState::ACTIVE;
DeletionState deletion_state = DeletionState::NORMAL;
};
struct InternalSubmapData { struct InternalSubmapData {
std::shared_ptr<const Submap> submap; std::shared_ptr<const Submap> submap;
SubmapState state = SubmapState::kActive; SubmapState state = SubmapState::kActive;
@ -64,8 +78,7 @@ struct PoseGraphData {
// How our various trajectories are related. // How our various trajectories are related.
TrajectoryConnectivityState trajectory_connectivity_state; TrajectoryConnectivityState trajectory_connectivity_state;
int num_trajectory_nodes = 0; int num_trajectory_nodes = 0;
std::set<int> finished_trajectories; std::map<int, InternalTrajectoryState> trajectories_state;
std::set<int> frozen_trajectories;
// Set of all initial trajectory poses. // Set of all initial trajectory poses.
std::map<int, PoseGraph::InitialTrajectoryPose> initial_trajectory_poses; std::map<int, PoseGraph::InitialTrajectoryPose> initial_trajectory_poses;

View File

@ -79,6 +79,8 @@ class PoseGraphInterface {
common::optional<transform::Rigid3d> fixed_frame_origin_in_map; common::optional<transform::Rigid3d> fixed_frame_origin_in_map;
}; };
enum class TrajectoryState { ACTIVE, FINISHED, FROZEN, DELETED };
using GlobalSlamOptimizationCallback = using GlobalSlamOptimizationCallback =
std::function<void(const std::map<int /* trajectory_id */, SubmapId>&, std::function<void(const std::map<int /* trajectory_id */, SubmapId>&,
const std::map<int /* trajectory_id */, NodeId>&)>; const std::map<int /* trajectory_id */, NodeId>&)>;
@ -111,6 +113,9 @@ class PoseGraphInterface {
virtual MapById<NodeId, TrajectoryNodePose> GetTrajectoryNodePoses() virtual MapById<NodeId, TrajectoryNodePose> GetTrajectoryNodePoses()
const = 0; const = 0;
// Returns the states of trajectories.
virtual std::map<int, TrajectoryState> GetTrajectoryStates() const = 0;
// Returns the current optimized landmark poses. // Returns the current optimized landmark poses.
virtual std::map<std::string, transform::Rigid3d> GetLandmarkPoses() virtual std::map<std::string, transform::Rigid3d> GetLandmarkPoses()
const = 0; const = 0;