diff --git a/cartographer/pose_graph/pose_graph_controller.cc b/cartographer/pose_graph/pose_graph_controller.cc index bdd56d9..5a7aa2c 100644 --- a/cartographer/pose_graph/pose_graph_controller.cc +++ b/cartographer/pose_graph/pose_graph_controller.cc @@ -19,6 +19,16 @@ namespace cartographer { namespace pose_graph { +void PoseGraphController::AddData(const proto::PoseGraphData& data) { + absl::MutexLock locker(&mutex_); + for (const auto& node : data.nodes()) { + AddNodeToPoseGraphData(node, &data_); + } + for (const auto& constraint : data.constraints()) { + AddConstraintToPoseGraphData(constraint, &data_); + } +} + void PoseGraphController::AddNode(const proto::Node& node) { absl::MutexLock locker(&mutex_); AddNodeToPoseGraphData(node, &data_); diff --git a/cartographer/pose_graph/pose_graph_controller.h b/cartographer/pose_graph/pose_graph_controller.h index e72cc31..3c915ed 100644 --- a/cartographer/pose_graph/pose_graph_controller.h +++ b/cartographer/pose_graph/pose_graph_controller.h @@ -19,6 +19,7 @@ #include "absl/synchronization/mutex.h" #include "cartographer/pose_graph/pose_graph_data.h" +#include "cartographer/pose_graph/proto/pose_graph_data.pb.h" #include "cartographer/pose_graph/solver/solver.h" namespace cartographer { @@ -32,6 +33,7 @@ class PoseGraphController { PoseGraphController(const PoseGraphController&) = delete; PoseGraphController& operator=(const PoseGraphController&) = delete; + void AddData(const proto::PoseGraphData& data) LOCKS_EXCLUDED(mutex_); void AddNode(const proto::Node& node) LOCKS_EXCLUDED(mutex_); void AddConstraint(const proto::Constraint& constraint) LOCKS_EXCLUDED(mutex_);