diff --git a/cartographer/mapping/global_trajectory_builder.h b/cartographer/mapping/global_trajectory_builder.h new file mode 100644 index 0000000..c45f7df --- /dev/null +++ b/cartographer/mapping/global_trajectory_builder.h @@ -0,0 +1,83 @@ +/* + * 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_MAPPING_GLOBAL_TRAJECTORY_BUILDER_H_ +#define CARTOGRAPHER_MAPPING_GLOBAL_TRAJECTORY_BUILDER_H_ + +#include "cartographer/mapping/global_trajectory_builder_interface.h" + +namespace cartographer { +namespace mapping { + +template +class GlobalTrajectoryBuilder + : public mapping::GlobalTrajectoryBuilderInterface { + public: + GlobalTrajectoryBuilder(const LocalTrajectoryBuilderOptions& options, + const int trajectory_id, + SparsePoseGraph* const sparse_pose_graph) + : trajectory_id_(trajectory_id), + sparse_pose_graph_(sparse_pose_graph), + local_trajectory_builder_(options) {} + ~GlobalTrajectoryBuilder() override {} + + GlobalTrajectoryBuilder(const GlobalTrajectoryBuilder&) = delete; + GlobalTrajectoryBuilder& operator=(const GlobalTrajectoryBuilder&) = delete; + + const mapping::PoseEstimate& pose_estimate() const override { + return local_trajectory_builder_.pose_estimate(); + } + + void AddRangefinderData(const common::Time time, + const Eigen::Vector3f& origin, + const sensor::PointCloud& ranges) override { + std::unique_ptr + insertion_result = local_trajectory_builder_.AddRangeData( + time, sensor::RangeData{origin, ranges, {}}); + if (insertion_result == nullptr) { + return; + } + sparse_pose_graph_->AddScan( + insertion_result->constant_data, insertion_result->pose_observation, + trajectory_id_, insertion_result->insertion_submaps); + } + + void AddSensorData(const sensor::ImuData& imu_data) override { + local_trajectory_builder_.AddImuData(imu_data); + sparse_pose_graph_->AddImuData(trajectory_id_, imu_data); + } + + void AddSensorData(const sensor::OdometryData& odometry_data) override { + local_trajectory_builder_.AddOdometerData(odometry_data); + sparse_pose_graph_->AddOdometerData(trajectory_id_, odometry_data); + } + + void AddSensorData( + const sensor::FixedFramePoseData& fixed_frame_pose) override { + sparse_pose_graph_->AddFixedFramePoseData(trajectory_id_, fixed_frame_pose); + } + + private: + const int trajectory_id_; + SparsePoseGraph* const sparse_pose_graph_; + LocalTrajectoryBuilder local_trajectory_builder_; +}; + +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_GLOBAL_TRAJECTORY_BUILDER_H_ diff --git a/cartographer/mapping/map_builder.cc b/cartographer/mapping/map_builder.cc index b0f2644..d913d3c 100644 --- a/cartographer/mapping/map_builder.cc +++ b/cartographer/mapping/map_builder.cc @@ -24,8 +24,9 @@ #include "cartographer/common/make_unique.h" #include "cartographer/mapping/collated_trajectory_builder.h" -#include "cartographer/mapping_2d/global_trajectory_builder.h" -#include "cartographer/mapping_3d/global_trajectory_builder.h" +#include "cartographer/mapping/global_trajectory_builder.h" +#include "cartographer/mapping_2d/local_trajectory_builder.h" +#include "cartographer/mapping_3d/local_trajectory_builder.h" #include "cartographer/sensor/range_data.h" #include "cartographer/sensor/voxel_filter.h" #include "cartographer/transform/rigid_transform.h" @@ -76,7 +77,10 @@ int MapBuilder::AddTrajectoryBuilder( trajectory_builders_.push_back( common::make_unique( &sensor_collator_, trajectory_id, expected_sensor_ids, - common::make_unique( + common::make_unique>( trajectory_options.trajectory_builder_3d_options(), trajectory_id, sparse_pose_graph_3d_.get()))); } else { @@ -84,7 +88,10 @@ int MapBuilder::AddTrajectoryBuilder( trajectory_builders_.push_back( common::make_unique( &sensor_collator_, trajectory_id, expected_sensor_ids, - common::make_unique( + common::make_unique>( trajectory_options.trajectory_builder_2d_options(), trajectory_id, sparse_pose_graph_2d_.get()))); } diff --git a/cartographer/mapping_2d/global_trajectory_builder.cc b/cartographer/mapping_2d/global_trajectory_builder.cc deleted file mode 100644 index 3d92b8f..0000000 --- a/cartographer/mapping_2d/global_trajectory_builder.cc +++ /dev/null @@ -1,66 +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_2d/global_trajectory_builder.h" - -namespace cartographer { -namespace mapping_2d { - -GlobalTrajectoryBuilder::GlobalTrajectoryBuilder( - const proto::LocalTrajectoryBuilderOptions& options, - const int trajectory_id, SparsePoseGraph* sparse_pose_graph) - : trajectory_id_(trajectory_id), - sparse_pose_graph_(sparse_pose_graph), - local_trajectory_builder_(options) {} - -GlobalTrajectoryBuilder::~GlobalTrajectoryBuilder() {} - -void GlobalTrajectoryBuilder::AddRangefinderData( - const common::Time time, const Eigen::Vector3f& origin, - const sensor::PointCloud& ranges) { - std::unique_ptr insertion_result = - local_trajectory_builder_.AddHorizontalRangeData( - time, sensor::RangeData{origin, ranges, {}}); - if (insertion_result == nullptr) { - return; - } - sparse_pose_graph_->AddScan( - insertion_result->constant_data, insertion_result->pose_estimate_2d, - trajectory_id_, insertion_result->insertion_submaps); -} - -void GlobalTrajectoryBuilder::AddSensorData(const sensor::ImuData& imu_data) { - local_trajectory_builder_.AddImuData(imu_data); - sparse_pose_graph_->AddImuData(trajectory_id_, imu_data); -} - -void GlobalTrajectoryBuilder::AddSensorData( - const sensor::OdometryData& odometry_data) { - local_trajectory_builder_.AddOdometerData(odometry_data); - sparse_pose_graph_->AddOdometerData(trajectory_id_, odometry_data); -} - -void GlobalTrajectoryBuilder::AddSensorData( - 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(); -} - -} // namespace mapping_2d -} // namespace cartographer diff --git a/cartographer/mapping_2d/global_trajectory_builder.h b/cartographer/mapping_2d/global_trajectory_builder.h deleted file mode 100644 index 6005e75..0000000 --- a/cartographer/mapping_2d/global_trajectory_builder.h +++ /dev/null @@ -1,59 +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. - */ - -#ifndef CARTOGRAPHER_MAPPING_2D_GLOBAL_TRAJECTORY_BUILDER_H_ -#define CARTOGRAPHER_MAPPING_2D_GLOBAL_TRAJECTORY_BUILDER_H_ - -#include "cartographer/mapping/global_trajectory_builder_interface.h" -#include "cartographer/mapping_2d/local_trajectory_builder.h" -#include "cartographer/mapping_2d/sparse_pose_graph.h" -#include "cartographer/mapping_3d/proto/local_trajectory_builder_options.pb.h" - -namespace cartographer { -namespace mapping_2d { - -class GlobalTrajectoryBuilder - : public mapping::GlobalTrajectoryBuilderInterface { - public: - GlobalTrajectoryBuilder(const proto::LocalTrajectoryBuilderOptions& options, - int trajectory_id, - SparsePoseGraph* sparse_pose_graph); - ~GlobalTrajectoryBuilder() override; - - GlobalTrajectoryBuilder(const GlobalTrajectoryBuilder&) = delete; - GlobalTrajectoryBuilder& operator=(const GlobalTrajectoryBuilder&) = delete; - - const mapping::PoseEstimate& pose_estimate() const override; - - // Projects 'ranges' into 2D. Therefore, 'ranges' should be approximately - // parallel to the ground plane. - void AddRangefinderData(common::Time time, const Eigen::Vector3f& origin, - const sensor::PointCloud& ranges) override; - void AddSensorData(const sensor::ImuData& imu_data) override; - void AddSensorData(const sensor::OdometryData& odometry_data) override; - void AddSensorData( - const sensor::FixedFramePoseData& fixed_frame_pose) override; - - private: - const int trajectory_id_; - SparsePoseGraph* const sparse_pose_graph_; - LocalTrajectoryBuilder local_trajectory_builder_; -}; - -} // namespace mapping_2d -} // namespace cartographer - -#endif // CARTOGRAPHER_MAPPING_2D_GLOBAL_TRAJECTORY_BUILDER_H_ diff --git a/cartographer/mapping_2d/local_trajectory_builder.cc b/cartographer/mapping_2d/local_trajectory_builder.cc index ba2680c..f8b8136 100644 --- a/cartographer/mapping_2d/local_trajectory_builder.cc +++ b/cartographer/mapping_2d/local_trajectory_builder.cc @@ -82,7 +82,7 @@ void LocalTrajectoryBuilder::ScanMatch( } std::unique_ptr -LocalTrajectoryBuilder::AddHorizontalRangeData( +LocalTrajectoryBuilder::AddRangeData( const common::Time time, const sensor::RangeData& range_data) { // Initialize extrapolator now if we do not ever use an IMU. if (!options_.use_imu_data()) { diff --git a/cartographer/mapping_2d/local_trajectory_builder.h b/cartographer/mapping_2d/local_trajectory_builder.h index b980c29..fa55667 100644 --- a/cartographer/mapping_2d/local_trajectory_builder.h +++ b/cartographer/mapping_2d/local_trajectory_builder.h @@ -42,7 +42,7 @@ class LocalTrajectoryBuilder { public: struct InsertionResult { std::shared_ptr constant_data; - transform::Rigid2d pose_estimate_2d; + transform::Rigid2d pose_observation; std::vector> insertion_submaps; }; @@ -54,7 +54,9 @@ class LocalTrajectoryBuilder { LocalTrajectoryBuilder& operator=(const LocalTrajectoryBuilder&) = delete; const mapping::PoseEstimate& pose_estimate() const; - std::unique_ptr AddHorizontalRangeData( + + // Range data must be approximately horizontal for 2D SLAM. + std::unique_ptr AddRangeData( common::Time, const sensor::RangeData& range_data); void AddImuData(const sensor::ImuData& imu_data); void AddOdometerData(const sensor::OdometryData& odometry_data); diff --git a/cartographer/mapping_3d/global_trajectory_builder.cc b/cartographer/mapping_3d/global_trajectory_builder.cc deleted file mode 100644 index 24062a6..0000000 --- a/cartographer/mapping_3d/global_trajectory_builder.cc +++ /dev/null @@ -1,64 +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_3d/global_trajectory_builder.h" - -namespace cartographer { -namespace mapping_3d { - -GlobalTrajectoryBuilder::GlobalTrajectoryBuilder( - const proto::LocalTrajectoryBuilderOptions& options, - const int trajectory_id, SparsePoseGraph* sparse_pose_graph) - : trajectory_id_(trajectory_id), - sparse_pose_graph_(sparse_pose_graph), - local_trajectory_builder_(options) {} - -GlobalTrajectoryBuilder::~GlobalTrajectoryBuilder() {} - -void GlobalTrajectoryBuilder::AddRangefinderData( - const common::Time time, const Eigen::Vector3f& origin, - const sensor::PointCloud& ranges) { - std::unique_ptr insertion_result = - local_trajectory_builder_.AddRangefinderData(time, origin, ranges); - if (insertion_result == nullptr) { - return; - } - sparse_pose_graph_->AddScan( - insertion_result->constant_data, insertion_result->pose_observation, - trajectory_id_, insertion_result->insertion_submaps); -} - -void GlobalTrajectoryBuilder::AddSensorData(const sensor::ImuData& imu_data) { - local_trajectory_builder_.AddImuData(imu_data); - sparse_pose_graph_->AddImuData(trajectory_id_, imu_data); -} - -void GlobalTrajectoryBuilder::AddSensorData( - const sensor::OdometryData& odometry_data) { - local_trajectory_builder_.AddOdometerData(odometry_data); -} - -void GlobalTrajectoryBuilder::AddSensorData( - 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(); -} - -} // namespace mapping_3d -} // namespace cartographer diff --git a/cartographer/mapping_3d/global_trajectory_builder.h b/cartographer/mapping_3d/global_trajectory_builder.h deleted file mode 100644 index d28389d..0000000 --- a/cartographer/mapping_3d/global_trajectory_builder.h +++ /dev/null @@ -1,57 +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. - */ - -#ifndef CARTOGRAPHER_MAPPING_3D_GLOBAL_TRAJECTORY_BUILDER_H_ -#define CARTOGRAPHER_MAPPING_3D_GLOBAL_TRAJECTORY_BUILDER_H_ - -#include "cartographer/mapping/global_trajectory_builder_interface.h" -#include "cartographer/mapping_3d/local_trajectory_builder.h" -#include "cartographer/mapping_3d/proto/local_trajectory_builder_options.pb.h" -#include "cartographer/mapping_3d/sparse_pose_graph.h" - -namespace cartographer { -namespace mapping_3d { - -class GlobalTrajectoryBuilder - : public mapping::GlobalTrajectoryBuilderInterface { - public: - GlobalTrajectoryBuilder(const proto::LocalTrajectoryBuilderOptions& options, - int trajectory_id, - SparsePoseGraph* sparse_pose_graph); - ~GlobalTrajectoryBuilder() override; - - GlobalTrajectoryBuilder(const GlobalTrajectoryBuilder&) = delete; - GlobalTrajectoryBuilder& operator=(const GlobalTrajectoryBuilder&) = delete; - - const mapping::PoseEstimate& pose_estimate() const override; - - void AddRangefinderData(common::Time time, const Eigen::Vector3f& origin, - const sensor::PointCloud& ranges) override; - void AddSensorData(const sensor::ImuData& imu_data) override; - void AddSensorData(const sensor::OdometryData& odometry_data) override; - void AddSensorData( - const sensor::FixedFramePoseData& fixed_frame_pose) override; - - private: - const int trajectory_id_; - SparsePoseGraph* const sparse_pose_graph_; - LocalTrajectoryBuilder local_trajectory_builder_; -}; - -} // namespace mapping_3d -} // namespace cartographer - -#endif // CARTOGRAPHER_MAPPING_3D_GLOBAL_TRAJECTORY_BUILDER_H_ diff --git a/cartographer/mapping_3d/local_trajectory_builder.cc b/cartographer/mapping_3d/local_trajectory_builder.cc index 41e2349..906f87b 100644 --- a/cartographer/mapping_3d/local_trajectory_builder.cc +++ b/cartographer/mapping_3d/local_trajectory_builder.cc @@ -58,9 +58,8 @@ void LocalTrajectoryBuilder::AddImuData(const sensor::ImuData& imu_data) { } std::unique_ptr -LocalTrajectoryBuilder::AddRangefinderData(const common::Time time, - const Eigen::Vector3f& origin, - const sensor::PointCloud& ranges) { +LocalTrajectoryBuilder::AddRangeData(const common::Time time, + const sensor::RangeData& range_data) { if (extrapolator_ == nullptr) { // Until we've initialized the extrapolator with our first IMU message, we // cannot compute the orientation of the rangefinder. @@ -77,8 +76,7 @@ LocalTrajectoryBuilder::AddRangefinderData(const common::Time time, first_pose_estimate_.inverse() * extrapolator_->ExtrapolatePose(time).cast(); const sensor::RangeData range_data_in_first_tracking = - sensor::TransformRangeData(sensor::RangeData{origin, ranges, {}}, - tracking_delta); + sensor::TransformRangeData(range_data, tracking_delta); for (const Eigen::Vector3f& hit : range_data_in_first_tracking.returns) { const Eigen::Vector3f delta = hit - range_data_in_first_tracking.origin; const float range = delta.norm(); diff --git a/cartographer/mapping_3d/local_trajectory_builder.h b/cartographer/mapping_3d/local_trajectory_builder.h index 5fd029d..66f3060 100644 --- a/cartographer/mapping_3d/local_trajectory_builder.h +++ b/cartographer/mapping_3d/local_trajectory_builder.h @@ -54,9 +54,8 @@ class LocalTrajectoryBuilder { LocalTrajectoryBuilder& operator=(const LocalTrajectoryBuilder&) = delete; void AddImuData(const sensor::ImuData& imu_data); - std::unique_ptr AddRangefinderData( - common::Time time, const Eigen::Vector3f& origin, - const sensor::PointCloud& ranges); + std::unique_ptr AddRangeData( + common::Time time, const sensor::RangeData& range_data); void AddOdometerData(const sensor::OdometryData& odometry_data); const mapping::PoseEstimate& pose_estimate() const; diff --git a/cartographer/mapping_3d/local_trajectory_builder_test.cc b/cartographer/mapping_3d/local_trajectory_builder_test.cc index 58052fe..5a2be7e 100644 --- a/cartographer/mapping_3d/local_trajectory_builder_test.cc +++ b/cartographer/mapping_3d/local_trajectory_builder_test.cc @@ -239,8 +239,10 @@ class LocalTrajectoryBuilderTest : public ::testing::Test { for (const TrajectoryNode& node : expected_trajectory) { AddLinearOnlyImuObservation(node.time, node.pose); const auto range_data = GenerateRangeData(node.pose); - if (local_trajectory_builder_->AddRangefinderData( - node.time, range_data.origin, range_data.returns) != nullptr) { + if (local_trajectory_builder_->AddRangeData( + node.time, + sensor::RangeData{range_data.origin, range_data.returns, {}}) != + nullptr) { const auto pose_estimate = local_trajectory_builder_->pose_estimate(); EXPECT_THAT(pose_estimate.pose, transform::IsNearly(node.pose, 1e-1)); ++num_poses; diff --git a/cartographer/mapping_3d/sparse_pose_graph.cc b/cartographer/mapping_3d/sparse_pose_graph.cc index a5196c0..37da179 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.cc +++ b/cartographer/mapping_3d/sparse_pose_graph.cc @@ -150,6 +150,13 @@ void SparsePoseGraph::AddImuData(const int trajectory_id, }); } +void SparsePoseGraph::AddOdometerData( + int trajectory_id, + const sensor::OdometryData& odometry_data) { + // TODO(cschuet): Add support for handling OdometryData in the optimization + // problem. +} + void SparsePoseGraph::AddFixedFramePoseData( const int trajectory_id, const sensor::FixedFramePoseData& fixed_frame_pose_data) { diff --git a/cartographer/mapping_3d/sparse_pose_graph.h b/cartographer/mapping_3d/sparse_pose_graph.h index a821fe2..0d3b2d1 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.h +++ b/cartographer/mapping_3d/sparse_pose_graph.h @@ -39,6 +39,7 @@ #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/odometry_data.h" #include "cartographer/sensor/point_cloud.h" #include "cartographer/transform/rigid_transform.h" #include "cartographer/transform/transform.h" @@ -76,6 +77,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { EXCLUDES(mutex_); 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);