From 0671e8835e0acbc6b919866ae3e6b0c401342020 Mon Sep 17 00:00:00 2001 From: jie Date: Thu, 24 Aug 2017 12:49:03 +0200 Subject: [PATCH] Add interface to process fixed frame pose, e.g. GPS. (#471) --- .../mapping/collated_trajectory_builder.cc | 5 +++ .../global_trajectory_builder_interface.h | 3 ++ cartographer/mapping/trajectory_builder.h | 6 +++ .../mapping_2d/global_trajectory_builder.cc | 5 +++ .../mapping_2d/global_trajectory_builder.h | 2 + cartographer/mapping_2d/sparse_pose_graph.cc | 6 +++ cartographer/mapping_2d/sparse_pose_graph.h | 5 ++- .../mapping_3d/global_trajectory_builder.cc | 5 +++ .../mapping_3d/global_trajectory_builder.h | 3 ++ cartographer/mapping_3d/sparse_pose_graph.cc | 6 +++ cartographer/mapping_3d/sparse_pose_graph.h | 5 ++- cartographer/sensor/data.h | 12 +++++- cartographer/sensor/fixed_frame_pose_data.cc | 37 ++++++++++++++++ cartographer/sensor/fixed_frame_pose_data.h | 43 +++++++++++++++++++ cartographer/sensor/proto/sensor.proto | 6 +++ 15 files changed, 146 insertions(+), 3 deletions(-) create mode 100644 cartographer/sensor/fixed_frame_pose_data.cc create mode 100644 cartographer/sensor/fixed_frame_pose_data.h diff --git a/cartographer/mapping/collated_trajectory_builder.cc b/cartographer/mapping/collated_trajectory_builder.cc index 2fe3ef2..32a5808 100644 --- a/cartographer/mapping/collated_trajectory_builder.cc +++ b/cartographer/mapping/collated_trajectory_builder.cc @@ -92,6 +92,11 @@ void CollatedTrajectoryBuilder::HandleCollatedSensorData( wrapped_trajectory_builder_->AddOdometerData(data->time, data->odometer_pose); return; + + case sensor::Data::Type::kFixedFramePose: + wrapped_trajectory_builder_->AddFixedFramePoseData( + sensor::FixedFramePoseData{data->time, data->fixed_frame_pose.pose}); + return; } LOG(FATAL); } diff --git a/cartographer/mapping/global_trajectory_builder_interface.h b/cartographer/mapping/global_trajectory_builder_interface.h index cd90504..63ccdd3 100644 --- a/cartographer/mapping/global_trajectory_builder_interface.h +++ b/cartographer/mapping/global_trajectory_builder_interface.h @@ -25,6 +25,7 @@ #include "cartographer/common/time.h" #include "cartographer/mapping/pose_estimate.h" #include "cartographer/mapping/submaps.h" +#include "cartographer/sensor/fixed_frame_pose_data.h" #include "cartographer/sensor/imu_data.h" #include "cartographer/sensor/point_cloud.h" #include "cartographer/sensor/range_data.h" @@ -55,6 +56,8 @@ class GlobalTrajectoryBuilderInterface { virtual void AddImuData(const sensor::ImuData& imu_data) = 0; virtual void AddOdometerData(common::Time time, const transform::Rigid3d& pose) = 0; + virtual void AddFixedFramePoseData( + const sensor::FixedFramePoseData& fixed_frame_pose) = 0; }; } // namespace mapping diff --git a/cartographer/mapping/trajectory_builder.h b/cartographer/mapping/trajectory_builder.h index 0fb6a65..4d48ed3 100644 --- a/cartographer/mapping/trajectory_builder.h +++ b/cartographer/mapping/trajectory_builder.h @@ -75,6 +75,12 @@ class TrajectoryBuilder { AddSensorData(sensor_id, common::make_unique(time, odometer_pose)); } + + void AddFixedFramePose(const string& sensor_id, common::Time time, + const transform::Rigid3d& fixed_frame_pose) { + AddSensorData(sensor_id, + common::make_unique(time, fixed_frame_pose)); + } }; } // namespace mapping diff --git a/cartographer/mapping_2d/global_trajectory_builder.cc b/cartographer/mapping_2d/global_trajectory_builder.cc index 05af4e2..544c5c8 100644 --- a/cartographer/mapping_2d/global_trajectory_builder.cc +++ b/cartographer/mapping_2d/global_trajectory_builder.cc @@ -56,6 +56,11 @@ void GlobalTrajectoryBuilder::AddOdometerData(const common::Time time, sensor::OdometryData{time, pose}); } +void GlobalTrajectoryBuilder::AddFixedFramePoseData( + const sensor::FixedFramePoseData& fixed_frame_pose) { + sparse_pose_graph_->AddFixedFramePoseData(trajectory_id_, fixed_frame_pose); +} + const mapping::PoseEstimate& GlobalTrajectoryBuilder::pose_estimate() const { return local_trajectory_builder_.pose_estimate(); } diff --git a/cartographer/mapping_2d/global_trajectory_builder.h b/cartographer/mapping_2d/global_trajectory_builder.h index 82ba132..94ee44d 100644 --- a/cartographer/mapping_2d/global_trajectory_builder.h +++ b/cartographer/mapping_2d/global_trajectory_builder.h @@ -44,6 +44,8 @@ class GlobalTrajectoryBuilder void AddImuData(const sensor::ImuData& imu_data) override; void AddOdometerData(common::Time time, const transform::Rigid3d& pose) override; + void AddFixedFramePoseData( + const sensor::FixedFramePoseData& fixed_frame_pose) override; private: const int trajectory_id_; diff --git a/cartographer/mapping_2d/sparse_pose_graph.cc b/cartographer/mapping_2d/sparse_pose_graph.cc index 045f421..d62946c 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.cc +++ b/cartographer/mapping_2d/sparse_pose_graph.cc @@ -166,6 +166,12 @@ void SparsePoseGraph::AddOdometerData( }); } +void SparsePoseGraph::AddFixedFramePoseData( + const int trajectory_id, + const sensor::FixedFramePoseData& fixed_frame_pose_data) { + LOG(FATAL) << "Not yet implemented for 2D."; +} + void SparsePoseGraph::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/sparse_pose_graph.h b/cartographer/mapping_2d/sparse_pose_graph.h index c30eee2..2717b3e 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.h +++ b/cartographer/mapping_2d/sparse_pose_graph.h @@ -38,6 +38,7 @@ #include "cartographer/mapping_2d/sparse_pose_graph/constraint_builder.h" #include "cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h" #include "cartographer/mapping_2d/submaps.h" +#include "cartographer/sensor/fixed_frame_pose_data.h" #include "cartographer/sensor/point_cloud.h" #include "cartographer/transform/rigid_transform.h" #include "cartographer/transform/transform.h" @@ -76,10 +77,12 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { const std::vector>& insertion_submaps) EXCLUDES(mutex_); - // Adds new IMU data to be used in the optimization. void AddImuData(int trajectory_id, const sensor::ImuData& imu_data); void AddOdometerData(int trajectory_id, const sensor::OdometryData& odometry_data); + void AddFixedFramePoseData( + int trajectory_id, + const sensor::FixedFramePoseData& fixed_frame_pose_data); void FreezeTrajectory(int trajectory_id) override; void AddSubmapFromProto(int trajectory_id, diff --git a/cartographer/mapping_3d/global_trajectory_builder.cc b/cartographer/mapping_3d/global_trajectory_builder.cc index dc11c19..f15ef58 100644 --- a/cartographer/mapping_3d/global_trajectory_builder.cc +++ b/cartographer/mapping_3d/global_trajectory_builder.cc @@ -52,6 +52,11 @@ void GlobalTrajectoryBuilder::AddOdometerData(const common::Time time, local_trajectory_builder_.AddOdometerData(time, pose); } +void GlobalTrajectoryBuilder::AddFixedFramePoseData( + const sensor::FixedFramePoseData& fixed_frame_pose) { + sparse_pose_graph_->AddFixedFramePoseData(trajectory_id_, fixed_frame_pose); +} + const mapping::PoseEstimate& GlobalTrajectoryBuilder::pose_estimate() const { return local_trajectory_builder_.pose_estimate(); } diff --git a/cartographer/mapping_3d/global_trajectory_builder.h b/cartographer/mapping_3d/global_trajectory_builder.h index ce5ab8e..ce042b2 100644 --- a/cartographer/mapping_3d/global_trajectory_builder.h +++ b/cartographer/mapping_3d/global_trajectory_builder.h @@ -41,6 +41,9 @@ class GlobalTrajectoryBuilder const sensor::PointCloud& ranges) override; void AddOdometerData(common::Time time, const transform::Rigid3d& pose) override; + void AddFixedFramePoseData( + const sensor::FixedFramePoseData& fixed_frame_pose) override; + const mapping::PoseEstimate& pose_estimate() const override; private: diff --git a/cartographer/mapping_3d/sparse_pose_graph.cc b/cartographer/mapping_3d/sparse_pose_graph.cc index 74e13db..9b7d085 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.cc +++ b/cartographer/mapping_3d/sparse_pose_graph.cc @@ -155,6 +155,12 @@ void SparsePoseGraph::AddImuData(const int trajectory_id, }); } +void SparsePoseGraph::AddFixedFramePoseData( + const int trajectory_id, + const sensor::FixedFramePoseData& fixed_frame_pose_data) { + LOG(FATAL) << "Not yet implemented for 3D."; +} + void SparsePoseGraph::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/sparse_pose_graph.h b/cartographer/mapping_3d/sparse_pose_graph.h index 1dbadfc..1f64a78 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.h +++ b/cartographer/mapping_3d/sparse_pose_graph.h @@ -38,6 +38,7 @@ #include "cartographer/mapping_3d/sparse_pose_graph/constraint_builder.h" #include "cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h" #include "cartographer/mapping_3d/submaps.h" +#include "cartographer/sensor/fixed_frame_pose_data.h" #include "cartographer/sensor/point_cloud.h" #include "cartographer/transform/rigid_transform.h" #include "cartographer/transform/transform.h" @@ -74,8 +75,10 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { const std::vector>& insertion_submaps) EXCLUDES(mutex_); - // Adds new IMU data to be used in the optimization. void AddImuData(int trajectory_id, const sensor::ImuData& imu_data); + void AddFixedFramePoseData( + int trajectory_id, + const sensor::FixedFramePoseData& fixed_frame_pose_data); void FreezeTrajectory(int trajectory_id) override; void AddSubmapFromProto(int trajectory_id, diff --git a/cartographer/sensor/data.h b/cartographer/sensor/data.h index 7d34240..b29665e 100644 --- a/cartographer/sensor/data.h +++ b/cartographer/sensor/data.h @@ -29,7 +29,7 @@ namespace sensor { // filled in. It is only used for time ordering sensor data before passing it // on. struct Data { - enum class Type { kImu, kRangefinder, kOdometer }; + enum class Type { kImu, kRangefinder, kOdometer, kFixedFramePose }; struct Imu { Eigen::Vector3d linear_acceleration; @@ -41,6 +41,10 @@ struct Data { PointCloud ranges; }; + struct FixedFramePose { + transform::Rigid3d pose; + }; + Data(const common::Time time, const Imu& imu) : type(Type::kImu), time(time), imu(imu) {} @@ -50,11 +54,17 @@ struct Data { Data(const common::Time time, const transform::Rigid3d& odometer_pose) : type(Type::kOdometer), time(time), odometer_pose(odometer_pose) {} + Data(const common::Time time, const FixedFramePose& fixed_frame_pose) + : type(Type::kFixedFramePose), + time(time), + fixed_frame_pose(fixed_frame_pose) {} + Type type; common::Time time; Imu imu; Rangefinder rangefinder; transform::Rigid3d odometer_pose; + FixedFramePose fixed_frame_pose; }; } // namespace sensor diff --git a/cartographer/sensor/fixed_frame_pose_data.cc b/cartographer/sensor/fixed_frame_pose_data.cc new file mode 100644 index 0000000..87626cf --- /dev/null +++ b/cartographer/sensor/fixed_frame_pose_data.cc @@ -0,0 +1,37 @@ +/* + * Copyright 2017 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/sensor/fixed_frame_pose_data.h" + +#include "cartographer/transform/transform.h" + +namespace cartographer { +namespace sensor { + +proto::FixedFramePoseData ToProto(const FixedFramePoseData& pose_data) { + proto::FixedFramePoseData proto; + proto.set_timestamp(common::ToUniversal(pose_data.time)); + *proto.mutable_pose() = transform::ToProto(pose_data.pose); + return proto; +} + +FixedFramePoseData FromProto(const proto::FixedFramePoseData& proto) { + return FixedFramePoseData{common::FromUniversal(proto.timestamp()), + transform::ToRigid3(proto.pose())}; +} + +} // namespace sensor +} // namespace cartographer diff --git a/cartographer/sensor/fixed_frame_pose_data.h b/cartographer/sensor/fixed_frame_pose_data.h new file mode 100644 index 0000000..19cdb41 --- /dev/null +++ b/cartographer/sensor/fixed_frame_pose_data.h @@ -0,0 +1,43 @@ +/* + * Copyright 2017 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_FIXED_FRAME_POSE_DATA_H_ +#define CARTOGRAPHER_SENSOR_FIXED_FRAME_POSE_DATA_H_ + +#include "cartographer/common/time.h" +#include "cartographer/sensor/proto/sensor.pb.h" +#include "cartographer/transform/rigid_transform.h" + +namespace cartographer { +namespace sensor { + +// The fixed frame pose data(like gps, pose, etc.) will be used in the +// optimization. +struct FixedFramePoseData { + common::Time time; + transform::Rigid3d pose; +}; + +// Converts 'pose_data' to a proto::FixedFramePoseData. +proto::FixedFramePoseData ToProto(const FixedFramePoseData& pose_data); + +// Converts 'proto' to an FixedFramePoseData. +FixedFramePoseData FromProto(const proto::FixedFramePoseData& proto); + +} // namespace sensor +} // namespace cartographer + +#endif // CARTOGRAPHER_SENSOR_FIXED_FRAME_POSE_DATA_H_ diff --git a/cartographer/sensor/proto/sensor.proto b/cartographer/sensor/proto/sensor.proto index 842d681..eb6b0bd 100644 --- a/cartographer/sensor/proto/sensor.proto +++ b/cartographer/sensor/proto/sensor.proto @@ -45,3 +45,9 @@ message OdometryData { optional int64 timestamp = 1; optional transform.proto.Rigid3d pose = 2; } + +// Proto representation of ::cartographer::sensor::FixedFramePoseData. +message FixedFramePoseData { + optional int64 timestamp = 1; + optional transform.proto.Rigid3d pose = 2; +}