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

Store landmark observations as LandmarkNodes in PoseGraph. (#850)

master
Alexander Belyaev 2018-01-29 10:17:05 +01:00 committed by GitHub
parent 49d89d0759
commit 8316444319
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
5 changed files with 53 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
cartographer/mapping/pose_graph_interface.h
View File

@ -49,6 +49,18 @@ class PoseGraphInterface {
enum Tag { INTRA_SUBMAP, INTER_SUBMAP } tag;
};
struct LandmarkNode {
struct LandmarkObservation {
int trajectory_id;
common::Time time;
transform::Rigid3d landmark_to_tracking_transform;
double translation_weight;
double rotation_weight;
};
std::vector<LandmarkObservation> landmark_observations;
transform::Rigid3d global_landmark_pose;
};
struct SubmapPose {
int version;
transform::Rigid3d pose;

29
cartographer/mapping_2d/pose_graph.cc
View File

@ -175,7 +175,17 @@ void PoseGraph::AddFixedFramePoseData(
void PoseGraph::AddLandmarkData(int trajectory_id,
const sensor::LandmarkData& landmark_data)
EXCLUDES(mutex_) {
LOG(FATAL) << "Not yet implemented.";
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,
});
}
}
void PoseGraph::ComputeConstraint(const mapping::NodeId& node_id,
@ -191,10 +201,10 @@ void PoseGraph::ComputeConstraint(const mapping::NodeId& node_id,
last_connection_time +
common::FromSeconds(
options_.global_constraint_search_after_n_seconds())) {
// If the node and the submap belong to the same trajectory or if there has
// been a recent global constraint that ties that node's trajectory to the
// submap's trajectory, it suffices to do a match constrained to a local
// search window.
// If the node and the submap belong to the same trajectory or if there
// has been a recent global constraint that ties that node's trajectory to
// the submap's trajectory, it suffices to do a match constrained to a
// local search window.
const transform::Rigid2d initial_relative_pose =
optimization_problem_.submap_data()
.at(submap_id)
@ -243,8 +253,8 @@ void PoseGraph::ComputeConstraintsForNode(
constant_data->gravity_alignment);
for (size_t i = 0; i < insertion_submaps.size(); ++i) {
const mapping::SubmapId submap_id = submap_ids[i];
// Even if this was the last node added to 'submap_id', the submap will only
// be marked as finished in 'submap_data_' further below.
// Even if this was the last node added to 'submap_id', the submap will
// only be marked as finished in 'submap_data_' further below.
CHECK(submap_data_.at(submap_id).state == SubmapState::kActive);
submap_data_.at(submap_id).node_ids.emplace(node_id);
const transform::Rigid2d constraint_transform =
@ -537,8 +547,9 @@ void PoseGraph::RunOptimization() {
}
// 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.
// frozen_trajectories_ 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_);
common::MutexLocker locker(&mutex_);

5
cartographer/mapping_2d/pose_graph.h
View File

@ -39,6 +39,7 @@
#include "cartographer/mapping_2d/pose_graph/optimization_problem.h"
#include "cartographer/mapping_2d/submaps.h"
#include "cartographer/sensor/fixed_frame_pose_data.h"
#include "cartographer/sensor/landmark_data.h"
#include "cartographer/sensor/odometry_data.h"
#include "cartographer/sensor/point_cloud.h"
#include "cartographer/transform/rigid_transform.h"
@ -244,6 +245,10 @@ class PoseGraph : public mapping::PoseGraph {
mapping::MapById<mapping::SubmapId, pose_graph::SubmapData>
global_submap_poses_ GUARDED_BY(mutex_);
// Global landmark poses with all observations.
std::map<std::string /* landmark ID */, PoseGraph::LandmarkNode>
landmark_nodes_ GUARDED_BY(mutex_);
// List of all trimmers to consult when optimizations finish.
std::vector<std::unique_ptr<mapping::PoseGraphTrimmer>> trimmers_
GUARDED_BY(mutex_);

12
cartographer/mapping_3d/pose_graph.cc
View File

@ -177,7 +177,17 @@ void PoseGraph::AddFixedFramePoseData(
void PoseGraph::AddLandmarkData(int trajectory_id,
const sensor::LandmarkData& landmark_data)
EXCLUDES(mutex_) {
LOG(FATAL) << "Not yet implemented.";
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,
});
}
}
void PoseGraph::ComputeConstraint(const mapping::NodeId& node_id,

5
cartographer/mapping_3d/pose_graph.h
View File

@ -39,6 +39,7 @@
#include "cartographer/mapping_3d/pose_graph/optimization_problem.h"
#include "cartographer/mapping_3d/submaps.h"
#include "cartographer/sensor/fixed_frame_pose_data.h"
#include "cartographer/sensor/landmark_data.h"
#include "cartographer/sensor/odometry_data.h"
#include "cartographer/sensor/point_cloud.h"
#include "cartographer/transform/rigid_transform.h"
@ -248,6 +249,10 @@ class PoseGraph : public mapping::PoseGraph {
mapping::MapById<mapping::SubmapId, pose_graph::SubmapData>
global_submap_poses_ GUARDED_BY(mutex_);
// Global landmark poses with all observations.
std::map<std::string /* landmark ID */, PoseGraph::LandmarkNode>
landmark_nodes_ GUARDED_BY(mutex_);
// List of all trimmers to consult when optimizations finish.
std::vector<std::unique_ptr<mapping::PoseGraphTrimmer>> trimmers_
GUARDED_BY(mutex_);