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

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
cartographer/cloud/internal/client/pose_graph_stub.cc
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() =

3
cartographer/cloud/internal/client/pose_graph_stub.h
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;

3
cartographer/cloud/internal/handlers/set_landmark_pose_handler_test.cc
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);
}

4
cartographer/mapping/internal/2d/pose_graph_2d.cc
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;
});
}

3
cartographer/mapping/internal/2d/pose_graph_2d.h
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_);

4
cartographer/mapping/internal/3d/pose_graph_3d.cc
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;
});
}

3
cartographer/mapping/internal/3d/pose_graph_3d.h
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_);

16
cartographer/mapping/internal/optimization/optimization_problem_2d.cc
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();) {

15
cartographer/mapping/internal/optimization/optimization_problem_3d.cc
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()) {

4
cartographer/mapping/internal/testing/mock_pose_graph.h
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));

3
cartographer/mapping/map_builder.cc
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;

4
cartographer/mapping/pose_graph_interface.h
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;