mapping_3d/sparse_pose_graph/optimization_problem.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 <algorithm>
20 #include <array>
21 #include <cmath>
22 #include <map>
23 #include <memory>
24 #include <string>
25 #include <vector>
26 
27 #include "Eigen/Core"
37 #include "ceres/ceres.h"
38 #include "ceres/jet.h"
39 #include "ceres/rotation.h"
40 #include "glog/logging.h"
41 
42 namespace cartographer {
43 namespace mapping_3d {
44 namespace sparse_pose_graph {
45 
46 namespace {
47 
48 struct ConstantYawQuaternionPlus {
49  template <typename T>
50  bool operator()(const T* x, const T* delta, T* x_plus_delta) const {
51  const T delta_norm =
52  ceres::sqrt(common::Pow2(delta[0]) + common::Pow2(delta[1]));
53  const T sin_delta_over_delta =
54  delta_norm < 1e-6 ? T(1.) : ceres::sin(delta_norm) / delta_norm;
55  T q_delta[4];
56  q_delta[0] = delta_norm < 1e-6 ? T(1.) : ceres::cos(delta_norm);
57  q_delta[1] = sin_delta_over_delta * delta[0];
58  q_delta[2] = sin_delta_over_delta * delta[1];
59  q_delta[3] = T(0.);
60  // We apply the 'delta' which is interpreted as an angle-axis rotation
61  // vector in the xy-plane of the submap frame. This way we can align to
62  // gravity because rotations around the z-axis in the submap frame do not
63  // change gravity alignment, while disallowing random rotations of the map
64  // that have nothing to do with gravity alignment (i.e. we disallow steps
65  // just changing "yaw" of the complete map).
66  ceres::QuaternionProduct(x, q_delta, x_plus_delta);
67  return true;
68  }
69 };
70 
71 } // namespace
72 
74  const mapping::sparse_pose_graph::proto::OptimizationProblemOptions&
75  options,
76  FixZ fix_z)
77  : options_(options), fix_z_(fix_z) {}
78 
80 
81 void OptimizationProblem::AddImuData(const int trajectory_id,
82  const common::Time time,
83  const Eigen::Vector3d& linear_acceleration,
84  const Eigen::Vector3d& angular_velocity) {
85  CHECK_GE(trajectory_id, 0);
86  imu_data_.resize(
87  std::max(imu_data_.size(), static_cast<size_t>(trajectory_id) + 1));
88  imu_data_[trajectory_id].push_back(
89  ImuData{time, linear_acceleration, angular_velocity});
90 }
91 
93  const int trajectory_id, const common::Time time,
94  const transform::Rigid3d& point_cloud_pose) {
95  CHECK_GE(trajectory_id, 0);
96  node_data_.resize(
97  std::max(node_data_.size(), static_cast<size_t>(trajectory_id) + 1));
98  node_data_[trajectory_id].push_back(NodeData{time, point_cloud_pose});
99 }
100 
101 void OptimizationProblem::AddSubmap(const int trajectory_id,
102  const transform::Rigid3d& submap_pose) {
103  CHECK_GE(trajectory_id, 0);
104  submap_data_.resize(
105  std::max(submap_data_.size(), static_cast<size_t>(trajectory_id) + 1));
106  submap_data_[trajectory_id].push_back(SubmapData{submap_pose});
107 }
108 
109 void OptimizationProblem::SetMaxNumIterations(const int32 max_num_iterations) {
110  options_.mutable_ceres_solver_options()->set_max_num_iterations(
111  max_num_iterations);
112 }
113 
114 void OptimizationProblem::Solve(const std::vector<Constraint>& constraints) {
115  if (node_data_.empty()) {
116  // Nothing to optimize.
117  return;
118  }
119 
120  ceres::Problem::Options problem_options;
121  ceres::Problem problem(problem_options);
122 
123  const auto translation_parameterization =
124  [this]() -> std::unique_ptr<ceres::LocalParameterization> {
125  return fix_z_ == FixZ::kYes
126  ? common::make_unique<ceres::SubsetParameterization>(
127  3, std::vector<int>{2})
128  : nullptr;
129  };
130 
131  // Set the starting point.
132  CHECK(!submap_data_.empty());
133  CHECK(!submap_data_[0].empty());
134  // TODO(hrapp): Move ceres data into SubmapData.
135  std::vector<std::deque<CeresPose>> C_submaps(submap_data_.size());
136  std::vector<std::deque<CeresPose>> C_nodes(node_data_.size());
137  for (size_t trajectory_id = 0; trajectory_id != submap_data_.size();
138  ++trajectory_id) {
139  for (size_t submap_index = 0;
140  submap_index != submap_data_[trajectory_id].size(); ++submap_index) {
141  if (trajectory_id == 0 && submap_index == 0) {
142  // Tie the first submap of the first trajectory to the origin.
143  C_submaps[trajectory_id].emplace_back(
144  transform::Rigid3d::Identity(), translation_parameterization(),
145  common::make_unique<ceres::AutoDiffLocalParameterization<
146  ConstantYawQuaternionPlus, 4, 2>>(),
147  &problem);
148  problem.SetParameterBlockConstant(
149  C_submaps[trajectory_id].back().translation());
150  } else {
151  C_submaps[trajectory_id].emplace_back(
152  submap_data_[trajectory_id][submap_index].pose,
153  translation_parameterization(),
154  common::make_unique<ceres::QuaternionParameterization>(), &problem);
155  }
156  }
157  }
158  for (size_t trajectory_id = 0; trajectory_id != node_data_.size();
159  ++trajectory_id) {
160  for (size_t node_index = 0; node_index != node_data_[trajectory_id].size();
161  ++node_index) {
162  C_nodes[trajectory_id].emplace_back(
163  node_data_[trajectory_id][node_index].point_cloud_pose,
164  translation_parameterization(),
165  common::make_unique<ceres::QuaternionParameterization>(), &problem);
166  }
167  }
168 
169  // Add cost functions for intra- and inter-submap constraints.
170  for (const Constraint& constraint : constraints) {
171  problem.AddResidualBlock(
172  new ceres::AutoDiffCostFunction<SpaCostFunction, 6, 4, 3, 4, 3>(
173  new SpaCostFunction(constraint.pose)),
174  // Only loop closure constraints should have a loss function.
175  constraint.tag == Constraint::INTER_SUBMAP
176  ? new ceres::HuberLoss(options_.huber_scale())
177  : nullptr,
178  C_submaps.at(constraint.submap_id.trajectory_id)
179  .at(constraint.submap_id.submap_index)
180  .rotation(),
181  C_submaps.at(constraint.submap_id.trajectory_id)
182  .at(constraint.submap_id.submap_index)
183  .translation(),
184  C_nodes.at(constraint.node_id.trajectory_id)
185  .at(constraint.node_id.node_index)
186  .rotation(),
187  C_nodes.at(constraint.node_id.trajectory_id)
188  .at(constraint.node_id.node_index)
189  .translation());
190  }
191 
192  // Add constraints based on IMU observations of angular velocities and
193  // linear acceleration.
194  for (size_t trajectory_id = 0; trajectory_id != node_data_.size();
195  ++trajectory_id) {
196  const std::deque<ImuData>& imu_data = imu_data_.at(trajectory_id);
197  CHECK(!imu_data.empty());
198  // TODO(whess): Add support for empty trajectories.
199  const auto& node_data = node_data_[trajectory_id];
200  CHECK(!node_data.empty());
201 
202  // Skip IMU data before the first node of this trajectory.
203  auto it = imu_data.cbegin();
204  while ((it + 1) != imu_data.cend() && (it + 1)->time <= node_data[0].time) {
205  ++it;
206  }
207 
208  for (size_t node_index = 1; node_index < node_data.size(); ++node_index) {
209  auto it2 = it;
210  const IntegrateImuResult<double> result =
211  IntegrateImu(imu_data, node_data[node_index - 1].time,
212  node_data[node_index].time, &it);
213  if (node_index + 1 < node_data.size()) {
214  const common::Time first_time = node_data[node_index - 1].time;
215  const common::Time second_time = node_data[node_index].time;
216  const common::Time third_time = node_data[node_index + 1].time;
217  const common::Duration first_duration = second_time - first_time;
218  const common::Duration second_duration = third_time - second_time;
219  const common::Time first_center = first_time + first_duration / 2;
220  const common::Time second_center = second_time + second_duration / 2;
221  const IntegrateImuResult<double> result_to_first_center =
222  IntegrateImu(imu_data, first_time, first_center, &it2);
223  const IntegrateImuResult<double> result_center_to_center =
224  IntegrateImu(imu_data, first_center, second_center, &it2);
225  // 'delta_velocity' is the change in velocity from the point in time
226  // halfway between the first and second poses to halfway between second
227  // and third pose. It is computed from IMU data and still contains a
228  // delta due to gravity. The orientation of this vector is in the IMU
229  // frame at the second pose.
230  const Eigen::Vector3d delta_velocity =
231  (result.delta_rotation.inverse() *
232  result_to_first_center.delta_rotation) *
233  result_center_to_center.delta_velocity;
234  problem.AddResidualBlock(
235  new ceres::AutoDiffCostFunction<AccelerationCostFunction, 3, 4, 3,
236  3, 3, 1>(
238  options_.acceleration_weight(), delta_velocity,
239  common::ToSeconds(first_duration),
240  common::ToSeconds(second_duration))),
241  nullptr, C_nodes[trajectory_id].at(node_index).rotation(),
242  C_nodes[trajectory_id].at(node_index - 1).translation(),
243  C_nodes[trajectory_id].at(node_index).translation(),
244  C_nodes[trajectory_id].at(node_index + 1).translation(),
246  }
247  problem.AddResidualBlock(
248  new ceres::AutoDiffCostFunction<RotationCostFunction, 3, 4, 4>(
249  new RotationCostFunction(options_.rotation_weight(),
250  result.delta_rotation)),
251  nullptr, C_nodes[trajectory_id].at(node_index - 1).rotation(),
252  C_nodes[trajectory_id].at(node_index).rotation());
253  }
254  }
255 
256  // Solve.
257  ceres::Solver::Summary summary;
258  ceres::Solve(
259  common::CreateCeresSolverOptions(options_.ceres_solver_options()),
260  &problem, &summary);
261 
262  if (options_.log_solver_summary()) {
263  LOG(INFO) << summary.FullReport();
264  LOG(INFO) << "Gravity was: " << gravity_constant_;
265  }
266 
267  // Store the result.
268  for (size_t trajectory_id = 0; trajectory_id != submap_data_.size();
269  ++trajectory_id) {
270  for (size_t submap_index = 0;
271  submap_index != submap_data_[trajectory_id].size(); ++submap_index) {
272  submap_data_[trajectory_id][submap_index].pose =
273  C_submaps[trajectory_id][submap_index].ToRigid();
274  }
275  }
276  for (size_t trajectory_id = 0; trajectory_id != node_data_.size();
277  ++trajectory_id) {
278  for (size_t node_index = 0; node_index != node_data_[trajectory_id].size();
279  ++node_index) {
280  node_data_[trajectory_id][node_index].point_cloud_pose =
281  C_nodes[trajectory_id][node_index].ToRigid();
282  }
283  }
284 }
285 
286 const std::vector<std::vector<NodeData>>& OptimizationProblem::node_data()
287  const {
288  return node_data_;
289 }
290 
291 const std::vector<std::vector<SubmapData>>& OptimizationProblem::submap_data()
292  const {
293  return submap_data_;
294 }
295 
296 } // namespace sparse_pose_graph
297 } // namespace mapping_3d
298 } // namespace cartographer
proto::RangeDataInserterOptions options_
constexpr T Pow2(T a)
Definition: math.h:50
void AddImuData(int trajectory_id, common::Time time, const Eigen::Vector3d &linear_acceleration, const Eigen::Vector3d &angular_velocity)
int32_t int32
Definition: port.h:30
UniversalTimeScaleClock::time_point Time
Definition: time.h:44
OptimizationProblem(const mapping::sparse_pose_graph::proto::OptimizationProblemOptions &options, FixZ fix_z)
transform::Rigid3d pose
UniversalTimeScaleClock::duration Duration
Definition: time.h:43
void AddTrajectoryNode(int trajectory_id, common::Time time, const transform::Rigid3d &point_cloud_pose)
ceres::Solver::Options CreateCeresSolverOptions(const proto::CeresSolverOptions &proto)
double ToSeconds(const Duration duration)
Definition: time.cc:29
_Unique_if< T >::_Single_object make_unique(Args &&...args)
Definition: make_unique.h:46
IntegrateImuResult< double > IntegrateImu(const std::deque< ImuData > &imu_data, const common::Time start_time, const common::Time end_time, std::deque< ImuData >::const_iterator *it)


cartographer
Author(s):
autogenerated on Mon Jun 10 2019 12:51:39