mapping_2d/sparse_pose_graph/optimization_problem.cc
Go to the documentation of this file.
1 /*
2  * Copyright 2016 The Cartographer Authors
3  *
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
9  *
10  * Unless required by applicable law or agreed to in writing, software
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
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
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()));
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));
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);
137  }
138  }
139
140  // Add cost functions for intra- and inter-submap constraints.
141  for (const Constraint& constraint : constraints) {
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) {
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
void AddSubmap(int trajectory_id, const transform::Rigid2d &submap_pose)
proto::RangeDataInserterOptions options_
OptimizationProblem(const mapping::sparse_pose_graph::proto::OptimizationProblemOptions &options)
void Solve(const std::vector< Constraint > &constraints)
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
const std::vector< std::vector< SubmapData > > & submap_data() const
const std::vector< std::vector< NodeData > > & node_data() const
std::vector< std::deque< mapping_3d::ImuData > > imu_data_
std::vector< std::vector< SubmapData > > submap_data_
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