diff --git a/cartographer/mapping_2d/sparse_pose_graph.cc b/cartographer/mapping_2d/sparse_pose_graph.cc index 4778302..c32a56f 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.cc +++ b/cartographer/mapping_2d/sparse_pose_graph.cc @@ -238,7 +238,8 @@ void SparsePoseGraph::ComputeConstraintsForScan( GrowSubmapTransformsAsNeeded(trajectory_id, insertion_submaps); CHECK_EQ(submap_ids.size(), insertion_submaps.size()); const mapping::SubmapId matching_id = submap_ids.front(); - const int num_trimmed_submaps = optimization_problem_.num_trimmed_submaps(trajectory_id); + const int num_trimmed_submaps = + optimization_problem_.num_trimmed_submaps(trajectory_id); const transform::Rigid2d optimized_pose = optimization_problem_.submap_data() .at(matching_id.trajectory_id) @@ -461,9 +462,9 @@ std::vector SparsePoseGraph::constraints() { transform::Rigid3d SparsePoseGraph::GetLocalToGlobalTransform( const int trajectory_id) { common::MutexLocker locker(&mutex_); - return ComputeLocalToGlobalTransform(optimized_submap_transforms_, - num_trimmed_submaps_at_last_optimization_, - trajectory_id); + return ComputeLocalToGlobalTransform( + optimized_submap_transforms_, num_trimmed_submaps_at_last_optimization_, + trajectory_id); } std::vector> SparsePoseGraph::GetConnectedTrajectories() { diff --git a/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc b/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc index 1d8eafe..6c9ee32 100644 --- a/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc +++ b/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc @@ -134,7 +134,8 @@ void OptimizationProblem::Solve(const std::vector& constraints) { // Set the starting point. // TODO(hrapp): Move ceres data into SubmapData. - std::vector>> C_submaps(submap_data_.size()); + std::vector>> C_submaps( + submap_data_.size()); std::vector>> C_nodes(node_data_.size()); bool first_submap = true; for (size_t trajectory_id = 0; trajectory_id != submap_data_.size(); @@ -221,7 +222,8 @@ void OptimizationProblem::Solve(const std::vector& constraints) { for (size_t trajectory_id = 0; trajectory_id != submap_data_.size(); ++trajectory_id) { for (size_t submap_data_index = 0; - submap_data_index != submap_data_[trajectory_id].size(); ++submap_data_index) { + submap_data_index != submap_data_[trajectory_id].size(); + ++submap_data_index) { submap_data_[trajectory_id][submap_data_index].pose = ToPose(C_submaps[trajectory_id][submap_data_index]); } diff --git a/cartographer/mapping_3d/global_trajectory_builder.h b/cartographer/mapping_3d/global_trajectory_builder.h index 5668131..a0513b8 100644 --- a/cartographer/mapping_3d/global_trajectory_builder.h +++ b/cartographer/mapping_3d/global_trajectory_builder.h @@ -20,7 +20,7 @@ #include #include "cartographer/mapping/global_trajectory_builder_interface.h" -#include "cartographer/mapping_3d/local_trajectory_builder.h" +#include "cartographer/mapping_3d/kalman_local_trajectory_builder.h" #include "cartographer/mapping_3d/proto/local_trajectory_builder_options.pb.h" #include "cartographer/mapping_3d/sparse_pose_graph.h" @@ -49,7 +49,7 @@ class GlobalTrajectoryBuilder private: const int trajectory_id_; mapping_3d::SparsePoseGraph* const sparse_pose_graph_; - std::unique_ptr local_trajectory_builder_; + std::unique_ptr local_trajectory_builder_; }; } // namespace mapping_3d diff --git a/cartographer/mapping_3d/kalman_local_trajectory_builder.h b/cartographer/mapping_3d/kalman_local_trajectory_builder.h index 1c933ea..775270b 100644 --- a/cartographer/mapping_3d/kalman_local_trajectory_builder.h +++ b/cartographer/mapping_3d/kalman_local_trajectory_builder.h @@ -22,7 +22,6 @@ #include "cartographer/common/time.h" #include "cartographer/kalman_filter/pose_tracker.h" #include "cartographer/mapping/global_trajectory_builder_interface.h" -#include "cartographer/mapping_3d/local_trajectory_builder_interface.h" #include "cartographer/mapping_3d/motion_filter.h" #include "cartographer/mapping_3d/proto/local_trajectory_builder_options.pb.h" #include "cartographer/mapping_3d/scan_matching/ceres_scan_matcher.h" @@ -30,30 +29,39 @@ #include "cartographer/mapping_3d/submaps.h" #include "cartographer/sensor/range_data.h" #include "cartographer/sensor/voxel_filter.h" +#include "cartographer/transform/rigid_transform.h" namespace cartographer { namespace mapping_3d { // Wires up the local SLAM stack (i.e. UKF, scan matching, etc.) without loop // closure. -class KalmanLocalTrajectoryBuilder : public LocalTrajectoryBuilderInterface { +class KalmanLocalTrajectoryBuilder { public: + using PoseEstimate = mapping::GlobalTrajectoryBuilderInterface::PoseEstimate; + + struct InsertionResult { + common::Time time; + sensor::RangeData range_data_in_tracking; + transform::Rigid3d pose_observation; + std::vector> insertion_submaps; + }; + explicit KalmanLocalTrajectoryBuilder( const proto::LocalTrajectoryBuilderOptions& options); - ~KalmanLocalTrajectoryBuilder() override; + ~KalmanLocalTrajectoryBuilder(); KalmanLocalTrajectoryBuilder(const KalmanLocalTrajectoryBuilder&) = delete; KalmanLocalTrajectoryBuilder& operator=(const KalmanLocalTrajectoryBuilder&) = delete; void AddImuData(common::Time time, const Eigen::Vector3d& linear_acceleration, - const Eigen::Vector3d& angular_velocity) override; + const Eigen::Vector3d& angular_velocity); std::unique_ptr AddRangefinderData( common::Time time, const Eigen::Vector3f& origin, - const sensor::PointCloud& ranges) override; - void AddOdometerData(common::Time time, - const transform::Rigid3d& pose) override; - const PoseEstimate& pose_estimate() const override; + const sensor::PointCloud& ranges); + void AddOdometerData(common::Time time, const transform::Rigid3d& pose); + const PoseEstimate& pose_estimate() const; private: std::unique_ptr AddAccumulatedRangeData( diff --git a/cartographer/mapping_3d/local_trajectory_builder.cc b/cartographer/mapping_3d/local_trajectory_builder.cc index 25f65d3..2d093de 100644 --- a/cartographer/mapping_3d/local_trajectory_builder.cc +++ b/cartographer/mapping_3d/local_trajectory_builder.cc @@ -17,12 +17,11 @@ #include "cartographer/mapping_3d/local_trajectory_builder.h" #include "cartographer/common/make_unique.h" -#include "cartographer/mapping_3d/kalman_local_trajectory_builder.h" namespace cartographer { namespace mapping_3d { -std::unique_ptr CreateLocalTrajectoryBuilder( +std::unique_ptr CreateLocalTrajectoryBuilder( const proto::LocalTrajectoryBuilderOptions& local_trajectory_builder_options) { return common::make_unique( diff --git a/cartographer/mapping_3d/local_trajectory_builder.h b/cartographer/mapping_3d/local_trajectory_builder.h index 09ca7ec..5338452 100644 --- a/cartographer/mapping_3d/local_trajectory_builder.h +++ b/cartographer/mapping_3d/local_trajectory_builder.h @@ -20,13 +20,13 @@ #include #include -#include "cartographer/mapping_3d/local_trajectory_builder_interface.h" +#include "cartographer/mapping_3d/kalman_local_trajectory_builder.h" #include "cartographer/mapping_3d/proto/local_trajectory_builder_options.pb.h" namespace cartographer { namespace mapping_3d { -std::unique_ptr CreateLocalTrajectoryBuilder( +std::unique_ptr CreateLocalTrajectoryBuilder( const proto::LocalTrajectoryBuilderOptions& local_trajectory_builder_options); diff --git a/cartographer/mapping_3d/local_trajectory_builder_interface.h b/cartographer/mapping_3d/local_trajectory_builder_interface.h deleted file mode 100644 index f8db3b5..0000000 --- a/cartographer/mapping_3d/local_trajectory_builder_interface.h +++ /dev/null @@ -1,68 +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_LOCAL_TRAJECTORY_BUILDER_INTERFACE_H_ -#define CARTOGRAPHER_MAPPING_3D_LOCAL_TRAJECTORY_BUILDER_INTERFACE_H_ - -#include -#include - -#include "cartographer/common/time.h" -#include "cartographer/mapping/global_trajectory_builder_interface.h" -#include "cartographer/mapping_3d/submaps.h" -#include "cartographer/sensor/range_data.h" -#include "cartographer/transform/rigid_transform.h" - -namespace cartographer { -namespace mapping_3d { - -class LocalTrajectoryBuilderInterface { - public: - using PoseEstimate = mapping::GlobalTrajectoryBuilderInterface::PoseEstimate; - - struct InsertionResult { - common::Time time; - sensor::RangeData range_data_in_tracking; - transform::Rigid3d pose_observation; - std::vector> insertion_submaps; - }; - - virtual ~LocalTrajectoryBuilderInterface() {} - - LocalTrajectoryBuilderInterface(const LocalTrajectoryBuilderInterface&) = - delete; - LocalTrajectoryBuilderInterface& operator=( - const LocalTrajectoryBuilderInterface&) = delete; - - virtual void AddImuData(common::Time time, - const Eigen::Vector3d& linear_acceleration, - const Eigen::Vector3d& angular_velocity) = 0; - virtual std::unique_ptr AddRangefinderData( - common::Time time, const Eigen::Vector3f& origin, - const sensor::PointCloud& ranges) = 0; - virtual void AddOdometerData(common::Time time, - const transform::Rigid3d& pose) = 0; - - virtual const PoseEstimate& pose_estimate() const = 0; - - protected: - LocalTrajectoryBuilderInterface() {} -}; - -} // namespace mapping_3d -} // namespace cartographer - -#endif // CARTOGRAPHER_MAPPING_3D_LOCAL_TRAJECTORY_BUILDER_INTERFACE_H_