diff --git a/cartographer/mapping/pose_graph_interface.h b/cartographer/mapping/pose_graph_interface.h index 8f0a3ce..4e56d13 100644 --- a/cartographer/mapping/pose_graph_interface.h +++ b/cartographer/mapping/pose_graph_interface.h @@ -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 landmark_observations; + transform::Rigid3d global_landmark_pose; + }; + struct SubmapPose { int version; transform::Rigid3d pose; diff --git a/cartographer/mapping_2d/pose_graph.cc b/cartographer/mapping_2d/pose_graph.cc index a61543d..44912bc 100644 --- a/cartographer/mapping_2d/pose_graph.cc +++ b/cartographer/mapping_2d/pose_graph.cc @@ -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_); diff --git a/cartographer/mapping_2d/pose_graph.h b/cartographer/mapping_2d/pose_graph.h index 6ecbbab..7ca5397 100644 --- a/cartographer/mapping_2d/pose_graph.h +++ b/cartographer/mapping_2d/pose_graph.h @@ -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 global_submap_poses_ GUARDED_BY(mutex_); + // Global landmark poses with all observations. + std::map + landmark_nodes_ GUARDED_BY(mutex_); + // List of all trimmers to consult when optimizations finish. std::vector> trimmers_ GUARDED_BY(mutex_); diff --git a/cartographer/mapping_3d/pose_graph.cc b/cartographer/mapping_3d/pose_graph.cc index e168f57..fdac718 100644 --- a/cartographer/mapping_3d/pose_graph.cc +++ b/cartographer/mapping_3d/pose_graph.cc @@ -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, diff --git a/cartographer/mapping_3d/pose_graph.h b/cartographer/mapping_3d/pose_graph.h index 175e332..b4c5e82 100644 --- a/cartographer/mapping_3d/pose_graph.h +++ b/cartographer/mapping_3d/pose_graph.h @@ -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 global_submap_poses_ GUARDED_BY(mutex_); + // Global landmark poses with all observations. + std::map + landmark_nodes_ GUARDED_BY(mutex_); + // List of all trimmers to consult when optimizations finish. std::vector> trimmers_ GUARDED_BY(mutex_);