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

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 #1470
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,
const transform::Rigid3d& global_pose) {
const transform::Rigid3d& global_pose,
const bool frozen) {
proto::SetLandmarkPoseRequest request;
request.mutable_landmark_pose()->set_landmark_id(landmark_id);
*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<std::string, transform::Rigid3d> GetLandmarkPoses() const override;
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;
bool IsTrajectoryFinished(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::Rigid3d(Eigen::Vector3d(1, 2, 3),
Eigen::Quaterniond(4, 5, 6, 7)),
kEps)));
kEps),
false));
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,
const transform::Rigid3d& global_pose) {
const transform::Rigid3d& global_pose,
const bool frozen) {
AddWorkItem([=]() LOCKS_EXCLUDED(mutex_) {
absl::MutexLock locker(&mutex_);
data_.landmark_nodes[landmark_id].global_landmark_pose = global_pose;
data_.landmark_nodes[landmark_id].frozen = frozen;
return WorkItem::Result::kDoNotRunOptimization;
});
}

View File

@ -133,7 +133,8 @@ class PoseGraph2D : public PoseGraph {
std::map<std::string, transform::Rigid3d> GetLandmarkPoses() const override
LOCKS_EXCLUDED(mutex_);
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_);
sensor::MapByTime<sensor::ImuData> GetImuData() const override
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,
const transform::Rigid3d& global_pose) {
const transform::Rigid3d& global_pose,
const bool frozen) {
AddWorkItem([=]() LOCKS_EXCLUDED(mutex_) {
absl::MutexLock locker(&mutex_);
data_.landmark_nodes[landmark_id].global_landmark_pose = global_pose;
data_.landmark_nodes[landmark_id].frozen = frozen;
return WorkItem::Result::kDoNotRunOptimization;
});
}

View File

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

View File

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

View File

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

View File

@ -48,8 +48,8 @@ class MockPoseGraph : public mapping::PoseGraphInterface {
std::map<int, mapping::PoseGraphInterface::TrajectoryState>());
MOCK_CONST_METHOD0(GetLandmarkPoses,
std::map<std::string, transform::Rigid3d>());
MOCK_METHOD2(SetLandmarkPose,
void(const std::string&, const transform::Rigid3d&));
MOCK_METHOD3(SetLandmarkPose,
void(const std::string&, const transform::Rigid3d&, const bool));
MOCK_METHOD1(DeleteTrajectory, void(int));
MOCK_CONST_METHOD1(IsTrajectoryFinished, 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.
for (const auto& landmark : pose_graph_proto.landmark_poses()) {
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;

View File

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