Allow introduction of new landmarks into loaded (frozen) map ()

In the current implementation, as soon as one trajectory is frozen it is not possible to use new landmarks anymore.
In this PR a suggest a change, that allows introducing new landmarks also in the presence of a frozen trajectory. This is done with the following steps:
- Store set of frozen landmarks
- Landmarks that are loaded from a pbstream, with the flag ` load_frozen_state=true` are inserted into the set of frozen landmarks via `setLandmarkPose` function
- frozen landmarks are set constant in the optimization problem
- new landmarks can be introduced and will be optimized in the optimization problem

Motivated by and closes 
master
Jonathan Huber 2019-01-17 16:57:30 +01:00 committed by Wally B. Feed
parent eec7af0a4f
commit fd67975761
12 changed files with 33 additions and 32 deletions

View File

@ -138,7 +138,8 @@ std::map<std::string, transform::Rigid3d> PoseGraphStub::GetLandmarkPoses()
} }
void PoseGraphStub::SetLandmarkPose(const std::string& landmark_id, void PoseGraphStub::SetLandmarkPose(const std::string& landmark_id,
const transform::Rigid3d& global_pose) { const transform::Rigid3d& global_pose,
const bool frozen) {
proto::SetLandmarkPoseRequest request; proto::SetLandmarkPoseRequest request;
request.mutable_landmark_pose()->set_landmark_id(landmark_id); request.mutable_landmark_pose()->set_landmark_id(landmark_id);
*request.mutable_landmark_pose()->mutable_global_pose() = *request.mutable_landmark_pose()->mutable_global_pose() =

View File

@ -45,7 +45,8 @@ class PoseGraphStub : public ::cartographer::mapping::PoseGraphInterface {
std::map<int, TrajectoryState> GetTrajectoryStates() 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,
const bool frozen = false) override;
void DeleteTrajectory(int trajectory_id) override; void DeleteTrajectory(int trajectory_id) override;
bool IsTrajectoryFinished(int trajectory_id) const override; bool IsTrajectoryFinished(int trajectory_id) const override;
bool IsTrajectoryFrozen(int trajectory_id) const override; bool IsTrajectoryFrozen(int trajectory_id) const override;

View File

@ -53,7 +53,8 @@ TEST_F(SetLandmarkPoseHandlerTest, SetLandmarkPose) {
transform::IsNearly( transform::IsNearly(
transform::Rigid3d(Eigen::Vector3d(1, 2, 3), transform::Rigid3d(Eigen::Vector3d(1, 2, 3),
Eigen::Quaterniond(4, 5, 6, 7)), Eigen::Quaterniond(4, 5, 6, 7)),
kEps))); kEps),
false));
test_server_->SendWrite(request); test_server_->SendWrite(request);
} }

View File

@ -927,10 +927,12 @@ std::map<std::string, transform::Rigid3d> PoseGraph2D::GetLandmarkPoses()
} }
void PoseGraph2D::SetLandmarkPose(const std::string& landmark_id, 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_) { AddWorkItem([=]() LOCKS_EXCLUDED(mutex_) {
absl::MutexLock locker(&mutex_); absl::MutexLock locker(&mutex_);
data_.landmark_nodes[landmark_id].global_landmark_pose = global_pose; data_.landmark_nodes[landmark_id].global_landmark_pose = global_pose;
data_.landmark_nodes[landmark_id].frozen = frozen;
return WorkItem::Result::kDoNotRunOptimization; return WorkItem::Result::kDoNotRunOptimization;
}); });
} }

View File

@ -133,7 +133,8 @@ class PoseGraph2D : public PoseGraph {
std::map<std::string, transform::Rigid3d> GetLandmarkPoses() const override std::map<std::string, transform::Rigid3d> GetLandmarkPoses() const override
LOCKS_EXCLUDED(mutex_); LOCKS_EXCLUDED(mutex_);
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,
const bool frozen = false) override
LOCKS_EXCLUDED(mutex_); LOCKS_EXCLUDED(mutex_);
sensor::MapByTime<sensor::ImuData> GetImuData() const override sensor::MapByTime<sensor::ImuData> GetImuData() const override
LOCKS_EXCLUDED(mutex_); LOCKS_EXCLUDED(mutex_);

View File

@ -948,10 +948,12 @@ std::map<std::string, transform::Rigid3d> PoseGraph3D::GetLandmarkPoses()
} }
void PoseGraph3D::SetLandmarkPose(const std::string& landmark_id, 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_) { AddWorkItem([=]() LOCKS_EXCLUDED(mutex_) {
absl::MutexLock locker(&mutex_); absl::MutexLock locker(&mutex_);
data_.landmark_nodes[landmark_id].global_landmark_pose = global_pose; data_.landmark_nodes[landmark_id].global_landmark_pose = global_pose;
data_.landmark_nodes[landmark_id].frozen = frozen;
return WorkItem::Result::kDoNotRunOptimization; return WorkItem::Result::kDoNotRunOptimization;
}); });
} }

View File

@ -131,7 +131,8 @@ class PoseGraph3D : public PoseGraph {
std::map<std::string, transform::Rigid3d> GetLandmarkPoses() const override std::map<std::string, transform::Rigid3d> GetLandmarkPoses() const override
LOCKS_EXCLUDED(mutex_); LOCKS_EXCLUDED(mutex_);
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,
const bool frozen = false) override
LOCKS_EXCLUDED(mutex_); LOCKS_EXCLUDED(mutex_);
sensor::MapByTime<sensor::ImuData> GetImuData() const override sensor::MapByTime<sensor::ImuData> GetImuData() const override
LOCKS_EXCLUDED(mutex_); LOCKS_EXCLUDED(mutex_);

View File

@ -79,16 +79,11 @@ transform::Rigid3d GetInitialLandmarkPose(
void AddLandmarkCostFunctions( void AddLandmarkCostFunctions(
const std::map<std::string, LandmarkNode>& landmark_nodes, const std::map<std::string, LandmarkNode>& landmark_nodes,
bool freeze_landmarks, const MapById<NodeId, NodeSpec2D>& node_data, const MapById<NodeId, NodeSpec2D>& node_data,
MapById<NodeId, std::array<double, 3>>* C_nodes, MapById<NodeId, std::array<double, 3>>* C_nodes,
std::map<std::string, CeresPose>* C_landmarks, ceres::Problem* problem, std::map<std::string, CeresPose>* C_landmarks, ceres::Problem* problem,
double huber_scale) { double huber_scale) {
for (const auto& landmark_node : landmark_nodes) { 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) { for (const auto& observation : landmark_node.second.landmark_observations) {
const std::string& landmark_id = landmark_node.first; const std::string& landmark_id = landmark_node.first;
const auto& begin_of_trajectory = const auto& begin_of_trajectory =
@ -123,7 +118,8 @@ void AddLandmarkCostFunctions(
CeresPose(starting_point, nullptr /* translation_parametrization */, CeresPose(starting_point, nullptr /* translation_parametrization */,
absl::make_unique<ceres::QuaternionParameterization>(), absl::make_unique<ceres::QuaternionParameterization>(),
problem)); problem));
if (freeze_landmarks) { // Set landmark constant if it is frozen.
if (landmark_node.second.frozen) {
problem->SetParameterBlockConstant( problem->SetParameterBlockConstant(
C_landmarks->at(landmark_id).translation()); C_landmarks->at(landmark_id).translation());
problem->SetParameterBlockConstant( problem->SetParameterBlockConstant(
@ -220,7 +216,6 @@ void OptimizationProblem2D::Solve(
MapById<NodeId, std::array<double, 3>> C_nodes; MapById<NodeId, std::array<double, 3>> C_nodes;
std::map<std::string, CeresPose> C_landmarks; std::map<std::string, CeresPose> C_landmarks;
bool first_submap = true; bool first_submap = true;
bool freeze_landmarks = !frozen_trajectories.empty();
for (const auto& submap_id_data : submap_data_) { for (const auto& submap_id_data : submap_data_) {
const bool frozen = const bool frozen =
frozen_trajectories.count(submap_id_data.id.trajectory_id) != 0; frozen_trajectories.count(submap_id_data.id.trajectory_id) != 0;
@ -255,9 +250,8 @@ void OptimizationProblem2D::Solve(
C_nodes.at(constraint.node_id).data()); C_nodes.at(constraint.node_id).data());
} }
// Add cost functions for landmarks. // Add cost functions for landmarks.
AddLandmarkCostFunctions(landmark_nodes, freeze_landmarks, node_data_, AddLandmarkCostFunctions(landmark_nodes, node_data_, &C_nodes, &C_landmarks,
&C_nodes, &C_landmarks, &problem, &problem, options_.huber_scale());
options_.huber_scale());
// Add penalties for violating odometry or changes between consecutive nodes // Add penalties for violating odometry or changes between consecutive nodes
// if odometry is not available. // if odometry is not available.
for (auto node_it = node_data_.begin(); node_it != node_data_.end();) { for (auto node_it = node_data_.begin(); node_it != node_data_.end();) {

View File

@ -123,16 +123,12 @@ transform::Rigid3d GetInitialLandmarkPose(
void AddLandmarkCostFunctions( void AddLandmarkCostFunctions(
const std::map<std::string, LandmarkNode>& landmark_nodes, const std::map<std::string, LandmarkNode>& landmark_nodes,
bool freeze_landmarks, const MapById<NodeId, NodeSpec3D>& node_data, const MapById<NodeId, NodeSpec3D>& node_data,
MapById<NodeId, CeresPose>* C_nodes, MapById<NodeId, CeresPose>* C_nodes,
std::map<std::string, CeresPose>* C_landmarks, ceres::Problem* problem, std::map<std::string, CeresPose>* C_landmarks, ceres::Problem* problem,
double huber_scale) { double huber_scale) {
for (const auto& landmark_node : landmark_nodes) { for (const auto& landmark_node : landmark_nodes) {
// Do not use landmarks that were not optimized for localization. // 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) { for (const auto& observation : landmark_node.second.landmark_observations) {
const std::string& landmark_id = landmark_node.first; const std::string& landmark_id = landmark_node.first;
const auto& begin_of_trajectory = const auto& begin_of_trajectory =
@ -167,7 +163,8 @@ void AddLandmarkCostFunctions(
CeresPose(starting_point, nullptr /* translation_parametrization */, CeresPose(starting_point, nullptr /* translation_parametrization */,
absl::make_unique<ceres::QuaternionParameterization>(), absl::make_unique<ceres::QuaternionParameterization>(),
problem)); problem));
if (freeze_landmarks) { // Set landmark constant if it is frozen.
if (landmark_node.second.frozen) {
problem->SetParameterBlockConstant( problem->SetParameterBlockConstant(
C_landmarks->at(landmark_id).translation()); C_landmarks->at(landmark_id).translation());
problem->SetParameterBlockConstant( problem->SetParameterBlockConstant(
@ -291,7 +288,6 @@ void OptimizationProblem3D::Solve(
MapById<NodeId, CeresPose> C_nodes; MapById<NodeId, CeresPose> C_nodes;
std::map<std::string, CeresPose> C_landmarks; std::map<std::string, CeresPose> C_landmarks;
bool first_submap = true; bool first_submap = true;
bool freeze_landmarks = !frozen_trajectories.empty();
for (const auto& submap_id_data : submap_data_) { for (const auto& submap_id_data : submap_data_) {
const bool frozen = const bool frozen =
frozen_trajectories.count(submap_id_data.id.trajectory_id) != 0; frozen_trajectories.count(submap_id_data.id.trajectory_id) != 0;
@ -351,9 +347,8 @@ void OptimizationProblem3D::Solve(
C_nodes.at(constraint.node_id).translation()); C_nodes.at(constraint.node_id).translation());
} }
// Add cost functions for landmarks. // Add cost functions for landmarks.
AddLandmarkCostFunctions(landmark_nodes, freeze_landmarks, node_data_, AddLandmarkCostFunctions(landmark_nodes, node_data_, &C_nodes, &C_landmarks,
&C_nodes, &C_landmarks, &problem, &problem, options_.huber_scale());
options_.huber_scale());
// Add constraints based on IMU observations of angular velocities and // Add constraints based on IMU observations of angular velocities and
// linear acceleration. // linear acceleration.
if (!options_.fix_z_in_3d()) { if (!options_.fix_z_in_3d()) {

View File

@ -48,8 +48,8 @@ class MockPoseGraph : public mapping::PoseGraphInterface {
std::map<int, mapping::PoseGraphInterface::TrajectoryState>()); 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_METHOD3(SetLandmarkPose,
void(const std::string&, const transform::Rigid3d&)); void(const std::string&, const transform::Rigid3d&, const bool));
MOCK_METHOD1(DeleteTrajectory, void(int)); MOCK_METHOD1(DeleteTrajectory, void(int));
MOCK_CONST_METHOD1(IsTrajectoryFinished, bool(int)); MOCK_CONST_METHOD1(IsTrajectoryFinished, bool(int));
MOCK_CONST_METHOD1(IsTrajectoryFrozen, bool(int)); MOCK_CONST_METHOD1(IsTrajectoryFrozen, bool(int));

View File

@ -279,7 +279,8 @@ std::map<int, int> MapBuilder::LoadState(
// Set global poses of landmarks. // Set global poses of landmarks.
for (const auto& landmark : pose_graph_proto.landmark_poses()) { for (const auto& landmark : pose_graph_proto.landmark_poses()) {
pose_graph_->SetLandmarkPose(landmark.landmark_id(), pose_graph_->SetLandmarkPose(landmark.landmark_id(),
transform::ToRigid3(landmark.global_pose())); transform::ToRigid3(landmark.global_pose()),
true);
} }
MapById<SubmapId, mapping::proto::Submap> submap_id_to_submap; MapById<SubmapId, mapping::proto::Submap> submap_id_to_submap;

View File

@ -62,6 +62,7 @@ class PoseGraphInterface {
}; };
std::vector<LandmarkObservation> landmark_observations; std::vector<LandmarkObservation> landmark_observations;
absl::optional<transform::Rigid3d> global_landmark_pose; absl::optional<transform::Rigid3d> global_landmark_pose;
bool frozen = false;
}; };
struct SubmapPose { struct SubmapPose {
@ -123,7 +124,8 @@ class PoseGraphInterface {
// Sets global pose of landmark 'landmark_id' to given 'global_pose'. // Sets global pose of landmark 'landmark_id' to given 'global_pose'.
virtual void SetLandmarkPose(const std::string& landmark_id, 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. // Deletes a trajectory asynchronously.
virtual void DeleteTrajectory(int trajectory_id) = 0; virtual void DeleteTrajectory(int trajectory_id) = 0;