From e1a182d1fa7786ebb19239fe41102eda602c6b2a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Sch=C3=BCtte?= Date: Thu, 11 Jan 2018 10:19:37 +0100 Subject: [PATCH] Implement LocalSlamResult data adding to PoseGraph. (#804) --- .../mapping/global_trajectory_builder.h | 8 +++ .../mapping/collated_trajectory_builder.cc | 3 +- .../mapping/collated_trajectory_builder.h | 16 +++-- .../mapping/local_slam_result_data.cc | 64 +++++++++++++++++ cartographer/mapping/local_slam_result_data.h | 71 +++++++++++++++++++ .../mapping/trajectory_builder_interface.h | 7 ++ cartographer/sensor/data.h | 28 ++------ cartographer/sensor/dispatchable.h | 51 +++++++++++++ cartographer/sensor/ordered_multi_queue.h | 2 +- cartographer_grpc/client_server_test.cc | 11 +++ cartographer_grpc/data_uploader.cc | 23 ++++++ cartographer_grpc/data_uploader.h | 31 ++++++++ cartographer_grpc/map_builder_server.h | 2 +- .../mapping/trajectory_builder_stub.cc | 7 ++ .../mapping/trajectory_builder_stub.h | 4 ++ 15 files changed, 295 insertions(+), 33 deletions(-) create mode 100644 cartographer/mapping/local_slam_result_data.cc create mode 100644 cartographer/mapping/local_slam_result_data.h create mode 100644 cartographer/sensor/dispatchable.h create mode 100644 cartographer_grpc/data_uploader.cc create mode 100644 cartographer_grpc/data_uploader.h diff --git a/cartographer/internal/mapping/global_trajectory_builder.h b/cartographer/internal/mapping/global_trajectory_builder.h index 05a22c2..9de6676 100644 --- a/cartographer/internal/mapping/global_trajectory_builder.h +++ b/cartographer/internal/mapping/global_trajectory_builder.h @@ -19,6 +19,7 @@ #include "cartographer/mapping/trajectory_builder_interface.h" +#include "cartographer/mapping/local_slam_result_data.h" #include "glog/logging.h" namespace cartographer { @@ -105,6 +106,13 @@ class GlobalTrajectoryBuilder : public mapping::TrajectoryBuilderInterface { pose_graph_->AddFixedFramePoseData(trajectory_id_, fixed_frame_pose); } + void AddLocalSlamResultData(std::unique_ptr + local_slam_result_data) override { + CHECK(!local_trajectory_builder_) << "Can't add LocalSlamResultData with " + "local_trajectory_builder_ present."; + local_slam_result_data->AddToPoseGraph(trajectory_id_, pose_graph_); + } + private: const int trajectory_id_; PoseGraph* const pose_graph_; diff --git a/cartographer/mapping/collated_trajectory_builder.cc b/cartographer/mapping/collated_trajectory_builder.cc index 44dadc2..f610c40 100644 --- a/cartographer/mapping/collated_trajectory_builder.cc +++ b/cartographer/mapping/collated_trajectory_builder.cc @@ -45,8 +45,7 @@ CollatedTrajectoryBuilder::CollatedTrajectoryBuilder( CollatedTrajectoryBuilder::~CollatedTrajectoryBuilder() {} -void CollatedTrajectoryBuilder::AddSensorData( - std::unique_ptr data) { +void CollatedTrajectoryBuilder::AddData(std::unique_ptr data) { sensor_collator_->AddSensorData(trajectory_id_, std::move(data)); } diff --git a/cartographer/mapping/collated_trajectory_builder.h b/cartographer/mapping/collated_trajectory_builder.h index a04e29d..72696c1 100644 --- a/cartographer/mapping/collated_trajectory_builder.h +++ b/cartographer/mapping/collated_trajectory_builder.h @@ -28,7 +28,7 @@ #include "cartographer/mapping/submaps.h" #include "cartographer/mapping/trajectory_builder_interface.h" #include "cartographer/sensor/collator.h" -#include "cartographer/sensor/data.h" +#include "cartographer/sensor/dispatchable.h" namespace cartographer { namespace mapping { @@ -50,27 +50,31 @@ class CollatedTrajectoryBuilder : public TrajectoryBuilderInterface { void AddSensorData( const std::string& sensor_id, const sensor::TimedPointCloudData& timed_point_cloud_data) override { - AddSensorData(sensor::MakeDispatchable(sensor_id, timed_point_cloud_data)); + AddData(sensor::MakeDispatchable(sensor_id, timed_point_cloud_data)); } void AddSensorData(const std::string& sensor_id, const sensor::ImuData& imu_data) override { - AddSensorData(sensor::MakeDispatchable(sensor_id, imu_data)); + AddData(sensor::MakeDispatchable(sensor_id, imu_data)); } void AddSensorData(const std::string& sensor_id, const sensor::OdometryData& odometry_data) override { - AddSensorData(sensor::MakeDispatchable(sensor_id, odometry_data)); + AddData(sensor::MakeDispatchable(sensor_id, odometry_data)); } void AddSensorData( const std::string& sensor_id, const sensor::FixedFramePoseData& fixed_frame_pose_data) override { - AddSensorData(sensor::MakeDispatchable(sensor_id, fixed_frame_pose_data)); + AddData(sensor::MakeDispatchable(sensor_id, fixed_frame_pose_data)); + } + void AddLocalSlamResultData(std::unique_ptr + local_slam_result_data) override { + AddData(std::move(local_slam_result_data)); } private: - void AddSensorData(std::unique_ptr data); + void AddData(std::unique_ptr data); void HandleCollatedSensorData(const std::string& sensor_id, std::unique_ptr data); diff --git a/cartographer/mapping/local_slam_result_data.cc b/cartographer/mapping/local_slam_result_data.cc new file mode 100644 index 0000000..42e8d4d --- /dev/null +++ b/cartographer/mapping/local_slam_result_data.cc @@ -0,0 +1,64 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "local_slam_result_data.h" +#include "cartographer/mapping/trajectory_builder_interface.h" + +namespace cartographer { +namespace mapping { + +LocalSlamResultData::LocalSlamResultData(const std::string &sensor_id, + common::Time time) + : Data(sensor_id), time_(time) {} + +LocalSlamResult2D::LocalSlamResult2D(const std::string &sensor_id, + common::Time time) + : LocalSlamResultData(sensor_id, time) {} + +void LocalSlamResult2D::AddToTrajectoryBuilder( + mapping::TrajectoryBuilderInterface *const trajectory_builder) { + trajectory_builder->AddLocalSlamResultData( + common::make_unique(*this)); +} + +void LocalSlamResult2D::AddToPoseGraph(int trajectory_id, + mapping::PoseGraph *pose_graph) const { + DCHECK(dynamic_cast(pose_graph)); + mapping_2d::PoseGraph *pose_graph_2d = + static_cast(pose_graph); + pose_graph_2d->AddNode(constant_data, trajectory_id, insertion_submaps); +} + +LocalSlamResult3D::LocalSlamResult3D(const std::string &sensor_id, + common::Time time) + : LocalSlamResultData(sensor_id, time) {} + +void LocalSlamResult3D::AddToTrajectoryBuilder( + mapping::TrajectoryBuilderInterface *const trajectory_builder) { + trajectory_builder->AddLocalSlamResultData( + common::make_unique(*this)); +} + +void LocalSlamResult3D::AddToPoseGraph(int trajectory_id, + mapping::PoseGraph *pose_graph) const { + DCHECK(dynamic_cast(pose_graph)); + mapping_3d::PoseGraph *pose_graph_3d = + static_cast(pose_graph); + pose_graph_3d->AddNode(constant_data, trajectory_id, insertion_submaps); +} + +} // namespace mapping +} // namespace cartographer diff --git a/cartographer/mapping/local_slam_result_data.h b/cartographer/mapping/local_slam_result_data.h new file mode 100644 index 0000000..99bda11 --- /dev/null +++ b/cartographer/mapping/local_slam_result_data.h @@ -0,0 +1,71 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_MAPPING_LOCAL_SLAM_RESULT_DATA_H +#define CARTOGRAPHER_MAPPING_LOCAL_SLAM_RESULT_DATA_H + +#include "cartographer/mapping_2d/pose_graph.h" +#include "cartographer/mapping_3d/pose_graph.h" +#include "cartographer/sensor/data.h" + +namespace cartographer { +namespace mapping { + +class TrajectoryBuilderInterface; +class LocalSlamResultData : public cartographer::sensor::Data { + public: + LocalSlamResultData(const std::string &sensor_id, common::Time time); + + common::Time GetTime() const override { return time_; } + virtual void AddToPoseGraph(int trajectory_id, + mapping::PoseGraph *pose_graph) const = 0; + + private: + common::Time time_; +}; + +class LocalSlamResult2D : public LocalSlamResultData { + public: + LocalSlamResult2D(const std::string &sensor_id, common::Time time); + + void AddToTrajectoryBuilder( + mapping::TrajectoryBuilderInterface *const trajectory_builder) override; + void AddToPoseGraph(int trajectory_id, + mapping::PoseGraph *pose_graph) const override; + + private: + std::shared_ptr constant_data; + std::vector> insertion_submaps; +}; + +class LocalSlamResult3D : public LocalSlamResultData { + public: + LocalSlamResult3D(const std::string &sensor_id, common::Time time); + + void AddToTrajectoryBuilder( + mapping::TrajectoryBuilderInterface *const trajectory_builder) override; + void AddToPoseGraph(int trajectory_id, + mapping::PoseGraph *pose_graph) const override; + + private: + std::shared_ptr constant_data; + std::vector> insertion_submaps; +}; + +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_LOCAL_SLAM_RESULT_DATA_H diff --git a/cartographer/mapping/trajectory_builder_interface.h b/cartographer/mapping/trajectory_builder_interface.h index c84194f..fd2e3a1 100644 --- a/cartographer/mapping/trajectory_builder_interface.h +++ b/cartographer/mapping/trajectory_builder_interface.h @@ -25,6 +25,7 @@ #include "cartographer/common/make_unique.h" #include "cartographer/common/port.h" #include "cartographer/common/time.h" +#include "cartographer/mapping/local_slam_result_data.h" #include "cartographer/mapping/proto/trajectory_builder_options.pb.h" #include "cartographer/mapping/submaps.h" #include "cartographer/sensor/fixed_frame_pose_data.h" @@ -76,6 +77,12 @@ class TrajectoryBuilderInterface { virtual void AddSensorData( const std::string& sensor_id, const sensor::FixedFramePoseData& fixed_frame_pose) = 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'. + virtual void AddLocalSlamResultData( + std::unique_ptr local_slam_result_data) = 0; }; } // namespace mapping diff --git a/cartographer/sensor/data.h b/cartographer/sensor/data.h index 064545d..919dfdb 100644 --- a/cartographer/sensor/data.h +++ b/cartographer/sensor/data.h @@ -19,10 +19,14 @@ #include "cartographer/common/make_unique.h" #include "cartographer/common/time.h" -#include "cartographer/mapping/trajectory_builder_interface.h" #include "cartographer/transform/rigid_transform.h" namespace cartographer { + +namespace mapping { +class TrajectoryBuilderInterface; +} + namespace sensor { class Data { @@ -39,28 +43,6 @@ class Data { const std::string sensor_id_; }; -template -class Dispatchable : public Data { - public: - Dispatchable(const std::string &sensor_id, const DataType &data) - : Data(sensor_id), data_(data) {} - - common::Time GetTime() const override { return data_.time; } - void AddToTrajectoryBuilder( - mapping::TrajectoryBuilderInterface *const trajectory_builder) override { - trajectory_builder->AddSensorData(sensor_id_, data_); - } - - private: - const DataType data_; -}; - -template -std::unique_ptr> MakeDispatchable( - const std::string &sensor_id, const DataType &data) { - return common::make_unique>(sensor_id, data); -} - } // namespace sensor } // namespace cartographer diff --git a/cartographer/sensor/dispatchable.h b/cartographer/sensor/dispatchable.h new file mode 100644 index 0000000..876743c --- /dev/null +++ b/cartographer/sensor/dispatchable.h @@ -0,0 +1,51 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_SENSOR_MAPPING_DISPATCHABLE_H_ +#define CARTOGRAPHER_SENSOR_MAPPING_DISPATCHABLE_H_ + +#include "cartographer/mapping/trajectory_builder_interface.h" +#include "cartographer/sensor/data.h" + +namespace cartographer { +namespace sensor { + +template +class Dispatchable : public Data { + public: + Dispatchable(const std::string &sensor_id, const DataType &data) + : Data(sensor_id), data_(data) {} + + common::Time GetTime() const override { return data_.time; } + void AddToTrajectoryBuilder( + mapping::TrajectoryBuilderInterface *const trajectory_builder) override { + trajectory_builder->AddSensorData(sensor_id_, data_); + } + + private: + const DataType data_; +}; + +template +std::unique_ptr> MakeDispatchable( + const std::string &sensor_id, const DataType &data) { + return common::make_unique>(sensor_id, data); +} + +} // namespace sensor +} // namespace cartographer + +#endif // CARTOGRAPHER_SENSOR_MAPPING_DISPATCHABLE_H_ diff --git a/cartographer/sensor/ordered_multi_queue.h b/cartographer/sensor/ordered_multi_queue.h index 37f7b09..6129b3c 100644 --- a/cartographer/sensor/ordered_multi_queue.h +++ b/cartographer/sensor/ordered_multi_queue.h @@ -26,7 +26,7 @@ #include "cartographer/common/blocking_queue.h" #include "cartographer/common/port.h" #include "cartographer/common/time.h" -#include "cartographer/sensor/data.h" +#include "cartographer/sensor/dispatchable.h" namespace cartographer { namespace sensor { diff --git a/cartographer_grpc/client_server_test.cc b/cartographer_grpc/client_server_test.cc index 87da928..f4891c8 100644 --- a/cartographer_grpc/client_server_test.cc +++ b/cartographer_grpc/client_server_test.cc @@ -18,6 +18,7 @@ #include #include "cartographer/internal/mapping/test_helpers.h" +#include "cartographer/mapping/local_slam_result_data.h" #include "cartographer_grpc/map_builder_server.h" #include "cartographer_grpc/map_builder_server_options.h" #include "cartographer_grpc/mapping/map_builder_stub.h" @@ -77,6 +78,16 @@ class MockTrajectoryBuilder MOCK_METHOD2(AddSensorData, void(const std::string&, const cartographer::sensor::FixedFramePoseData&)); + + // Some of the platforms we run on may ship with a version of gmock which does + // not yet support move-only types. + MOCK_METHOD1(DoAddLocalSlamResultData, + void(cartographer::mapping::LocalSlamResultData*)); + void AddLocalSlamResultData( + std::unique_ptr + local_slam_result_data) override { + DoAddLocalSlamResultData(local_slam_result_data.get()); + } }; class ClientServerTest : public ::testing::Test { diff --git a/cartographer_grpc/data_uploader.cc b/cartographer_grpc/data_uploader.cc new file mode 100644 index 0000000..c7fa8ce --- /dev/null +++ b/cartographer_grpc/data_uploader.cc @@ -0,0 +1,23 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer_grpc/data_uploader.h" + +namespace cartographer_grpc { + +DataUploader::DataUploader(const std::string &data_uploader) {} + +} // namespace cartographer_grpc diff --git a/cartographer_grpc/data_uploader.h b/cartographer_grpc/data_uploader.h new file mode 100644 index 0000000..4d00da0 --- /dev/null +++ b/cartographer_grpc/data_uploader.h @@ -0,0 +1,31 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_GRPC_DATA_UPLOADER_H +#define CARTOGRAPHER_GRPC_DATA_UPLOADER_H + +#include + +namespace cartographer_grpc { + +class DataUploader { + public: + DataUploader(const std::string &server_address); +}; + +} // namespace cartographer_grpc + +#endif // CARTOGRAPHER_GRPC_DATA_UPLOADER_H diff --git a/cartographer_grpc/map_builder_server.h b/cartographer_grpc/map_builder_server.h index 86e37ce..d6dba06 100644 --- a/cartographer_grpc/map_builder_server.h +++ b/cartographer_grpc/map_builder_server.h @@ -21,7 +21,7 @@ #include "cartographer/common/time.h" #include "cartographer/mapping/map_builder.h" #include "cartographer/mapping/trajectory_builder_interface.h" -#include "cartographer/sensor/data.h" +#include "cartographer/sensor/dispatchable.h" #include "cartographer_grpc/framework/execution_context.h" #include "cartographer_grpc/framework/server.h" #include "cartographer_grpc/proto/map_builder_server_options.pb.h" diff --git a/cartographer_grpc/mapping/trajectory_builder_stub.cc b/cartographer_grpc/mapping/trajectory_builder_stub.cc index 79421c3..e638996 100644 --- a/cartographer_grpc/mapping/trajectory_builder_stub.cc +++ b/cartographer_grpc/mapping/trajectory_builder_stub.cc @@ -16,6 +16,7 @@ #include "cartographer_grpc/mapping/trajectory_builder_stub.h" +#include "cartographer/mapping/local_slam_result_data.h" #include "cartographer_grpc/proto/map_builder_service.pb.h" #include "glog/logging.h" @@ -124,6 +125,12 @@ void TrajectoryBuilderStub::AddSensorData( fixed_frame_writer_.client_writer->Write(request); } +void TrajectoryBuilderStub::AddLocalSlamResultData( + std::unique_ptr + local_slam_result_data) { + LOG(FATAL) << "Not implemented"; +} + proto::SensorMetadata TrajectoryBuilderStub::CreateSensorMetadata( const std::string& sensor_id) { proto::SensorMetadata sensor_metadata; diff --git a/cartographer_grpc/mapping/trajectory_builder_stub.h b/cartographer_grpc/mapping/trajectory_builder_stub.h index 09b6fa5..12f4a5a 100644 --- a/cartographer_grpc/mapping/trajectory_builder_stub.h +++ b/cartographer_grpc/mapping/trajectory_builder_stub.h @@ -19,6 +19,7 @@ #include +#include "cartographer/mapping/local_slam_result_data.h" #include "cartographer/mapping/trajectory_builder_interface.h" #include "cartographer_grpc/proto/map_builder_service.grpc.pb.h" #include "grpc++/grpc++.h" @@ -47,6 +48,9 @@ class TrajectoryBuilderStub void AddSensorData(const std::string& sensor_id, const cartographer::sensor::FixedFramePoseData& fixed_frame_pose) override; + void AddLocalSlamResultData( + std::unique_ptr + local_slam_result_data) override; private: template