mapping_2d/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 
32 #include "ceres/ceres.h"
33 #include "glog/logging.h"
34 
35 namespace cartographer {
36 namespace mapping_2d {
37 namespace sparse_pose_graph {
38 
39 namespace {
40 
41 // Converts a pose into the 3 optimization variable format used for Ceres:
42 // translation in x and y, followed by the rotation angle representing the
43 // orientation.
44 std::array<double, 3> FromPose(const transform::Rigid2d& pose) {
45  return {{pose.translation().x(), pose.translation().y(),
46  pose.normalized_angle()}};
47 }
48 
49 // Converts a pose as represented for Ceres back to an transform::Rigid2d pose.
50 transform::Rigid2d ToPose(const std::array<double, 3>& values) {
51  return transform::Rigid2d({values[0], values[1]}, values[2]);
52 }
53 
54 } // namespace
55 
57  const mapping::sparse_pose_graph::proto::OptimizationProblemOptions&
58  options)
59  : options_(options) {}
60 
62 
63 void OptimizationProblem::AddImuData(const int trajectory_id,
64  const common::Time time,
65  const Eigen::Vector3d& linear_acceleration,
66  const Eigen::Vector3d& angular_velocity) {
67  CHECK_GE(trajectory_id, 0);
68  imu_data_.resize(
69  std::max(imu_data_.size(), static_cast<size_t>(trajectory_id) + 1));
70  imu_data_[trajectory_id].push_back(
71  mapping_3d::ImuData{time, linear_acceleration, angular_velocity});
72 }
73 
75  const int trajectory_id, const common::Time time,
76  const transform::Rigid2d& initial_point_cloud_pose,
77  const transform::Rigid2d& point_cloud_pose) {
78  CHECK_GE(trajectory_id, 0);
79  node_data_.resize(
80  std::max(node_data_.size(), static_cast<size_t>(trajectory_id) + 1));
81  node_data_[trajectory_id].push_back(
82  NodeData{time, initial_point_cloud_pose, point_cloud_pose});
83 }
84 
85 void OptimizationProblem::AddSubmap(const int trajectory_id,
86  const transform::Rigid2d& submap_pose) {
87  CHECK_GE(trajectory_id, 0);
88  submap_data_.resize(
89  std::max(submap_data_.size(), static_cast<size_t>(trajectory_id) + 1));
90  submap_data_[trajectory_id].push_back(SubmapData{submap_pose});
91 }
92 
93 void OptimizationProblem::SetMaxNumIterations(const int32 max_num_iterations) {
94  options_.mutable_ceres_solver_options()->set_max_num_iterations(
95  max_num_iterations);
96 }
97 
98 void OptimizationProblem::Solve(const std::vector<Constraint>& constraints) {
99  if (node_data_.empty()) {
100  // Nothing to optimize.
101  return;
102  }
103 
104  ceres::Problem::Options problem_options;
105  ceres::Problem problem(problem_options);
106 
107  // Set the starting point.
108  // TODO(hrapp): Move ceres data into SubmapData.
109  std::vector<std::deque<std::array<double, 3>>> C_submaps(submap_data_.size());
110  std::vector<std::deque<std::array<double, 3>>> C_nodes(node_data_.size());
111  for (size_t trajectory_id = 0; trajectory_id != submap_data_.size();
112  ++trajectory_id) {
113  for (size_t submap_index = 0;
114  submap_index != submap_data_[trajectory_id].size(); ++submap_index) {
115  if (trajectory_id == 0 && submap_index == 0) {
116  // Fix the pose of the first submap of the first trajectory.
117  C_submaps[trajectory_id].push_back(
118  FromPose(transform::Rigid2d::Identity()));
119  problem.AddParameterBlock(C_submaps[trajectory_id].back().data(), 3);
120  problem.SetParameterBlockConstant(
121  C_submaps[trajectory_id].back().data());
122  } else {
123  C_submaps[trajectory_id].push_back(
124  FromPose(submap_data_[trajectory_id][submap_index].pose));
125  problem.AddParameterBlock(C_submaps[trajectory_id].back().data(), 3);
126  }
127  }
128  }
129  for (size_t trajectory_id = 0; trajectory_id != node_data_.size();
130  ++trajectory_id) {
131  C_nodes[trajectory_id].resize(node_data_[trajectory_id].size());
132  for (size_t node_index = 0; node_index != node_data_[trajectory_id].size();
133  ++node_index) {
134  C_nodes[trajectory_id][node_index] =
135  FromPose(node_data_[trajectory_id][node_index].point_cloud_pose);
136  problem.AddParameterBlock(C_nodes[trajectory_id][node_index].data(), 3);
137  }
138  }
139 
140  // Add cost functions for intra- and inter-submap constraints.
141  for (const Constraint& constraint : constraints) {
142  problem.AddResidualBlock(
143  new ceres::AutoDiffCostFunction<SpaCostFunction, 3, 3, 3>(
144  new SpaCostFunction(constraint.pose)),
145  // Only loop closure constraints should have a loss function.
146  constraint.tag == Constraint::INTER_SUBMAP
147  ? new ceres::HuberLoss(options_.huber_scale())
148  : nullptr,
149  C_submaps.at(constraint.submap_id.trajectory_id)
150  .at(constraint.submap_id.submap_index)
151  .data(),
152  C_nodes.at(constraint.node_id.trajectory_id)
153  .at(constraint.node_id.node_index)
154  .data());
155  }
156 
157  // Add penalties for changes between consecutive scans.
158  for (size_t trajectory_id = 0; trajectory_id != node_data_.size();
159  ++trajectory_id) {
160  for (size_t node_index = 1; node_index < node_data_[trajectory_id].size();
161  ++node_index) {
162  problem.AddResidualBlock(
163  new ceres::AutoDiffCostFunction<SpaCostFunction, 3, 3, 3>(
165  transform::Embed3D(node_data_[trajectory_id][node_index - 1]
166  .initial_point_cloud_pose.inverse() *
167  node_data_[trajectory_id][node_index]
168  .initial_point_cloud_pose),
169  options_.consecutive_scan_translation_penalty_factor(),
170  options_.consecutive_scan_rotation_penalty_factor()})),
171  nullptr /* loss function */,
172  C_nodes[trajectory_id][node_index - 1].data(),
173  C_nodes[trajectory_id][node_index].data());
174  }
175  }
176 
177  // Solve.
178  ceres::Solver::Summary summary;
179  ceres::Solve(
180  common::CreateCeresSolverOptions(options_.ceres_solver_options()),
181  &problem, &summary);
182 
183  if (options_.log_solver_summary()) {
184  LOG(INFO) << summary.FullReport();
185  }
186 
187  // Store the result.
188  for (size_t trajectory_id = 0; trajectory_id != submap_data_.size();
189  ++trajectory_id) {
190  for (size_t submap_index = 0;
191  submap_index != submap_data_[trajectory_id].size(); ++submap_index) {
192  submap_data_[trajectory_id][submap_index].pose =
193  ToPose(C_submaps[trajectory_id][submap_index]);
194  }
195  }
196  for (size_t trajectory_id = 0; trajectory_id != node_data_.size();
197  ++trajectory_id) {
198  for (size_t node_index = 0; node_index != node_data_[trajectory_id].size();
199  ++node_index) {
200  node_data_[trajectory_id][node_index].point_cloud_pose =
201  ToPose(C_nodes[trajectory_id][node_index]);
202  }
203  }
204 }
205 
206 const std::vector<std::vector<NodeData>>& OptimizationProblem::node_data()
207  const {
208  return node_data_;
209 }
210 
211 const std::vector<std::vector<SubmapData>>& OptimizationProblem::submap_data()
212  const {
213  return submap_data_;
214 }
215 
216 } // namespace sparse_pose_graph
217 } // namespace mapping_2d
218 } // namespace cartographer
proto::RangeDataInserterOptions options_
OptimizationProblem(const mapping::sparse_pose_graph::proto::OptimizationProblemOptions &options)
Rigid3< T > Embed3D(const Rigid2< T > &transform)
Definition: transform.h:110
int32_t int32
Definition: port.h:30
UniversalTimeScaleClock::time_point Time
Definition: time.h:44
void AddImuData(int trajectory_id, common::Time time, const Eigen::Vector3d &linear_acceleration, const Eigen::Vector3d &angular_velocity)
transform::Rigid3d pose
static Rigid2< FloatType > Identity()
Rigid2< double > Rigid2d
ceres::Solver::Options CreateCeresSolverOptions(const proto::CeresSolverOptions &proto)
void AddTrajectoryNode(int trajectory_id, common::Time time, const transform::Rigid2d &initial_point_cloud_pose, const transform::Rigid2d &point_cloud_pose)


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