diff --git a/cartographer/internal/mapping/global_trajectory_builder.h b/cartographer/internal/mapping/global_trajectory_builder.h index 9de6676..0e04905 100644 --- a/cartographer/internal/mapping/global_trajectory_builder.h +++ b/cartographer/internal/mapping/global_trajectory_builder.h @@ -106,6 +106,11 @@ class GlobalTrajectoryBuilder : public mapping::TrajectoryBuilderInterface { pose_graph_->AddFixedFramePoseData(trajectory_id_, fixed_frame_pose); } + void AddSensorData(const std::string& sensor_id, + const sensor::LandmarkData& landmark_data) override { + pose_graph_->AddLandmarkData(trajectory_id_, landmark_data); + } + void AddLocalSlamResultData(std::unique_ptr local_slam_result_data) override { CHECK(!local_trajectory_builder_) << "Can't add LocalSlamResultData with " diff --git a/cartographer/mapping/collated_trajectory_builder.h b/cartographer/mapping/collated_trajectory_builder.h index 407b882..de6f221 100644 --- a/cartographer/mapping/collated_trajectory_builder.h +++ b/cartographer/mapping/collated_trajectory_builder.h @@ -69,6 +69,12 @@ class CollatedTrajectoryBuilder : public TrajectoryBuilderInterface { const sensor::FixedFramePoseData& fixed_frame_pose_data) override { AddData(sensor::MakeDispatchable(sensor_id, fixed_frame_pose_data)); } + + void AddSensorData(const std::string& sensor_id, + const sensor::LandmarkData& landmark_data) override { + AddData(sensor::MakeDispatchable(sensor_id, landmark_data)); + } + void AddLocalSlamResultData(std::unique_ptr local_slam_result_data) override { AddData(std::move(local_slam_result_data)); diff --git a/cartographer/mapping/pose_graph.h b/cartographer/mapping/pose_graph.h index 24df274..4f647ac 100644 --- a/cartographer/mapping/pose_graph.h +++ b/cartographer/mapping/pose_graph.h @@ -34,6 +34,7 @@ #include "cartographer/mapping/trajectory_node.h" #include "cartographer/sensor/fixed_frame_pose_data.h" #include "cartographer/sensor/imu_data.h" +#include "cartographer/sensor/landmark_data.h" #include "cartographer/sensor/map_by_time.h" #include "cartographer/sensor/odometry_data.h" #include "cartographer/transform/rigid_transform.h" @@ -71,6 +72,10 @@ class PoseGraph : public PoseGraphInterface { int trajectory_id, const sensor::FixedFramePoseData& fixed_frame_pose_data) = 0; + // Inserts landmarks observations. + virtual void AddLandmarkData(int trajectory_id, + const sensor::LandmarkData& landmark_data) = 0; + // Finishes the given trajectory. virtual void FinishTrajectory(int trajectory_id) = 0; diff --git a/cartographer/mapping/trajectory_builder_interface.h b/cartographer/mapping/trajectory_builder_interface.h index 8a5d912..4c386e9 100644 --- a/cartographer/mapping/trajectory_builder_interface.h +++ b/cartographer/mapping/trajectory_builder_interface.h @@ -29,6 +29,7 @@ #include "cartographer/mapping/submaps.h" #include "cartographer/sensor/fixed_frame_pose_data.h" #include "cartographer/sensor/imu_data.h" +#include "cartographer/sensor/landmark_data.h" #include "cartographer/sensor/odometry_data.h" #include "cartographer/sensor/timed_point_cloud_data.h" @@ -78,7 +79,8 @@ class TrajectoryBuilderInterface { virtual void AddSensorData( const std::string& sensor_id, const sensor::FixedFramePoseData& fixed_frame_pose) = 0; - + virtual void AddSensorData(const std::string& sensor_id, + const sensor::LandmarkData& landmark_data) = 0; // Allows to directly add local SLAM results to the 'PoseGraph'. Note that it // is invalid to add local SLAM results for a trajectory that has a // 'LocalTrajectoryBuilder'. diff --git a/cartographer/mapping_2d/pose_graph.cc b/cartographer/mapping_2d/pose_graph.cc index 7579b46..a61543d 100644 --- a/cartographer/mapping_2d/pose_graph.cc +++ b/cartographer/mapping_2d/pose_graph.cc @@ -172,6 +172,12 @@ void PoseGraph::AddFixedFramePoseData( LOG(FATAL) << "Not yet implemented for 2D."; } +void PoseGraph::AddLandmarkData(int trajectory_id, + const sensor::LandmarkData& landmark_data) + EXCLUDES(mutex_) { + LOG(FATAL) << "Not yet implemented."; +} + void PoseGraph::ComputeConstraint(const mapping::NodeId& node_id, const mapping::SubmapId& submap_id) { CHECK(submap_data_.at(submap_id).state == SubmapState::kFinished); diff --git a/cartographer/mapping_2d/pose_graph.h b/cartographer/mapping_2d/pose_graph.h index 1b39d51..6ecbbab 100644 --- a/cartographer/mapping_2d/pose_graph.h +++ b/cartographer/mapping_2d/pose_graph.h @@ -85,6 +85,9 @@ class PoseGraph : public mapping::PoseGraph { int trajectory_id, const sensor::FixedFramePoseData& fixed_frame_pose_data) override EXCLUDES(mutex_); + void AddLandmarkData(int trajectory_id, + const sensor::LandmarkData& landmark_data) override + EXCLUDES(mutex_); void FinishTrajectory(int trajectory_id) override; bool IsTrajectoryFinished(int trajectory_id) override; diff --git a/cartographer/mapping_3d/pose_graph.cc b/cartographer/mapping_3d/pose_graph.cc index 396e11c..e168f57 100644 --- a/cartographer/mapping_3d/pose_graph.cc +++ b/cartographer/mapping_3d/pose_graph.cc @@ -174,6 +174,12 @@ void PoseGraph::AddFixedFramePoseData( }); } +void PoseGraph::AddLandmarkData(int trajectory_id, + const sensor::LandmarkData& landmark_data) + EXCLUDES(mutex_) { + LOG(FATAL) << "Not yet implemented."; +} + void PoseGraph::ComputeConstraint(const mapping::NodeId& node_id, const mapping::SubmapId& submap_id) { CHECK(submap_data_.at(submap_id).state == SubmapState::kFinished); diff --git a/cartographer/mapping_3d/pose_graph.h b/cartographer/mapping_3d/pose_graph.h index ebc79f4..175e332 100644 --- a/cartographer/mapping_3d/pose_graph.h +++ b/cartographer/mapping_3d/pose_graph.h @@ -85,6 +85,9 @@ class PoseGraph : public mapping::PoseGraph { int trajectory_id, const sensor::FixedFramePoseData& fixed_frame_pose_data) override EXCLUDES(mutex_); + void AddLandmarkData(int trajectory_id, + const sensor::LandmarkData& landmark_data) override + EXCLUDES(mutex_); void FinishTrajectory(int trajectory_id) override; bool IsTrajectoryFinished(int trajectory_id) override; diff --git a/cartographer_grpc/client_server_test.cc b/cartographer_grpc/client_server_test.cc index e2a5496..6e59171 100644 --- a/cartographer_grpc/client_server_test.cc +++ b/cartographer_grpc/client_server_test.cc @@ -78,6 +78,8 @@ class MockTrajectoryBuilder MOCK_METHOD2(AddSensorData, void(const std::string&, const cartographer::sensor::FixedFramePoseData&)); + MOCK_METHOD2(AddSensorData, void(const std::string&, + const cartographer::sensor::LandmarkData&)); // Some of the platforms we run on may ship with a version of gmock which does // not yet support move-only types. diff --git a/cartographer_grpc/mapping/trajectory_builder_stub.cc b/cartographer_grpc/mapping/trajectory_builder_stub.cc index 744af61..85aaea9 100644 --- a/cartographer_grpc/mapping/trajectory_builder_stub.cc +++ b/cartographer_grpc/mapping/trajectory_builder_stub.cc @@ -127,6 +127,12 @@ void TrajectoryBuilderStub::AddSensorData( fixed_frame_writer_.client_writer->Write(request); } +void TrajectoryBuilderStub::AddSensorData( + const std::string& sensor_id, + const cartographer::sensor::LandmarkData& landmark_data) { + LOG(FATAL) << "Not implemented"; +} + void TrajectoryBuilderStub::AddLocalSlamResultData( std::unique_ptr local_slam_result_data) { diff --git a/cartographer_grpc/mapping/trajectory_builder_stub.h b/cartographer_grpc/mapping/trajectory_builder_stub.h index ef816b4..bf30e3c 100644 --- a/cartographer_grpc/mapping/trajectory_builder_stub.h +++ b/cartographer_grpc/mapping/trajectory_builder_stub.h @@ -49,6 +49,9 @@ class TrajectoryBuilderStub void AddSensorData(const std::string& sensor_id, const cartographer::sensor::FixedFramePoseData& fixed_frame_pose) override; + void AddSensorData( + const std::string& sensor_id, + const cartographer::sensor::LandmarkData& landmark_data) override; void AddLocalSlamResultData( std::unique_ptr local_slam_result_data) override;