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

Landmark improvements (#901)

master
Juraj Oršulić 2018-02-14 15:39:43 +01:00 committed by Alexander Belyaev
parent cf01184114
commit fb631ac9e6
4 changed files with 44 additions and 35 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
cartographer/mapping_2d/pose_graph.cc
View File

@ -176,16 +176,15 @@ void PoseGraph::AddLandmarkData(int trajectory_id,
const sensor::LandmarkData& landmark_data)
EXCLUDES(mutex_) {
common::MutexLocker locker(&mutex_);
for (const auto& observation : landmark_data.landmark_observations) {
landmark_nodes_[observation.id].landmark_observations.emplace_back(
PoseGraph::LandmarkNode::LandmarkObservation{
trajectory_id,
landmark_data.time,
observation.landmark_to_tracking_transform,
observation.translation_weight,
observation.rotation_weight,
});
}
AddWorkItem([=]() REQUIRES(mutex_) {
for (const auto& observation : landmark_data.landmark_observations) {
landmark_nodes_[observation.id].landmark_observations.emplace_back(
PoseGraph::LandmarkNode::LandmarkObservation{
trajectory_id, landmark_data.time,
observation.landmark_to_tracking_transform,
observation.translation_weight, observation.rotation_weight});
}
});
}
void PoseGraph::ComputeConstraint(const mapping::NodeId& node_id,
@ -546,10 +545,10 @@ void PoseGraph::RunOptimization() {
return;
}
// No other thread is accessing the optimization_problem_, constraints_ and
// frozen_trajectories_ when executing the Solve. Solve is time consuming,
// so not taking the mutex before Solve to avoid blocking foreground
// processing.
// No other thread is accessing the optimization_problem_, constraints_,
// frozen_trajectories_ and landmark_nodes_ when executing the Solve. Solve
// is time consuming, so not taking the mutex before Solve to avoid blocking
// foreground processing.
optimization_problem_.Solve(constraints_, frozen_trajectories_,
landmark_nodes_);
common::MutexLocker locker(&mutex_);

13
cartographer/mapping_2d/pose_graph/optimization_problem.cc
View File

@ -79,18 +79,23 @@ void AddLandmarkCostFunctions(
for (const auto& landmark_node : landmark_nodes) {
for (const auto& observation : landmark_node.second.landmark_observations) {
const std::string& landmark_id = landmark_node.first;
const auto& begin_of_trajectory =
node_data.BeginOfTrajectory(observation.trajectory_id);
// The landmark observation was made before the trajectory was created.
if (observation.time < begin_of_trajectory->data.time) {
continue;
}
// Find the trajectory nodes before and after the landmark observation.
auto next =
node_data.lower_bound(observation.trajectory_id, observation.time);
// The landmark observation was made before the trajectory was created.
if (next == node_data.BeginOfTrajectory(observation.trajectory_id)) {
continue;
}
// The landmark observation was made, but the next trajectory node has
// not been added yet.
if (next == node_data.EndOfTrajectory(observation.trajectory_id)) {
continue;
}
if (next == begin_of_trajectory) {
next = std::next(next);
}
auto prev = std::prev(next);
// Add parameter blocks for the landmark ID if they were not added before.
if (!C_landmarks->count(landmark_id)) {

26
cartographer/mapping_3d/pose_graph.cc
View File

@ -178,16 +178,15 @@ void PoseGraph::AddLandmarkData(int trajectory_id,
const sensor::LandmarkData& landmark_data)
EXCLUDES(mutex_) {
common::MutexLocker locker(&mutex_);
for (const auto& observation : landmark_data.landmark_observations) {
landmark_nodes_[observation.id].landmark_observations.emplace_back(
PoseGraph::LandmarkNode::LandmarkObservation{
trajectory_id,
landmark_data.time,
observation.landmark_to_tracking_transform,
observation.translation_weight,
observation.rotation_weight,
});
}
AddWorkItem([=]() REQUIRES(mutex_) {
for (const auto& observation : landmark_data.landmark_observations) {
landmark_nodes_[observation.id].landmark_observations.emplace_back(
PoseGraph::LandmarkNode::LandmarkObservation{
trajectory_id, landmark_data.time,
observation.landmark_to_tracking_transform,
observation.translation_weight, observation.rotation_weight});
}
});
}
void PoseGraph::ComputeConstraint(const mapping::NodeId& node_id,
@ -575,9 +574,10 @@ void PoseGraph::RunOptimization() {
return;
}
// No other thread is accessing the optimization_problem_, constraints_ and
// frozen_trajectories_ when executing the Solve. Solve is time consuming, so
// not taking the mutex before Solve to avoid blocking foreground processing.
// No other thread is accessing the optimization_problem_, constraints_,
// frozen_trajectories_ and landmark_nodes_ when executing the Solve. Solve
// is time consuming, so not taking the mutex before Solve to avoid blocking
// foreground processing.
optimization_problem_.Solve(constraints_, frozen_trajectories_,
landmark_nodes_);
common::MutexLocker locker(&mutex_);

13
cartographer/mapping_3d/pose_graph/optimization_problem.cc
View File

@ -120,18 +120,23 @@ void AddLandmarkCostFunctions(
for (const auto& landmark_node : landmark_nodes) {
for (const auto& observation : landmark_node.second.landmark_observations) {
const std::string& landmark_id = landmark_node.first;
const auto& begin_of_trajectory =
node_data.BeginOfTrajectory(observation.trajectory_id);
// The landmark observation was made before the trajectory was created.
if (observation.time < begin_of_trajectory->data.time) {
continue;
}
// Find the trajectory nodes before and after the landmark observation.
auto next =
node_data.lower_bound(observation.trajectory_id, observation.time);
// The landmark observation was made before the trajectory was created.
if (next == node_data.BeginOfTrajectory(observation.trajectory_id)) {
continue;
}
// The landmark observation was made, but the next trajectory node has
// not been added yet.
if (next == node_data.EndOfTrajectory(observation.trajectory_id)) {
continue;
}
if (next == begin_of_trajectory) {
next = std::next(next);
}
auto prev = std::prev(next);
// Add parameter blocks for the landmark ID if they were not added before.
if (!C_landmarks->count(landmark_id)) {