local_trajectory_builder_2d.h
Go to the documentation of this file.
00001 /*
00002  * Copyright 2016 The Cartographer Authors
00003  *
00004  * Licensed under the Apache License, Version 2.0 (the "License");
00005  * you may not use this file except in compliance with the License.
00006  * You may obtain a copy of the License at
00007  *
00008  *      http://www.apache.org/licenses/LICENSE-2.0
00009  *
00010  * Unless required by applicable law or agreed to in writing, software
00011  * distributed under the License is distributed on an "AS IS" BASIS,
00012  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00013  * See the License for the specific language governing permissions and
00014  * limitations under the License.
00015  */
00016 
00017 #ifndef CARTOGRAPHER_MAPPING_INTERNAL_2D_LOCAL_TRAJECTORY_BUILDER_2D_H_
00018 #define CARTOGRAPHER_MAPPING_INTERNAL_2D_LOCAL_TRAJECTORY_BUILDER_2D_H_
00019 
00020 #include <chrono>
00021 #include <memory>
00022 
00023 #include "cartographer/common/time.h"
00024 #include "cartographer/mapping/2d/submap_2d.h"
00025 #include "cartographer/mapping/internal/2d/scan_matching/ceres_scan_matcher_2d.h"
00026 #include "cartographer/mapping/internal/2d/scan_matching/real_time_correlative_scan_matcher_2d.h"
00027 #include "cartographer/mapping/internal/motion_filter.h"
00028 #include "cartographer/mapping/internal/range_data_collator.h"
00029 #include "cartographer/mapping/pose_extrapolator.h"
00030 #include "cartographer/mapping/proto/2d/local_trajectory_builder_options_2d.pb.h"
00031 #include "cartographer/metrics/family_factory.h"
00032 #include "cartographer/sensor/imu_data.h"
00033 #include "cartographer/sensor/internal/voxel_filter.h"
00034 #include "cartographer/sensor/odometry_data.h"
00035 #include "cartographer/sensor/range_data.h"
00036 #include "cartographer/transform/rigid_transform.h"
00037 
00038 namespace cartographer {
00039 namespace mapping {
00040 
00041 // Wires up the local SLAM stack (i.e. pose extrapolator, scan matching, etc.)
00042 // without loop closure.
00043 // TODO(gaschler): Add test for this class similar to the 3D test.
00044 class LocalTrajectoryBuilder2D {
00045  public:
00046   struct InsertionResult {
00047     std::shared_ptr<const TrajectoryNode::Data> constant_data;
00048     std::vector<std::shared_ptr<const Submap2D>> insertion_submaps;
00049   };
00050   struct MatchingResult {
00051     common::Time time;
00052     transform::Rigid3d local_pose;
00053     sensor::RangeData range_data_in_local;
00054     // 'nullptr' if dropped by the motion filter.
00055     std::unique_ptr<const InsertionResult> insertion_result;
00056   };
00057 
00058   explicit LocalTrajectoryBuilder2D(
00059       const proto::LocalTrajectoryBuilderOptions2D& options,
00060       const std::vector<std::string>& expected_range_sensor_ids);
00061   ~LocalTrajectoryBuilder2D();
00062 
00063   LocalTrajectoryBuilder2D(const LocalTrajectoryBuilder2D&) = delete;
00064   LocalTrajectoryBuilder2D& operator=(const LocalTrajectoryBuilder2D&) = delete;
00065 
00066   // Returns 'MatchingResult' when range data accumulation completed,
00067   // otherwise 'nullptr'. Range data must be approximately horizontal
00068   // for 2D SLAM. `TimedPointCloudData::time` is when the last point in
00069   // `range_data` was acquired, `TimedPointCloudData::ranges` contains the
00070   // relative time of point with respect to `TimedPointCloudData::time`.
00071   std::unique_ptr<MatchingResult> AddRangeData(
00072       const std::string& sensor_id,
00073       const sensor::TimedPointCloudData& range_data);
00074   void AddImuData(const sensor::ImuData& imu_data);
00075   void AddOdometryData(const sensor::OdometryData& odometry_data);
00076 
00077   static void RegisterMetrics(metrics::FamilyFactory* family_factory);
00078 
00079  private:
00080   std::unique_ptr<MatchingResult> AddAccumulatedRangeData(
00081       common::Time time, const sensor::RangeData& gravity_aligned_range_data,
00082       const transform::Rigid3d& gravity_alignment,
00083       const absl::optional<common::Duration>& sensor_duration);
00084   sensor::RangeData TransformToGravityAlignedFrameAndFilter(
00085       const transform::Rigid3f& transform_to_gravity_aligned_frame,
00086       const sensor::RangeData& range_data) const;
00087   std::unique_ptr<InsertionResult> InsertIntoSubmap(
00088       common::Time time, const sensor::RangeData& range_data_in_local,
00089       const sensor::PointCloud& filtered_gravity_aligned_point_cloud,
00090       const transform::Rigid3d& pose_estimate,
00091       const Eigen::Quaterniond& gravity_alignment);
00092 
00093   // Scan matches 'filtered_gravity_aligned_point_cloud' and returns the
00094   // observed pose, or nullptr on failure.
00095   std::unique_ptr<transform::Rigid2d> ScanMatch(
00096       common::Time time, const transform::Rigid2d& pose_prediction,
00097       const sensor::PointCloud& filtered_gravity_aligned_point_cloud);
00098 
00099   // Lazily constructs a PoseExtrapolator.
00100   void InitializeExtrapolator(common::Time time);
00101 
00102   const proto::LocalTrajectoryBuilderOptions2D options_;
00103   ActiveSubmaps2D active_submaps_;
00104 
00105   MotionFilter motion_filter_;
00106   scan_matching::RealTimeCorrelativeScanMatcher2D
00107       real_time_correlative_scan_matcher_;
00108   scan_matching::CeresScanMatcher2D ceres_scan_matcher_;
00109 
00110   std::unique_ptr<PoseExtrapolator> extrapolator_;
00111 
00112   int num_accumulated_ = 0;
00113   sensor::RangeData accumulated_range_data_;
00114 
00115   absl::optional<std::chrono::steady_clock::time_point> last_wall_time_;
00116   absl::optional<double> last_thread_cpu_time_seconds_;
00117   absl::optional<common::Time> last_sensor_time_;
00118 
00119   RangeDataCollator range_data_collator_;
00120 };
00121 
00122 }  // namespace mapping
00123 }  // namespace cartographer
00124 
00125 #endif  // CARTOGRAPHER_MAPPING_INTERNAL_2D_LOCAL_TRAJECTORY_BUILDER_2D_H_


cartographer
Author(s): The Cartographer Authors
autogenerated on Thu May 9 2019 02:27:35