00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017 #include "cartographer/mapping/internal/3d/scan_matching/fast_correlative_scan_matcher_3d.h"
00018
00019 #include <algorithm>
00020 #include <cmath>
00021 #include <functional>
00022 #include <limits>
00023
00024 #include "Eigen/Geometry"
00025 #include "absl/memory/memory.h"
00026 #include "cartographer/common/math.h"
00027 #include "cartographer/mapping/internal/3d/scan_matching/low_resolution_matcher.h"
00028 #include "cartographer/mapping/proto/scan_matching//fast_correlative_scan_matcher_options_3d.pb.h"
00029 #include "cartographer/transform/transform.h"
00030 #include "glog/logging.h"
00031
00032 namespace cartographer {
00033 namespace mapping {
00034 namespace scan_matching {
00035
00036 proto::FastCorrelativeScanMatcherOptions3D
00037 CreateFastCorrelativeScanMatcherOptions3D(
00038 common::LuaParameterDictionary* const parameter_dictionary) {
00039 proto::FastCorrelativeScanMatcherOptions3D options;
00040 options.set_branch_and_bound_depth(
00041 parameter_dictionary->GetInt("branch_and_bound_depth"));
00042 options.set_full_resolution_depth(
00043 parameter_dictionary->GetInt("full_resolution_depth"));
00044 options.set_min_rotational_score(
00045 parameter_dictionary->GetDouble("min_rotational_score"));
00046 options.set_min_low_resolution_score(
00047 parameter_dictionary->GetDouble("min_low_resolution_score"));
00048 options.set_linear_xy_search_window(
00049 parameter_dictionary->GetDouble("linear_xy_search_window"));
00050 options.set_linear_z_search_window(
00051 parameter_dictionary->GetDouble("linear_z_search_window"));
00052 options.set_angular_search_window(
00053 parameter_dictionary->GetDouble("angular_search_window"));
00054 return options;
00055 }
00056
00057 PrecomputationGridStack3D::PrecomputationGridStack3D(
00058 const HybridGrid& hybrid_grid,
00059 const proto::FastCorrelativeScanMatcherOptions3D& options) {
00060 CHECK_GE(options.branch_and_bound_depth(), 1);
00061 CHECK_GE(options.full_resolution_depth(), 1);
00062 precomputation_grids_.reserve(options.branch_and_bound_depth());
00063 precomputation_grids_.push_back(ConvertToPrecomputationGrid(hybrid_grid));
00064 Eigen::Array3i last_width = Eigen::Array3i::Ones();
00065 for (int depth = 1; depth != options.branch_and_bound_depth(); ++depth) {
00066 const bool half_resolution = depth >= options.full_resolution_depth();
00067 const Eigen::Array3i next_width = ((1 << depth) * Eigen::Array3i::Ones());
00068 const int full_voxels_per_high_resolution_voxel =
00069 1 << std::max(0, depth - options.full_resolution_depth());
00070 const Eigen::Array3i shift = (next_width - last_width +
00071 (full_voxels_per_high_resolution_voxel - 1)) /
00072 full_voxels_per_high_resolution_voxel;
00073 precomputation_grids_.push_back(
00074 PrecomputeGrid(precomputation_grids_.back(), half_resolution, shift));
00075 last_width = next_width;
00076 }
00077 }
00078
00079 struct DiscreteScan3D {
00080 transform::Rigid3f pose;
00081
00082 std::vector<std::vector<Eigen::Array3i>> cell_indices_per_depth;
00083 float rotational_score;
00084 };
00085
00086 struct Candidate3D {
00087 Candidate3D(const int scan_index, const Eigen::Array3i& offset)
00088 : scan_index(scan_index), offset(offset) {}
00089
00090 static Candidate3D Unsuccessful() {
00091 return Candidate3D(0, Eigen::Array3i::Zero());
00092 }
00093
00094
00095 int scan_index;
00096
00097
00098
00099
00100 Eigen::Array3i offset;
00101
00102
00103 float score = -std::numeric_limits<float>::infinity();
00104
00105
00106 float low_resolution_score = 0.f;
00107
00108 bool operator<(const Candidate3D& other) const { return score < other.score; }
00109 bool operator>(const Candidate3D& other) const { return score > other.score; }
00110 };
00111
00112 FastCorrelativeScanMatcher3D::FastCorrelativeScanMatcher3D(
00113 const HybridGrid& hybrid_grid,
00114 const HybridGrid* const low_resolution_hybrid_grid,
00115 const Eigen::VectorXf* rotational_scan_matcher_histogram,
00116 const proto::FastCorrelativeScanMatcherOptions3D& options)
00117 : options_(options),
00118 resolution_(hybrid_grid.resolution()),
00119 width_in_voxels_(hybrid_grid.grid_size()),
00120 precomputation_grid_stack_(
00121 absl::make_unique<PrecomputationGridStack3D>(hybrid_grid, options)),
00122 low_resolution_hybrid_grid_(low_resolution_hybrid_grid),
00123 rotational_scan_matcher_(rotational_scan_matcher_histogram) {}
00124
00125 FastCorrelativeScanMatcher3D::~FastCorrelativeScanMatcher3D() {}
00126
00127 std::unique_ptr<FastCorrelativeScanMatcher3D::Result>
00128 FastCorrelativeScanMatcher3D::Match(
00129 const transform::Rigid3d& global_node_pose,
00130 const transform::Rigid3d& global_submap_pose,
00131 const TrajectoryNode::Data& constant_data, const float min_score) const {
00132 const auto low_resolution_matcher = scan_matching::CreateLowResolutionMatcher(
00133 low_resolution_hybrid_grid_, &constant_data.low_resolution_point_cloud);
00134 const SearchParameters search_parameters{
00135 common::RoundToInt(options_.linear_xy_search_window() / resolution_),
00136 common::RoundToInt(options_.linear_z_search_window() / resolution_),
00137 options_.angular_search_window(), &low_resolution_matcher};
00138 return MatchWithSearchParameters(
00139 search_parameters, global_node_pose.cast<float>(),
00140 global_submap_pose.cast<float>(),
00141 constant_data.high_resolution_point_cloud,
00142 constant_data.rotational_scan_matcher_histogram,
00143 constant_data.gravity_alignment, min_score);
00144 }
00145
00146 std::unique_ptr<FastCorrelativeScanMatcher3D::Result>
00147 FastCorrelativeScanMatcher3D::MatchFullSubmap(
00148 const Eigen::Quaterniond& global_node_rotation,
00149 const Eigen::Quaterniond& global_submap_rotation,
00150 const TrajectoryNode::Data& constant_data, const float min_score) const {
00151 float max_point_distance = 0.f;
00152 for (const sensor::RangefinderPoint& point :
00153 constant_data.high_resolution_point_cloud) {
00154 max_point_distance = std::max(max_point_distance, point.position.norm());
00155 }
00156 const int linear_window_size =
00157 (width_in_voxels_ + 1) / 2 +
00158 common::RoundToInt(max_point_distance / resolution_ + 0.5f);
00159 const auto low_resolution_matcher = scan_matching::CreateLowResolutionMatcher(
00160 low_resolution_hybrid_grid_, &constant_data.low_resolution_point_cloud);
00161 const SearchParameters search_parameters{
00162 linear_window_size, linear_window_size, M_PI, &low_resolution_matcher};
00163 return MatchWithSearchParameters(
00164 search_parameters,
00165 transform::Rigid3f::Rotation(global_node_rotation.cast<float>()),
00166 transform::Rigid3f::Rotation(global_submap_rotation.cast<float>()),
00167 constant_data.high_resolution_point_cloud,
00168 constant_data.rotational_scan_matcher_histogram,
00169 constant_data.gravity_alignment, min_score);
00170 }
00171
00172 std::unique_ptr<FastCorrelativeScanMatcher3D::Result>
00173 FastCorrelativeScanMatcher3D::MatchWithSearchParameters(
00174 const FastCorrelativeScanMatcher3D::SearchParameters& search_parameters,
00175 const transform::Rigid3f& global_node_pose,
00176 const transform::Rigid3f& global_submap_pose,
00177 const sensor::PointCloud& point_cloud,
00178 const Eigen::VectorXf& rotational_scan_matcher_histogram,
00179 const Eigen::Quaterniond& gravity_alignment, const float min_score) const {
00180 const std::vector<DiscreteScan3D> discrete_scans = GenerateDiscreteScans(
00181 search_parameters, point_cloud, rotational_scan_matcher_histogram,
00182 gravity_alignment, global_node_pose, global_submap_pose);
00183
00184 const std::vector<Candidate3D> lowest_resolution_candidates =
00185 ComputeLowestResolutionCandidates(search_parameters, discrete_scans);
00186
00187 const Candidate3D best_candidate = BranchAndBound(
00188 search_parameters, discrete_scans, lowest_resolution_candidates,
00189 precomputation_grid_stack_->max_depth(), min_score);
00190 if (best_candidate.score > min_score) {
00191 return absl::make_unique<Result>(Result{
00192 best_candidate.score,
00193 GetPoseFromCandidate(discrete_scans, best_candidate).cast<double>(),
00194 discrete_scans[best_candidate.scan_index].rotational_score,
00195 best_candidate.low_resolution_score});
00196 }
00197 return nullptr;
00198 }
00199
00200 DiscreteScan3D FastCorrelativeScanMatcher3D::DiscretizeScan(
00201 const FastCorrelativeScanMatcher3D::SearchParameters& search_parameters,
00202 const sensor::PointCloud& point_cloud, const transform::Rigid3f& pose,
00203 const float rotational_score) const {
00204 std::vector<std::vector<Eigen::Array3i>> cell_indices_per_depth;
00205 const PrecomputationGrid3D& original_grid =
00206 precomputation_grid_stack_->Get(0);
00207 std::vector<Eigen::Array3i> full_resolution_cell_indices;
00208 for (const sensor::RangefinderPoint& point :
00209 sensor::TransformPointCloud(point_cloud, pose)) {
00210 full_resolution_cell_indices.push_back(
00211 original_grid.GetCellIndex(point.position));
00212 }
00213 const int full_resolution_depth = std::min(options_.full_resolution_depth(),
00214 options_.branch_and_bound_depth());
00215 CHECK_GE(full_resolution_depth, 1);
00216 for (int i = 0; i != full_resolution_depth; ++i) {
00217 cell_indices_per_depth.push_back(full_resolution_cell_indices);
00218 }
00219 const int low_resolution_depth =
00220 options_.branch_and_bound_depth() - full_resolution_depth;
00221 CHECK_GE(low_resolution_depth, 0);
00222 const Eigen::Array3i search_window_start(
00223 -search_parameters.linear_xy_window_size,
00224 -search_parameters.linear_xy_window_size,
00225 -search_parameters.linear_z_window_size);
00226 for (int i = 0; i != low_resolution_depth; ++i) {
00227 const int reduction_exponent = i + 1;
00228 const Eigen::Array3i low_resolution_search_window_start(
00229 search_window_start[0] >> reduction_exponent,
00230 search_window_start[1] >> reduction_exponent,
00231 search_window_start[2] >> reduction_exponent);
00232 cell_indices_per_depth.emplace_back();
00233 for (const Eigen::Array3i& cell_index : full_resolution_cell_indices) {
00234 const Eigen::Array3i cell_at_start = cell_index + search_window_start;
00235 const Eigen::Array3i low_resolution_cell_at_start(
00236 cell_at_start[0] >> reduction_exponent,
00237 cell_at_start[1] >> reduction_exponent,
00238 cell_at_start[2] >> reduction_exponent);
00239 cell_indices_per_depth.back().push_back(
00240 low_resolution_cell_at_start - low_resolution_search_window_start);
00241 }
00242 }
00243 return DiscreteScan3D{pose, cell_indices_per_depth, rotational_score};
00244 }
00245
00246 std::vector<DiscreteScan3D> FastCorrelativeScanMatcher3D::GenerateDiscreteScans(
00247 const FastCorrelativeScanMatcher3D::SearchParameters& search_parameters,
00248 const sensor::PointCloud& point_cloud,
00249 const Eigen::VectorXf& rotational_scan_matcher_histogram,
00250 const Eigen::Quaterniond& gravity_alignment,
00251 const transform::Rigid3f& global_node_pose,
00252 const transform::Rigid3f& global_submap_pose) const {
00253 std::vector<DiscreteScan3D> result;
00254
00255
00256 float max_scan_range = 3.f * resolution_;
00257 for (const sensor::RangefinderPoint& point : point_cloud) {
00258 const float range = point.position.norm();
00259 max_scan_range = std::max(range, max_scan_range);
00260 }
00261 const float kSafetyMargin = 1.f - 1e-2f;
00262 const float angular_step_size =
00263 kSafetyMargin * std::acos(1.f - common::Pow2(resolution_) /
00264 (2.f * common::Pow2(max_scan_range)));
00265 const int angular_window_size = common::RoundToInt(
00266 search_parameters.angular_search_window / angular_step_size);
00267 std::vector<float> angles;
00268 for (int rz = -angular_window_size; rz <= angular_window_size; ++rz) {
00269 angles.push_back(rz * angular_step_size);
00270 }
00271 const transform::Rigid3f node_to_submap =
00272 global_submap_pose.inverse() * global_node_pose;
00273 const std::vector<float> scores = rotational_scan_matcher_.Match(
00274 rotational_scan_matcher_histogram,
00275 transform::GetYaw(node_to_submap.rotation() *
00276 gravity_alignment.inverse().cast<float>()),
00277 angles);
00278 for (size_t i = 0; i != angles.size(); ++i) {
00279 if (scores[i] < options_.min_rotational_score()) {
00280 continue;
00281 }
00282 const Eigen::Vector3f angle_axis(0.f, 0.f, angles[i]);
00283
00284
00285
00286 const transform::Rigid3f pose(
00287 node_to_submap.translation(),
00288 global_submap_pose.rotation().inverse() *
00289 transform::AngleAxisVectorToRotationQuaternion(angle_axis) *
00290 global_node_pose.rotation());
00291 result.push_back(
00292 DiscretizeScan(search_parameters, point_cloud, pose, scores[i]));
00293 }
00294 return result;
00295 }
00296
00297 std::vector<Candidate3D>
00298 FastCorrelativeScanMatcher3D::GenerateLowestResolutionCandidates(
00299 const FastCorrelativeScanMatcher3D::SearchParameters& search_parameters,
00300 const int num_discrete_scans) const {
00301 const int linear_step_size = 1 << precomputation_grid_stack_->max_depth();
00302 const int num_lowest_resolution_linear_xy_candidates =
00303 (2 * search_parameters.linear_xy_window_size + linear_step_size) /
00304 linear_step_size;
00305 const int num_lowest_resolution_linear_z_candidates =
00306 (2 * search_parameters.linear_z_window_size + linear_step_size) /
00307 linear_step_size;
00308 const int num_candidates =
00309 num_discrete_scans *
00310 common::Power(num_lowest_resolution_linear_xy_candidates, 2) *
00311 num_lowest_resolution_linear_z_candidates;
00312 std::vector<Candidate3D> candidates;
00313 candidates.reserve(num_candidates);
00314 for (int scan_index = 0; scan_index != num_discrete_scans; ++scan_index) {
00315 for (int z = -search_parameters.linear_z_window_size;
00316 z <= search_parameters.linear_z_window_size; z += linear_step_size) {
00317 for (int y = -search_parameters.linear_xy_window_size;
00318 y <= search_parameters.linear_xy_window_size;
00319 y += linear_step_size) {
00320 for (int x = -search_parameters.linear_xy_window_size;
00321 x <= search_parameters.linear_xy_window_size;
00322 x += linear_step_size) {
00323 candidates.emplace_back(scan_index, Eigen::Array3i(x, y, z));
00324 }
00325 }
00326 }
00327 }
00328 CHECK_EQ(candidates.size(), num_candidates);
00329 return candidates;
00330 }
00331
00332 void FastCorrelativeScanMatcher3D::ScoreCandidates(
00333 const int depth, const std::vector<DiscreteScan3D>& discrete_scans,
00334 std::vector<Candidate3D>* const candidates) const {
00335 const int reduction_exponent =
00336 std::max(0, depth - options_.full_resolution_depth() + 1);
00337 for (Candidate3D& candidate : *candidates) {
00338 int sum = 0;
00339 const DiscreteScan3D& discrete_scan = discrete_scans[candidate.scan_index];
00340 const Eigen::Array3i offset(candidate.offset[0] >> reduction_exponent,
00341 candidate.offset[1] >> reduction_exponent,
00342 candidate.offset[2] >> reduction_exponent);
00343 CHECK_LT(depth, discrete_scan.cell_indices_per_depth.size());
00344 for (const Eigen::Array3i& cell_index :
00345 discrete_scan.cell_indices_per_depth[depth]) {
00346 const Eigen::Array3i proposed_cell_index = cell_index + offset;
00347 sum += precomputation_grid_stack_->Get(depth).value(proposed_cell_index);
00348 }
00349 candidate.score = PrecomputationGrid3D::ToProbability(
00350 sum /
00351 static_cast<float>(discrete_scan.cell_indices_per_depth[depth].size()));
00352 }
00353 std::sort(candidates->begin(), candidates->end(),
00354 std::greater<Candidate3D>());
00355 }
00356
00357 std::vector<Candidate3D>
00358 FastCorrelativeScanMatcher3D::ComputeLowestResolutionCandidates(
00359 const FastCorrelativeScanMatcher3D::SearchParameters& search_parameters,
00360 const std::vector<DiscreteScan3D>& discrete_scans) const {
00361 std::vector<Candidate3D> lowest_resolution_candidates =
00362 GenerateLowestResolutionCandidates(search_parameters,
00363 discrete_scans.size());
00364 ScoreCandidates(precomputation_grid_stack_->max_depth(), discrete_scans,
00365 &lowest_resolution_candidates);
00366 return lowest_resolution_candidates;
00367 }
00368
00369 transform::Rigid3f FastCorrelativeScanMatcher3D::GetPoseFromCandidate(
00370 const std::vector<DiscreteScan3D>& discrete_scans,
00371 const Candidate3D& candidate) const {
00372 return transform::Rigid3f::Translation(
00373 resolution_ * candidate.offset.matrix().cast<float>()) *
00374 discrete_scans[candidate.scan_index].pose;
00375 }
00376
00377 Candidate3D FastCorrelativeScanMatcher3D::BranchAndBound(
00378 const FastCorrelativeScanMatcher3D::SearchParameters& search_parameters,
00379 const std::vector<DiscreteScan3D>& discrete_scans,
00380 const std::vector<Candidate3D>& candidates, const int candidate_depth,
00381 float min_score) const {
00382 if (candidate_depth == 0) {
00383 for (const Candidate3D& candidate : candidates) {
00384 if (candidate.score <= min_score) {
00385
00386
00387 return Candidate3D::Unsuccessful();
00388 }
00389 const float low_resolution_score =
00390 (*search_parameters.low_resolution_matcher)(
00391 GetPoseFromCandidate(discrete_scans, candidate));
00392 if (low_resolution_score >= options_.min_low_resolution_score()) {
00393
00394 Candidate3D best_candidate = candidate;
00395 best_candidate.low_resolution_score = low_resolution_score;
00396 return best_candidate;
00397 }
00398 }
00399
00400
00401 return Candidate3D::Unsuccessful();
00402 }
00403
00404 Candidate3D best_high_resolution_candidate = Candidate3D::Unsuccessful();
00405 best_high_resolution_candidate.score = min_score;
00406 for (const Candidate3D& candidate : candidates) {
00407 if (candidate.score <= min_score) {
00408 break;
00409 }
00410 std::vector<Candidate3D> higher_resolution_candidates;
00411 const int half_width = 1 << (candidate_depth - 1);
00412 for (int z : {0, half_width}) {
00413 if (candidate.offset.z() + z > search_parameters.linear_z_window_size) {
00414 break;
00415 }
00416 for (int y : {0, half_width}) {
00417 if (candidate.offset.y() + y >
00418 search_parameters.linear_xy_window_size) {
00419 break;
00420 }
00421 for (int x : {0, half_width}) {
00422 if (candidate.offset.x() + x >
00423 search_parameters.linear_xy_window_size) {
00424 break;
00425 }
00426 higher_resolution_candidates.emplace_back(
00427 candidate.scan_index, candidate.offset + Eigen::Array3i(x, y, z));
00428 }
00429 }
00430 }
00431 ScoreCandidates(candidate_depth - 1, discrete_scans,
00432 &higher_resolution_candidates);
00433 best_high_resolution_candidate = std::max(
00434 best_high_resolution_candidate,
00435 BranchAndBound(search_parameters, discrete_scans,
00436 higher_resolution_candidates, candidate_depth - 1,
00437 best_high_resolution_candidate.score));
00438 }
00439 return best_high_resolution_candidate;
00440 }
00441
00442 }
00443 }
00444 }