optimizing_local_trajectory_builder.h
Go to the documentation of this file.
1 /*
2  * Copyright 2016 The Cartographer Authors
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  * http://www.apache.org/licenses/LICENSE-2.0
9  *
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  */
16 
17 #ifndef CARTOGRAPHER_MAPPING_3D_OPTIMIZING_LOCAL_TRAJECTORY_BUILDER_H_
18 #define CARTOGRAPHER_MAPPING_3D_OPTIMIZING_LOCAL_TRAJECTORY_BUILDER_H_
19 
20 #include <array>
21 #include <deque>
22 #include <memory>
23 
29 #include "cartographer/mapping_3d/proto/local_trajectory_builder_options.pb.h"
34 
35 namespace cartographer {
36 namespace mapping_3d {
37 
38 // Batches up some sensor data and optimizes them in one go to get a locally
39 // consistent trajectory.
42  public:
44  const proto::LocalTrajectoryBuilderOptions& options);
46 
48  delete;
50  const OptimizingLocalTrajectoryBuilder&) = delete;
51 
52  void AddImuData(common::Time time, const Eigen::Vector3d& linear_acceleration,
53  const Eigen::Vector3d& angular_velocity) override;
54  std::unique_ptr<InsertionResult> AddRangefinderData(
55  common::Time time, const Eigen::Vector3f& origin,
56  const sensor::PointCloud& ranges) override;
57  void AddOdometerData(common::Time time,
58  const transform::Rigid3d& pose) override;
59  mapping_3d::Submaps* submaps() override;
60  const PoseEstimate& pose_estimate() const override;
61 
62  private:
63  struct State {
64  std::array<double, 3> translation;
65  std::array<double, 4> rotation; // Rotation quaternion as (w, x, y, z).
66  std::array<double, 3> velocity;
67 
68  State(const Eigen::Vector3d& translation,
69  const Eigen::Quaterniond& rotation, const Eigen::Vector3d& velocity)
70  : translation{{translation.x(), translation.y(), translation.z()}},
71  rotation{{rotation.w(), rotation.x(), rotation.y(), rotation.z()}},
72  velocity{{velocity.x(), velocity.y(), velocity.z()}} {}
73 
74  Eigen::Quaterniond ToQuaternion() const {
75  return Eigen::Quaterniond(rotation[0], rotation[1], rotation[2],
76  rotation[3]);
77  }
78 
80  return transform::Rigid3d(
81  Eigen::Vector3d(translation[0], translation[1], translation[2]),
82  ToQuaternion());
83  }
84  };
85 
86  struct Batch {
92  };
93 
94  struct OdometerData {
96 
97  // Dead-reckoning pose of the odometry.
99  };
100 
101  State PredictState(const State& start_state, const common::Time start_time,
102  const common::Time end_time);
103 
105 
106  std::unique_ptr<InsertionResult> AddAccumulatedRangeData(
107  common::Time time, const transform::Rigid3d& pose_observation,
108  const sensor::RangeData& range_data_in_tracking);
109 
110  std::unique_ptr<InsertionResult> InsertIntoSubmap(
111  const common::Time time, const sensor::RangeData& range_data_in_tracking,
112  const transform::Rigid3d& pose_observation);
113 
114  void TransformStates(const transform::Rigid3d& transform);
115  std::unique_ptr<InsertionResult> MaybeOptimize(common::Time time);
116 
117  const proto::LocalTrajectoryBuilderOptions options_;
118  const ceres::Solver::Options ceres_solver_options_;
119  std::unique_ptr<mapping_3d::Submaps> submaps_;
121 
122  std::deque<Batch> batches_;
123  double gravity_constant_ = 9.8;
124  std::deque<ImuData> imu_data_;
125  std::deque<OdometerData> odometer_data_;
126 
128 
130 };
131 
132 } // namespace mapping_3d
133 } // namespace cartographer
134 
135 #endif // CARTOGRAPHER_MAPPING_3D_OPTIMIZING_LOCAL_TRAJECTORY_BUILDER_H_
void AddImuData(common::Time time, const Eigen::Vector3d &linear_acceleration, const Eigen::Vector3d &angular_velocity) override
std::unique_ptr< InsertionResult > InsertIntoSubmap(const common::Time time, const sensor::RangeData &range_data_in_tracking, const transform::Rigid3d &pose_observation)
Rigid3< double > Rigid3d
State PredictState(const State &start_state, const common::Time start_time, const common::Time end_time)
std::unique_ptr< InsertionResult > AddRangefinderData(common::Time time, const Eigen::Vector3f &origin, const sensor::PointCloud &ranges) override
UniversalTimeScaleClock::time_point Time
Definition: time.h:44
transform::Rigid3d pose
void AddOdometerData(common::Time time, const transform::Rigid3d &pose) override
std::unique_ptr< InsertionResult > AddAccumulatedRangeData(common::Time time, const transform::Rigid3d &pose_observation, const sensor::RangeData &range_data_in_tracking)
std::vector< Eigen::Vector3f > PointCloud
Definition: point_cloud.h:30
std::unique_ptr< InsertionResult > MaybeOptimize(common::Time time)
State(const Eigen::Vector3d &translation, const Eigen::Quaterniond &rotation, const Eigen::Vector3d &velocity)
OptimizingLocalTrajectoryBuilder(const proto::LocalTrajectoryBuilderOptions &options)
OptimizingLocalTrajectoryBuilder & operator=(const OptimizingLocalTrajectoryBuilder &)=delete


cartographer
Author(s):
autogenerated on Wed Jun 5 2019 21:57:58