diff --git a/cartographer/mapping/CMakeLists.txt b/cartographer/mapping/CMakeLists.txt index 1850be7..1c1248c 100644 --- a/cartographer/mapping/CMakeLists.txt +++ b/cartographer/mapping/CMakeLists.txt @@ -15,15 +15,32 @@ add_subdirectory("proto") add_subdirectory("sparse_pose_graph") -google_library(mapping_global_trajectory_builder_interface +google_library(mapping_collated_trajectory_builder + USES_GLOG SRCS - global_trajectory_builder_interface.cc + collated_trajectory_builder.cc + HDRS + collated_trajectory_builder.h + DEPENDS + common_port + common_rate_timer + common_time + kalman_filter_pose_tracker + mapping_global_trajectory_builder_interface + mapping_submaps + mapping_trajectory_builder + sensor_collator + sensor_data +) + +google_library(mapping_global_trajectory_builder_interface HDRS global_trajectory_builder_interface.h DEPENDS common_time kalman_filter_pose_tracker mapping_submaps + mapping_trajectory_builder sensor_laser sensor_point_cloud ) @@ -53,16 +70,14 @@ google_library(mapping_map_builder common_make_unique common_thread_pool mapping_2d_global_trajectory_builder - mapping_2d_local_trajectory_builder mapping_2d_sparse_pose_graph - mapping_2d_submaps mapping_3d_global_trajectory_builder mapping_3d_local_trajectory_builder_options - mapping_3d_proto_local_trajectory_builder_options mapping_3d_sparse_pose_graph mapping_global_trajectory_builder_interface mapping_proto_map_builder_options mapping_sparse_pose_graph + mapping_submaps mapping_trajectory_node sensor_laser sensor_voxel_filter @@ -125,6 +140,20 @@ google_library(mapping_submaps transform_transform ) +google_library(mapping_trajectory_builder + HDRS + trajectory_builder.h + DEPENDS + common_make_unique + common_port + common_time + kalman_filter_pose_tracker + mapping_submaps + sensor_data + sensor_laser + sensor_point_cloud +) + google_library(mapping_trajectory_connectivity USES_GLOG SRCS diff --git a/cartographer/mapping/collated_trajectory_builder.cc b/cartographer/mapping/collated_trajectory_builder.cc new file mode 100644 index 0000000..e1a6b37 --- /dev/null +++ b/cartographer/mapping/collated_trajectory_builder.cc @@ -0,0 +1,112 @@ +/* + * 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. + */ + +#include "cartographer/mapping/collated_trajectory_builder.h" + +#include "cartographer/common/time.h" +#include "glog/logging.h" + +namespace cartographer { +namespace mapping { + +namespace { + +constexpr double kSensorDataRatesLoggingPeriodSeconds = 15.; + +} // namespace + +CollatedTrajectoryBuilder::CollatedTrajectoryBuilder( + sensor::Collator* const sensor_collator, const int trajectory_id, + const std::unordered_set& expected_sensor_ids, + std::unique_ptr + wrapped_trajectory_builder) + : sensor_collator_(sensor_collator), + trajectory_id_(trajectory_id), + wrapped_trajectory_builder_(std::move(wrapped_trajectory_builder)), + last_logging_time_(std::chrono::steady_clock::now()) { + sensor_collator_->AddTrajectory( + trajectory_id, expected_sensor_ids, + [this](const string& sensor_id, std::unique_ptr data) { + HandleCollatedSensorData(sensor_id, std::move(data)); + }); +} + +CollatedTrajectoryBuilder::~CollatedTrajectoryBuilder() {} + +const Submaps* CollatedTrajectoryBuilder::submaps() const { + return wrapped_trajectory_builder_->submaps(); +} + +Submaps* CollatedTrajectoryBuilder::submaps() { + return wrapped_trajectory_builder_->submaps(); +} + +kalman_filter::PoseTracker* CollatedTrajectoryBuilder::pose_tracker() const { + return wrapped_trajectory_builder_->pose_tracker(); +} + +const TrajectoryBuilder::PoseEstimate& +CollatedTrajectoryBuilder::pose_estimate() const { + return wrapped_trajectory_builder_->pose_estimate(); +} + +void CollatedTrajectoryBuilder::AddSensorData( + const string& sensor_id, std::unique_ptr data) { + sensor_collator_->AddSensorData(trajectory_id_, sensor_id, std::move(data)); +} + +void CollatedTrajectoryBuilder::HandleCollatedSensorData( + const string& sensor_id, std::unique_ptr data) { + auto it = rate_timers_.find(sensor_id); + if (it == rate_timers_.end()) { + it = rate_timers_ + .emplace( + std::piecewise_construct, std::forward_as_tuple(sensor_id), + std::forward_as_tuple( + common::FromSeconds(kSensorDataRatesLoggingPeriodSeconds))) + .first; + } + it->second.Pulse(data->time); + + if (std::chrono::steady_clock::now() - last_logging_time_ > + common::FromSeconds(kSensorDataRatesLoggingPeriodSeconds)) { + for (const auto& pair : rate_timers_) { + LOG(INFO) << pair.first << " rate: " << pair.second.DebugString(); + } + last_logging_time_ = std::chrono::steady_clock::now(); + } + + switch (data->type) { + case sensor::Data::Type::kImu: + wrapped_trajectory_builder_->AddImuData(data->time, + data->imu.linear_acceleration, + data->imu.angular_velocity); + return; + + case sensor::Data::Type::kLaserFan: + wrapped_trajectory_builder_->AddLaserFan(data->time, data->laser_fan); + return; + + case sensor::Data::Type::kOdometry: + wrapped_trajectory_builder_->AddOdometerPose( + data->time, data->odometry.pose, data->odometry.covariance); + return; + } + LOG(FATAL); +} + +} // namespace mapping +} // namespace cartographer diff --git a/cartographer/mapping/collated_trajectory_builder.h b/cartographer/mapping/collated_trajectory_builder.h new file mode 100644 index 0000000..3ce7c83 --- /dev/null +++ b/cartographer/mapping/collated_trajectory_builder.h @@ -0,0 +1,77 @@ +/* + * 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_MAPPING_COLLATED_TRAJECTORY_BUILDER_H_ +#define CARTOGRAPHER_MAPPING_COLLATED_TRAJECTORY_BUILDER_H_ + +#include +#include +#include +#include +#include + +#include "cartographer/common/port.h" +#include "cartographer/common/rate_timer.h" +#include "cartographer/kalman_filter/pose_tracker.h" +#include "cartographer/mapping/global_trajectory_builder_interface.h" +#include "cartographer/mapping/submaps.h" +#include "cartographer/mapping/trajectory_builder.h" +#include "cartographer/sensor/collator.h" +#include "cartographer/sensor/data.h" + +namespace cartographer { +namespace mapping { + +// Handles collating sensor data using a sensor::Collator, then passing it on to +// a mapping::GlobalTrajectoryBuilderInterface which is common for 2D and 3D. +class CollatedTrajectoryBuilder : public TrajectoryBuilder { + public: + CollatedTrajectoryBuilder( + sensor::Collator* sensor_collator, int trajectory_id, + const std::unordered_set& expected_sensor_ids, + std::unique_ptr + wrapped_trajectory_builder); + ~CollatedTrajectoryBuilder() override; + + CollatedTrajectoryBuilder(const CollatedTrajectoryBuilder&) = delete; + CollatedTrajectoryBuilder& operator=(const CollatedTrajectoryBuilder&) = + delete; + + const Submaps* submaps() const override; + Submaps* submaps() override; + kalman_filter::PoseTracker* pose_tracker() const override; + const PoseEstimate& pose_estimate() const override; + + void AddSensorData(const string& sensor_id, + std::unique_ptr data) override; + + private: + void HandleCollatedSensorData(const string& sensor_id, + std::unique_ptr data); + + sensor::Collator* const sensor_collator_; + const int trajectory_id_; + std::unique_ptr wrapped_trajectory_builder_; + + // Time at which we last logged the rates of incoming sensor data. + std::chrono::steady_clock::time_point last_logging_time_; + std::map> rate_timers_; +}; + +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_COLLATED_TRAJECTORY_BUILDER_H_ diff --git a/cartographer/mapping/global_trajectory_builder_interface.cc b/cartographer/mapping/global_trajectory_builder_interface.cc deleted file mode 100644 index 9c3fccc..0000000 --- a/cartographer/mapping/global_trajectory_builder_interface.cc +++ /dev/null @@ -1,35 +0,0 @@ -/* - * 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. - */ - -#include "cartographer/mapping/global_trajectory_builder_interface.h" - -namespace cartographer { -namespace mapping { - -GlobalTrajectoryBuilderInterface::PoseEstimate::PoseEstimate( - const common::Time time, const kalman_filter::PoseAndCovariance& prediction, - const kalman_filter::PoseAndCovariance& observation, - const kalman_filter::PoseAndCovariance& estimate, - const transform::Rigid3d& pose, const sensor::PointCloud& point_cloud) - : time(time), - prediction(prediction), - observation(observation), - estimate(estimate), - pose(pose), - point_cloud(point_cloud) {} - -} // namespace mapping -} // namespace cartographer diff --git a/cartographer/mapping/global_trajectory_builder_interface.h b/cartographer/mapping/global_trajectory_builder_interface.h index 656595e..f29e4d8 100644 --- a/cartographer/mapping/global_trajectory_builder_interface.h +++ b/cartographer/mapping/global_trajectory_builder_interface.h @@ -24,6 +24,7 @@ #include "cartographer/common/time.h" #include "cartographer/kalman_filter/pose_tracker.h" #include "cartographer/mapping/submaps.h" +#include "cartographer/mapping/trajectory_builder.h" #include "cartographer/sensor/laser.h" #include "cartographer/sensor/point_cloud.h" @@ -36,34 +37,7 @@ namespace mapping { // optimized pose estimates. class GlobalTrajectoryBuilderInterface { public: - // Represents a newly computed pose. Each of the following steps in the pose - // estimation pipeline are provided for debugging: - // - // 1. UKF prediction - // 2. Absolute pose observation (e.g. from scan matching) - // 3. UKF estimate after integrating any measurements - // - // Finally, 'pose' is the end-user visualization of orientation and - // 'point_cloud' is the point cloud, in the local map frame. - struct PoseEstimate { - PoseEstimate() = default; - PoseEstimate(common::Time time, - const kalman_filter::PoseAndCovariance& prediction, - const kalman_filter::PoseAndCovariance& observation, - const kalman_filter::PoseAndCovariance& estimate, - const transform::Rigid3d& pose, - const sensor::PointCloud& point_cloud); - - common::Time time = common::Time::min(); - kalman_filter::PoseAndCovariance prediction = { - transform::Rigid3d::Identity(), kalman_filter::PoseCovariance::Zero()}; - kalman_filter::PoseAndCovariance observation = { - transform::Rigid3d::Identity(), kalman_filter::PoseCovariance::Zero()}; - kalman_filter::PoseAndCovariance estimate = { - transform::Rigid3d::Identity(), kalman_filter::PoseCovariance::Zero()}; - transform::Rigid3d pose = transform::Rigid3d::Identity(); - sensor::PointCloud point_cloud; - }; + using PoseEstimate = TrajectoryBuilder::PoseEstimate; GlobalTrajectoryBuilderInterface() {} virtual ~GlobalTrajectoryBuilderInterface() {} diff --git a/cartographer/mapping/map_builder.h b/cartographer/mapping/map_builder.h index 512a594..a10402e 100644 --- a/cartographer/mapping/map_builder.h +++ b/cartographer/mapping/map_builder.h @@ -28,11 +28,9 @@ #include "cartographer/mapping/global_trajectory_builder_interface.h" #include "cartographer/mapping/proto/map_builder_options.pb.h" #include "cartographer/mapping/sparse_pose_graph.h" +#include "cartographer/mapping/submaps.h" #include "cartographer/mapping/trajectory_node.h" -#include "cartographer/mapping_2d/local_trajectory_builder.h" #include "cartographer/mapping_2d/sparse_pose_graph.h" -#include "cartographer/mapping_2d/submaps.h" -#include "cartographer/mapping_3d/proto/local_trajectory_builder_options.pb.h" #include "cartographer/mapping_3d/sparse_pose_graph.h" namespace cartographer { diff --git a/cartographer/mapping/trajectory_builder.h b/cartographer/mapping/trajectory_builder.h new file mode 100644 index 0000000..19b0175 --- /dev/null +++ b/cartographer/mapping/trajectory_builder.h @@ -0,0 +1,114 @@ +/* + * 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_MAPPING_TRAJECTORY_BUILDER_H_ +#define CARTOGRAPHER_MAPPING_TRAJECTORY_BUILDER_H_ + +#include +#include +#include + +#include "cartographer/common/make_unique.h" +#include "cartographer/common/port.h" +#include "cartographer/common/time.h" +#include "cartographer/kalman_filter/pose_tracker.h" +#include "cartographer/mapping/submaps.h" +#include "cartographer/sensor/data.h" +#include "cartographer/sensor/laser.h" +#include "cartographer/sensor/point_cloud.h" + +namespace cartographer { +namespace mapping { + +// This interface is used for both 2D and 3D SLAM. +class TrajectoryBuilder { + public: + // Represents a newly computed pose. Each of the following steps in the pose + // estimation pipeline are provided for debugging: + // + // 1. UKF prediction + // 2. Absolute pose observation (e.g. from scan matching) + // 3. UKF estimate after integrating any measurements + // + // Finally, 'pose' is the end-user visualization of orientation and + // 'point_cloud' is the point cloud, in the local map frame. + struct PoseEstimate { + PoseEstimate() = default; + PoseEstimate(common::Time time, + const kalman_filter::PoseAndCovariance& prediction, + const kalman_filter::PoseAndCovariance& observation, + const kalman_filter::PoseAndCovariance& estimate, + const transform::Rigid3d& pose, + const sensor::PointCloud& point_cloud) + : time(time), + prediction(prediction), + observation(observation), + estimate(estimate), + pose(pose), + point_cloud(point_cloud) {} + + common::Time time = common::Time::min(); + kalman_filter::PoseAndCovariance prediction = { + transform::Rigid3d::Identity(), kalman_filter::PoseCovariance::Zero()}; + kalman_filter::PoseAndCovariance observation = { + transform::Rigid3d::Identity(), kalman_filter::PoseCovariance::Zero()}; + kalman_filter::PoseAndCovariance estimate = { + transform::Rigid3d::Identity(), kalman_filter::PoseCovariance::Zero()}; + transform::Rigid3d pose = transform::Rigid3d::Identity(); + sensor::PointCloud point_cloud; + }; + + TrajectoryBuilder() {} + virtual ~TrajectoryBuilder() {} + + TrajectoryBuilder(const TrajectoryBuilder&) = delete; + TrajectoryBuilder& operator=(const TrajectoryBuilder&) = delete; + + virtual const Submaps* submaps() const = 0; + virtual Submaps* submaps() = 0; + virtual kalman_filter::PoseTracker* pose_tracker() const = 0; + virtual const PoseEstimate& pose_estimate() const = 0; + + virtual void AddSensorData(const string& sensor_id, + std::unique_ptr data) = 0; + + void AddLaserFan(const string& sensor_id, common::Time time, + const sensor::LaserFan& laser_fan) { + AddSensorData(sensor_id, + common::make_unique(time, laser_fan)); + } + + void AddImuData(const string& sensor_id, common::Time time, + const Eigen::Vector3d& linear_acceleration, + const Eigen::Vector3d& angular_velocity) { + AddSensorData(sensor_id, common::make_unique( + time, sensor::Data::Imu{linear_acceleration, + angular_velocity})); + } + + void AddOdometerPose(const string& sensor_id, common::Time time, + const transform::Rigid3d& pose, + const kalman_filter::PoseCovariance& covariance) { + AddSensorData(sensor_id, + common::make_unique( + time, sensor::Data::Odometry{pose, covariance})); + } +}; + +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_TRAJECTORY_BUILDER_H_ diff --git a/cartographer/mapping_2d/global_trajectory_builder.h b/cartographer/mapping_2d/global_trajectory_builder.h index ee3f4ce..136a6db 100644 --- a/cartographer/mapping_2d/global_trajectory_builder.h +++ b/cartographer/mapping_2d/global_trajectory_builder.h @@ -40,7 +40,8 @@ class GlobalTrajectoryBuilder const mapping::GlobalTrajectoryBuilderInterface::PoseEstimate& pose_estimate() const override; - // Handles approximately horizontal laser fans. + // Projects 'laser_fan' to 2D, and therefore should be approximately + // horizontal. void AddLaserFan(common::Time time, const sensor::LaserFan& laser_fan) override; void AddImuData(common::Time time, const Eigen::Vector3d& linear_acceleration,