mapping_2d/local_trajectory_builder.cc
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 
18 
19 #include <limits>
20 
23 
24 namespace cartographer {
25 namespace mapping_2d {
26 
28  const proto::LocalTrajectoryBuilderOptions& options)
29  : options_(options),
30  submaps_(options.submaps_options()),
31  motion_filter_(options_.motion_filter_options()),
33  options_.real_time_correlative_scan_matcher_options()),
34  ceres_scan_matcher_(options_.ceres_scan_matcher_options()),
35  odometry_state_tracker_(options_.num_odometry_states()) {}
36 
38 
40 
42  const transform::Rigid3f& tracking_to_tracking_2d,
43  const sensor::RangeData& range_data) const {
44  // Drop any returns below the minimum range and convert returns beyond the
45  // maximum range into misses.
46  sensor::RangeData returns_and_misses{range_data.origin, {}, {}};
47  for (const Eigen::Vector3f& hit : range_data.returns) {
48  const float range = (hit - range_data.origin).norm();
49  if (range >= options_.min_range()) {
50  if (range <= options_.max_range()) {
51  returns_and_misses.returns.push_back(hit);
52  } else {
53  returns_and_misses.misses.push_back(
54  range_data.origin + options_.missing_data_ray_length() *
55  (hit - range_data.origin).normalized());
56  }
57  }
58  }
60  sensor::TransformRangeData(returns_and_misses, tracking_to_tracking_2d),
61  options_.min_z(), options_.max_z());
62  return sensor::RangeData{
63  cropped.origin,
64  sensor::VoxelFiltered(cropped.returns, options_.voxel_filter_size()),
65  sensor::VoxelFiltered(cropped.misses, options_.voxel_filter_size())};
66 }
67 
69  common::Time time, const transform::Rigid3d& pose_prediction,
70  const transform::Rigid3d& tracking_to_tracking_2d,
71  const sensor::RangeData& range_data_in_tracking_2d,
72  transform::Rigid3d* pose_observation) {
73  const ProbabilityGrid& probability_grid =
74  submaps_.Get(submaps_.matching_index())->probability_grid();
75  transform::Rigid2d pose_prediction_2d =
76  transform::Project2D(pose_prediction * tracking_to_tracking_2d.inverse());
77  // The online correlative scan matcher will refine the initial estimate for
78  // the Ceres scan matcher.
79  transform::Rigid2d initial_ceres_pose = pose_prediction_2d;
80  sensor::AdaptiveVoxelFilter adaptive_voxel_filter(
81  options_.adaptive_voxel_filter_options());
82  const sensor::PointCloud filtered_point_cloud_in_tracking_2d =
83  adaptive_voxel_filter.Filter(range_data_in_tracking_2d.returns);
84  if (options_.use_online_correlative_scan_matching()) {
86  pose_prediction_2d, filtered_point_cloud_in_tracking_2d,
87  probability_grid, &initial_ceres_pose);
88  }
89 
90  transform::Rigid2d tracking_2d_to_map;
91  ceres::Solver::Summary summary;
92  ceres_scan_matcher_.Match(pose_prediction_2d, initial_ceres_pose,
93  filtered_point_cloud_in_tracking_2d,
94  probability_grid, &tracking_2d_to_map, &summary);
95 
96  *pose_observation =
97  transform::Embed3D(tracking_2d_to_map) * tracking_to_tracking_2d;
98 }
99 
100 std::unique_ptr<LocalTrajectoryBuilder::InsertionResult>
102  const common::Time time, const sensor::RangeData& range_data) {
103  // Initialize IMU tracker now if we do not ever use an IMU.
104  if (!options_.use_imu_data()) {
105  InitializeImuTracker(time);
106  }
107 
108  if (imu_tracker_ == nullptr) {
109  // Until we've initialized the IMU tracker with our first IMU message, we
110  // cannot compute the orientation of the rangefinder.
111  LOG(INFO) << "ImuTracker not yet initialized.";
112  return nullptr;
113  }
114 
115  Predict(time);
116  const transform::Rigid3d odometry_prediction =
118  const transform::Rigid3d model_prediction = pose_estimate_;
119  // TODO(whess): Prefer IMU over odom orientation if configured?
120  const transform::Rigid3d& pose_prediction = odometry_prediction;
121 
122  // Computes the rotation without yaw, as defined by GetYaw().
123  const transform::Rigid3d tracking_to_tracking_2d =
125  Eigen::Quaterniond(Eigen::AngleAxisd(
126  -transform::GetYaw(pose_prediction), Eigen::Vector3d::UnitZ())) *
127  pose_prediction.rotation());
128 
129  const sensor::RangeData range_data_in_tracking_2d =
130  TransformAndFilterRangeData(tracking_to_tracking_2d.cast<float>(),
131  range_data);
132 
133  if (range_data_in_tracking_2d.returns.empty()) {
134  LOG(WARNING) << "Dropped empty horizontal range data.";
135  return nullptr;
136  }
137 
138  ScanMatch(time, pose_prediction, tracking_to_tracking_2d,
139  range_data_in_tracking_2d, &pose_estimate_);
140  odometry_correction_ = transform::Rigid3d::Identity();
142  // We add an odometry state, so that the correction from the scan matching
143  // is not removed by the next odometry data we get.
147  odometry_prediction.inverse() * pose_estimate_});
148  }
149 
150  // Improve the velocity estimate.
151  if (last_scan_match_time_ > common::Time::min() &&
152  time > last_scan_match_time_) {
153  const double delta_t = common::ToSeconds(time - last_scan_match_time_);
155  model_prediction.translation().head<2>()) /
156  delta_t;
157  }
159 
160  // Remove the untracked z-component which floats around 0 in the UKF.
161  const auto translation = pose_estimate_.translation();
163  transform::Rigid3d::Vector(translation.x(), translation.y(), 0.),
165 
166  const transform::Rigid3d tracking_2d_to_map =
167  pose_estimate_ * tracking_to_tracking_2d.inverse();
170  sensor::TransformPointCloud(range_data_in_tracking_2d.returns,
171  tracking_2d_to_map.cast<float>())};
172 
173  const transform::Rigid2d pose_estimate_2d =
174  transform::Project2D(tracking_2d_to_map);
175  if (motion_filter_.IsSimilar(time, transform::Embed3D(pose_estimate_2d))) {
176  return nullptr;
177  }
178 
179  const mapping::Submap* const matching_submap =
181  std::vector<const mapping::Submap*> insertion_submaps;
182  for (int insertion_index : submaps_.insertion_indices()) {
183  insertion_submaps.push_back(submaps_.Get(insertion_index));
184  }
186  TransformRangeData(range_data_in_tracking_2d,
187  transform::Embed3D(pose_estimate_2d.cast<float>())));
188 
189  return common::make_unique<InsertionResult>(InsertionResult{
190  time, matching_submap, insertion_submaps, tracking_to_tracking_2d,
191  range_data_in_tracking_2d, pose_estimate_2d});
192 }
193 
196  return last_pose_estimate_;
197 }
198 
200  const common::Time time, const Eigen::Vector3d& linear_acceleration,
201  const Eigen::Vector3d& angular_velocity) {
202  CHECK(options_.use_imu_data()) << "An unexpected IMU packet was added.";
203 
204  InitializeImuTracker(time);
205  Predict(time);
206  imu_tracker_->AddImuLinearAccelerationObservation(linear_acceleration);
207  imu_tracker_->AddImuAngularVelocityObservation(angular_velocity);
208 }
209 
211  const common::Time time, const transform::Rigid3d& odometer_pose) {
212  if (imu_tracker_ == nullptr) {
213  // Until we've initialized the IMU tracker we do not want to call Predict().
214  LOG(INFO) << "ImuTracker not yet initialized.";
215  return;
216  }
217 
218  Predict(time);
220  const auto& previous_odometry_state = odometry_state_tracker_.newest();
221  const transform::Rigid3d delta =
222  previous_odometry_state.odometer_pose.inverse() * odometer_pose;
223  const transform::Rigid3d new_pose =
224  previous_odometry_state.state_pose * delta;
226  }
228  {time, odometer_pose, pose_estimate_ * odometry_correction_});
229 }
230 
232  if (imu_tracker_ == nullptr) {
233  imu_tracker_ = common::make_unique<mapping::ImuTracker>(
234  options_.imu_gravity_time_constant(), time);
235  }
236 }
237 
239  CHECK(imu_tracker_ != nullptr);
240  CHECK_LE(time_, time);
241  const double last_yaw = transform::GetYaw(imu_tracker_->orientation());
242  imu_tracker_->Advance(time);
243  if (time_ > common::Time::min()) {
244  const double delta_t = common::ToSeconds(time - time_);
245  // Constant velocity model.
246  const Eigen::Vector3d translation =
248  delta_t *
249  Eigen::Vector3d(velocity_estimate_.x(), velocity_estimate_.y(), 0.);
250  // Use the current IMU tracker roll and pitch for gravity alignment, and
251  // apply its change in yaw.
252  const Eigen::Quaterniond rotation =
253  Eigen::AngleAxisd(
255  Eigen::Vector3d::UnitZ()) *
256  imu_tracker_->orientation();
257  pose_estimate_ = transform::Rigid3d(translation, rotation);
258  }
259  time_ = time;
260 }
261 
262 } // namespace mapping_2d
263 } // namespace cartographer
sensor::RangeData TransformAndFilterRangeData(const transform::Rigid3f &tracking_to_tracking_2d, const sensor::RangeData &range_data) const
proto::RangeDataInserterOptions options_
PointCloud TransformPointCloud(const PointCloud &point_cloud, const transform::Rigid3f &transform)
Definition: point_cloud.cc:25
const mapping::GlobalTrajectoryBuilderInterface::PoseEstimate & pose_estimate() const
const Quaternion & rotation() const
static Rigid3 Rotation(const AngleAxis &angle_axis)
std::unique_ptr< RealTimeCorrelativeScanMatcher > real_time_correlative_scan_matcher_
bool IsSimilar(common::Time time, const transform::Rigid3d &pose)
void AddOdometryState(const OdometryState &odometry_state)
void ScanMatch(common::Time time, const transform::Rigid3d &pose_prediction, const transform::Rigid3d &tracking_to_tracking_2d, const sensor::RangeData &range_data_in_tracking_2d, transform::Rigid3d *pose_observation)
Rigid3< double > Rigid3d
PointCloud VoxelFiltered(const PointCloud &point_cloud, const float size)
Definition: voxel_filter.cc:80
Rigid3< T > Embed3D(const Rigid2< T > &transform)
Definition: transform.h:110
Rigid2< OtherType > cast() const
std::vector< int > insertion_indices() const
Definition: submaps.cc:38
Rigid3< OtherType > cast() const
const Submap * Get(int index) const override
Definition: 2d/submaps.cc:175
UniversalTimeScaleClock::time_point Time
Definition: time.h:44
T GetYaw(const Eigen::Quaternion< T > &rotation)
Definition: transform.h:43
mapping::GlobalTrajectoryBuilderInterface::PoseEstimate last_pose_estimate_
RangeData TransformRangeData(const RangeData &range_data, const transform::Rigid3f &transform)
Definition: range_data.cc:41
std::unique_ptr< InsertionResult > AddHorizontalRangeData(common::Time, const sensor::RangeData &range_data)
Rigid2< T > Project2D(const Rigid3< T > &transform)
Definition: transform.h:103
PointCloud Filter(const PointCloud &point_cloud) const
void AddOdometerData(common::Time time, const transform::Rigid3d &pose)
scan_matching::RealTimeCorrelativeScanMatcher real_time_correlative_scan_matcher_
const Vector & translation() const
std::vector< Eigen::Vector3f > PointCloud
Definition: point_cloud.h:30
void AddImuData(common::Time time, const Eigen::Vector3d &linear_acceleration, const Eigen::Vector3d &angular_velocity)
bool hit
Definition: 3d/submaps.cc:36
void Match(const transform::Rigid2d &previous_pose, const transform::Rigid2d &initial_pose_estimate, const sensor::PointCloud &point_cloud, const ProbabilityGrid &probability_grid, transform::Rigid2d *pose_estimate, ceres::Solver::Summary *summary) const
double ToSeconds(const Duration duration)
Definition: time.cc:29
double Match(const transform::Rigid2d &initial_pose_estimate, const sensor::PointCloud &point_cloud, const ProbabilityGrid &probability_grid, transform::Rigid2d *pose_estimate) const
LocalTrajectoryBuilder(const proto::LocalTrajectoryBuilderOptions &options)
void InsertRangeData(const sensor::RangeData &range_data)
Definition: 2d/submaps.cc:162
RangeData CropRangeData(const RangeData &range_data, const float min_z, const float max_z)
Definition: range_data.cc:50
Eigen::Matrix< double, 3, 1 > Vector
std::unique_ptr< Submaps > submaps_
std::unique_ptr< CeresScanMatcher > ceres_scan_matcher_


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