00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017 #include "cartographer/mapping/internal/optimization/optimization_problem_2d.h"
00018
00019 #include <algorithm>
00020 #include <array>
00021 #include <cmath>
00022 #include <map>
00023 #include <memory>
00024 #include <string>
00025 #include <vector>
00026
00027 #include "cartographer/common/ceres_solver_options.h"
00028 #include "cartographer/common/histogram.h"
00029 #include "cartographer/common/math.h"
00030 #include "cartographer/mapping/internal/optimization/ceres_pose.h"
00031 #include "cartographer/mapping/internal/optimization/cost_functions/landmark_cost_function_2d.h"
00032 #include "cartographer/mapping/internal/optimization/cost_functions/spa_cost_function_2d.h"
00033 #include "cartographer/sensor/odometry_data.h"
00034 #include "cartographer/transform/transform.h"
00035 #include "ceres/ceres.h"
00036 #include "glog/logging.h"
00037
00038 namespace cartographer {
00039 namespace mapping {
00040 namespace optimization {
00041 namespace {
00042
00043 using ::cartographer::mapping::optimization::CeresPose;
00044 using LandmarkNode = ::cartographer::mapping::PoseGraphInterface::LandmarkNode;
00045
00046
00047
00048
00049 std::array<double, 3> FromPose(const transform::Rigid2d& pose) {
00050 return {{pose.translation().x(), pose.translation().y(),
00051 pose.normalized_angle()}};
00052 }
00053
00054
00055 transform::Rigid2d ToPose(const std::array<double, 3>& values) {
00056 return transform::Rigid2d({values[0], values[1]}, values[2]);
00057 }
00058
00059
00060
00061 transform::Rigid3d GetInitialLandmarkPose(
00062 const LandmarkNode::LandmarkObservation& observation,
00063 const NodeSpec2D& prev_node, const NodeSpec2D& next_node,
00064 const std::array<double, 3>& prev_node_pose,
00065 const std::array<double, 3>& next_node_pose) {
00066 const double interpolation_parameter =
00067 common::ToSeconds(observation.time - prev_node.time) /
00068 common::ToSeconds(next_node.time - prev_node.time);
00069
00070 const std::tuple<std::array<double, 4>, std::array<double, 3>>
00071 rotation_and_translation =
00072 InterpolateNodes2D(prev_node_pose.data(), prev_node.gravity_alignment,
00073 next_node_pose.data(), next_node.gravity_alignment,
00074 interpolation_parameter);
00075 return transform::Rigid3d::FromArrays(std::get<0>(rotation_and_translation),
00076 std::get<1>(rotation_and_translation)) *
00077 observation.landmark_to_tracking_transform;
00078 }
00079
00080 void AddLandmarkCostFunctions(
00081 const std::map<std::string, LandmarkNode>& landmark_nodes,
00082 const MapById<NodeId, NodeSpec2D>& node_data,
00083 MapById<NodeId, std::array<double, 3>>* C_nodes,
00084 std::map<std::string, CeresPose>* C_landmarks, ceres::Problem* problem,
00085 double huber_scale) {
00086 for (const auto& landmark_node : landmark_nodes) {
00087 for (const auto& observation : landmark_node.second.landmark_observations) {
00088 const std::string& landmark_id = landmark_node.first;
00089 const auto& begin_of_trajectory =
00090 node_data.BeginOfTrajectory(observation.trajectory_id);
00091
00092 if (observation.time < begin_of_trajectory->data.time) {
00093 continue;
00094 }
00095
00096 auto next =
00097 node_data.lower_bound(observation.trajectory_id, observation.time);
00098
00099
00100 if (next == node_data.EndOfTrajectory(observation.trajectory_id)) {
00101 continue;
00102 }
00103 if (next == begin_of_trajectory) {
00104 next = std::next(next);
00105 }
00106 auto prev = std::prev(next);
00107
00108 std::array<double, 3>* prev_node_pose = &C_nodes->at(prev->id);
00109 std::array<double, 3>* next_node_pose = &C_nodes->at(next->id);
00110 if (!C_landmarks->count(landmark_id)) {
00111 const transform::Rigid3d starting_point =
00112 landmark_node.second.global_landmark_pose.has_value()
00113 ? landmark_node.second.global_landmark_pose.value()
00114 : GetInitialLandmarkPose(observation, prev->data, next->data,
00115 *prev_node_pose, *next_node_pose);
00116 C_landmarks->emplace(
00117 landmark_id,
00118 CeresPose(starting_point, nullptr ,
00119 absl::make_unique<ceres::QuaternionParameterization>(),
00120 problem));
00121
00122 if (landmark_node.second.frozen) {
00123 problem->SetParameterBlockConstant(
00124 C_landmarks->at(landmark_id).translation());
00125 problem->SetParameterBlockConstant(
00126 C_landmarks->at(landmark_id).rotation());
00127 }
00128 }
00129 problem->AddResidualBlock(
00130 LandmarkCostFunction2D::CreateAutoDiffCostFunction(
00131 observation, prev->data, next->data),
00132 new ceres::HuberLoss(huber_scale), prev_node_pose->data(),
00133 next_node_pose->data(), C_landmarks->at(landmark_id).rotation(),
00134 C_landmarks->at(landmark_id).translation());
00135 }
00136 }
00137 }
00138
00139 }
00140
00141 OptimizationProblem2D::OptimizationProblem2D(
00142 const proto::OptimizationProblemOptions& options)
00143 : options_(options) {}
00144
00145 OptimizationProblem2D::~OptimizationProblem2D() {}
00146
00147 void OptimizationProblem2D::AddImuData(const int trajectory_id,
00148 const sensor::ImuData& imu_data) {
00149 imu_data_.Append(trajectory_id, imu_data);
00150 }
00151
00152 void OptimizationProblem2D::AddOdometryData(
00153 const int trajectory_id, const sensor::OdometryData& odometry_data) {
00154 odometry_data_.Append(trajectory_id, odometry_data);
00155 }
00156
00157 void OptimizationProblem2D::AddTrajectoryNode(const int trajectory_id,
00158 const NodeSpec2D& node_data) {
00159 node_data_.Append(trajectory_id, node_data);
00160 }
00161
00162 void OptimizationProblem2D::InsertTrajectoryNode(const NodeId& node_id,
00163 const NodeSpec2D& node_data) {
00164 node_data_.Insert(node_id, node_data);
00165 }
00166
00167 void OptimizationProblem2D::TrimTrajectoryNode(const NodeId& node_id) {
00168 imu_data_.Trim(node_data_, node_id);
00169 odometry_data_.Trim(node_data_, node_id);
00170 node_data_.Trim(node_id);
00171 }
00172
00173 void OptimizationProblem2D::AddSubmap(
00174 const int trajectory_id, const transform::Rigid2d& global_submap_pose) {
00175 submap_data_.Append(trajectory_id, SubmapSpec2D{global_submap_pose});
00176 }
00177
00178 void OptimizationProblem2D::InsertSubmap(
00179 const SubmapId& submap_id, const transform::Rigid2d& global_submap_pose) {
00180 submap_data_.Insert(submap_id, SubmapSpec2D{global_submap_pose});
00181 }
00182
00183 void OptimizationProblem2D::TrimSubmap(const SubmapId& submap_id) {
00184 submap_data_.Trim(submap_id);
00185 }
00186
00187 void OptimizationProblem2D::SetMaxNumIterations(
00188 const int32 max_num_iterations) {
00189 options_.mutable_ceres_solver_options()->set_max_num_iterations(
00190 max_num_iterations);
00191 }
00192
00193 void OptimizationProblem2D::Solve(
00194 const std::vector<Constraint>& constraints,
00195 const std::map<int, PoseGraphInterface::TrajectoryState>&
00196 trajectories_state,
00197 const std::map<std::string, LandmarkNode>& landmark_nodes) {
00198 if (node_data_.empty()) {
00199
00200 return;
00201 }
00202
00203 std::set<int> frozen_trajectories;
00204 for (const auto& it : trajectories_state) {
00205 if (it.second == PoseGraphInterface::TrajectoryState::FROZEN) {
00206 frozen_trajectories.insert(it.first);
00207 }
00208 }
00209
00210 ceres::Problem::Options problem_options;
00211 ceres::Problem problem(problem_options);
00212
00213
00214
00215 MapById<SubmapId, std::array<double, 3>> C_submaps;
00216 MapById<NodeId, std::array<double, 3>> C_nodes;
00217 std::map<std::string, CeresPose> C_landmarks;
00218 bool first_submap = true;
00219 for (const auto& submap_id_data : submap_data_) {
00220 const bool frozen =
00221 frozen_trajectories.count(submap_id_data.id.trajectory_id) != 0;
00222 C_submaps.Insert(submap_id_data.id,
00223 FromPose(submap_id_data.data.global_pose));
00224 problem.AddParameterBlock(C_submaps.at(submap_id_data.id).data(), 3);
00225 if (first_submap || frozen) {
00226 first_submap = false;
00227
00228
00229 problem.SetParameterBlockConstant(C_submaps.at(submap_id_data.id).data());
00230 }
00231 }
00232 for (const auto& node_id_data : node_data_) {
00233 const bool frozen =
00234 frozen_trajectories.count(node_id_data.id.trajectory_id) != 0;
00235 C_nodes.Insert(node_id_data.id, FromPose(node_id_data.data.global_pose_2d));
00236 problem.AddParameterBlock(C_nodes.at(node_id_data.id).data(), 3);
00237 if (frozen) {
00238 problem.SetParameterBlockConstant(C_nodes.at(node_id_data.id).data());
00239 }
00240 }
00241
00242 for (const Constraint& constraint : constraints) {
00243 problem.AddResidualBlock(
00244 CreateAutoDiffSpaCostFunction(constraint.pose),
00245
00246 constraint.tag == Constraint::INTER_SUBMAP
00247 ? new ceres::HuberLoss(options_.huber_scale())
00248 : nullptr,
00249 C_submaps.at(constraint.submap_id).data(),
00250 C_nodes.at(constraint.node_id).data());
00251 }
00252
00253 AddLandmarkCostFunctions(landmark_nodes, node_data_, &C_nodes, &C_landmarks,
00254 &problem, options_.huber_scale());
00255
00256
00257 for (auto node_it = node_data_.begin(); node_it != node_data_.end();) {
00258 const int trajectory_id = node_it->id.trajectory_id;
00259 const auto trajectory_end = node_data_.EndOfTrajectory(trajectory_id);
00260 if (frozen_trajectories.count(trajectory_id) != 0) {
00261 node_it = trajectory_end;
00262 continue;
00263 }
00264
00265 auto prev_node_it = node_it;
00266 for (++node_it; node_it != trajectory_end; ++node_it) {
00267 const NodeId first_node_id = prev_node_it->id;
00268 const NodeSpec2D& first_node_data = prev_node_it->data;
00269 prev_node_it = node_it;
00270 const NodeId second_node_id = node_it->id;
00271 const NodeSpec2D& second_node_data = node_it->data;
00272
00273 if (second_node_id.node_index != first_node_id.node_index + 1) {
00274 continue;
00275 }
00276
00277
00278 std::unique_ptr<transform::Rigid3d> relative_odometry =
00279 CalculateOdometryBetweenNodes(trajectory_id, first_node_data,
00280 second_node_data);
00281 if (relative_odometry != nullptr) {
00282 problem.AddResidualBlock(
00283 CreateAutoDiffSpaCostFunction(Constraint::Pose{
00284 *relative_odometry, options_.odometry_translation_weight(),
00285 options_.odometry_rotation_weight()}),
00286 nullptr , C_nodes.at(first_node_id).data(),
00287 C_nodes.at(second_node_id).data());
00288 }
00289
00290
00291 const transform::Rigid3d relative_local_slam_pose =
00292 transform::Embed3D(first_node_data.local_pose_2d.inverse() *
00293 second_node_data.local_pose_2d);
00294 problem.AddResidualBlock(
00295 CreateAutoDiffSpaCostFunction(
00296 Constraint::Pose{relative_local_slam_pose,
00297 options_.local_slam_pose_translation_weight(),
00298 options_.local_slam_pose_rotation_weight()}),
00299 nullptr , C_nodes.at(first_node_id).data(),
00300 C_nodes.at(second_node_id).data());
00301 }
00302 }
00303
00304
00305 ceres::Solver::Summary summary;
00306 ceres::Solve(
00307 common::CreateCeresSolverOptions(options_.ceres_solver_options()),
00308 &problem, &summary);
00309 if (options_.log_solver_summary()) {
00310 LOG(INFO) << summary.FullReport();
00311 }
00312
00313
00314 for (const auto& C_submap_id_data : C_submaps) {
00315 submap_data_.at(C_submap_id_data.id).global_pose =
00316 ToPose(C_submap_id_data.data);
00317 }
00318 for (const auto& C_node_id_data : C_nodes) {
00319 node_data_.at(C_node_id_data.id).global_pose_2d =
00320 ToPose(C_node_id_data.data);
00321 }
00322 for (const auto& C_landmark : C_landmarks) {
00323 landmark_data_[C_landmark.first] = C_landmark.second.ToRigid();
00324 }
00325 }
00326
00327 std::unique_ptr<transform::Rigid3d> OptimizationProblem2D::InterpolateOdometry(
00328 const int trajectory_id, const common::Time time) const {
00329 const auto it = odometry_data_.lower_bound(trajectory_id, time);
00330 if (it == odometry_data_.EndOfTrajectory(trajectory_id)) {
00331 return nullptr;
00332 }
00333 if (it == odometry_data_.BeginOfTrajectory(trajectory_id)) {
00334 if (it->time == time) {
00335 return absl::make_unique<transform::Rigid3d>(it->pose);
00336 }
00337 return nullptr;
00338 }
00339 const auto prev_it = std::prev(it);
00340 return absl::make_unique<transform::Rigid3d>(
00341 Interpolate(transform::TimestampedTransform{prev_it->time, prev_it->pose},
00342 transform::TimestampedTransform{it->time, it->pose}, time)
00343 .transform);
00344 }
00345
00346 std::unique_ptr<transform::Rigid3d>
00347 OptimizationProblem2D::CalculateOdometryBetweenNodes(
00348 const int trajectory_id, const NodeSpec2D& first_node_data,
00349 const NodeSpec2D& second_node_data) const {
00350 if (odometry_data_.HasTrajectory(trajectory_id)) {
00351 const std::unique_ptr<transform::Rigid3d> first_node_odometry =
00352 InterpolateOdometry(trajectory_id, first_node_data.time);
00353 const std::unique_ptr<transform::Rigid3d> second_node_odometry =
00354 InterpolateOdometry(trajectory_id, second_node_data.time);
00355 if (first_node_odometry != nullptr && second_node_odometry != nullptr) {
00356 transform::Rigid3d relative_odometry =
00357 transform::Rigid3d::Rotation(first_node_data.gravity_alignment) *
00358 first_node_odometry->inverse() * (*second_node_odometry) *
00359 transform::Rigid3d::Rotation(
00360 second_node_data.gravity_alignment.inverse());
00361 return absl::make_unique<transform::Rigid3d>(relative_odometry);
00362 }
00363 }
00364 return nullptr;
00365 }
00366
00367 }
00368 }
00369 }