correlative_scan_matcher_2d.h
Go to the documentation of this file.
00001 /*
00002  * Copyright 2016 The Cartographer Authors
00003  *
00004  * Licensed under the Apache License, Version 2.0 (the "License");
00005  * you may not use this file except in compliance with the License.
00006  * You may obtain a copy of the License at
00007  *
00008  *      http://www.apache.org/licenses/LICENSE-2.0
00009  *
00010  * Unless required by applicable law or agreed to in writing, software
00011  * distributed under the License is distributed on an "AS IS" BASIS,
00012  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00013  * See the License for the specific language governing permissions and
00014  * limitations under the License.
00015  */
00016 
00017 #ifndef CARTOGRAPHER_MAPPING_INTERNAL_2D_SCAN_MATCHING_CORRELATIVE_SCAN_MATCHER_2D_H_
00018 #define CARTOGRAPHER_MAPPING_INTERNAL_2D_SCAN_MATCHING_CORRELATIVE_SCAN_MATCHER_2D_H_
00019 
00020 #include <vector>
00021 
00022 #include "Eigen/Core"
00023 #include "cartographer/common/lua_parameter_dictionary.h"
00024 #include "cartographer/mapping/2d/map_limits.h"
00025 #include "cartographer/mapping/2d/xy_index.h"
00026 #include "cartographer/sensor/point_cloud.h"
00027 
00028 namespace cartographer {
00029 namespace mapping {
00030 namespace scan_matching {
00031 
00032 typedef std::vector<Eigen::Array2i> DiscreteScan2D;
00033 
00034 // Describes the search space.
00035 struct SearchParameters {
00036   // Linear search window in pixel offsets; bounds are inclusive.
00037   struct LinearBounds {
00038     int min_x;
00039     int max_x;
00040     int min_y;
00041     int max_y;
00042   };
00043 
00044   SearchParameters(double linear_search_window, double angular_search_window,
00045                    const sensor::PointCloud& point_cloud, double resolution);
00046 
00047   // For testing.
00048   SearchParameters(int num_linear_perturbations, int num_angular_perturbations,
00049                    double angular_perturbation_step_size, double resolution);
00050 
00051   // Tightens the search window as much as possible.
00052   void ShrinkToFit(const std::vector<DiscreteScan2D>& scans,
00053                    const CellLimits& cell_limits);
00054 
00055   int num_angular_perturbations;
00056   double angular_perturbation_step_size;
00057   double resolution;
00058   int num_scans;
00059   std::vector<LinearBounds> linear_bounds;  // Per rotated scans.
00060 };
00061 
00062 // Generates a collection of rotated scans.
00063 std::vector<sensor::PointCloud> GenerateRotatedScans(
00064     const sensor::PointCloud& point_cloud,
00065     const SearchParameters& search_parameters);
00066 
00067 // Translates and discretizes the rotated scans into a vector of integer
00068 // indices.
00069 std::vector<DiscreteScan2D> DiscretizeScans(
00070     const MapLimits& map_limits, const std::vector<sensor::PointCloud>& scans,
00071     const Eigen::Translation2f& initial_translation);
00072 
00073 // A possible solution.
00074 struct Candidate2D {
00075   Candidate2D(const int init_scan_index, const int init_x_index_offset,
00076               const int init_y_index_offset,
00077               const SearchParameters& search_parameters)
00078       : scan_index(init_scan_index),
00079         x_index_offset(init_x_index_offset),
00080         y_index_offset(init_y_index_offset),
00081         x(-y_index_offset * search_parameters.resolution),
00082         y(-x_index_offset * search_parameters.resolution),
00083         orientation((scan_index - search_parameters.num_angular_perturbations) *
00084                     search_parameters.angular_perturbation_step_size) {}
00085 
00086   // Index into the rotated scans vector.
00087   int scan_index = 0;
00088 
00089   // Linear offset from the initial pose.
00090   int x_index_offset = 0;
00091   int y_index_offset = 0;
00092 
00093   // Pose of this Candidate2D relative to the initial pose.
00094   double x = 0.;
00095   double y = 0.;
00096   double orientation = 0.;
00097 
00098   // Score, higher is better.
00099   float score = 0.f;
00100 
00101   bool operator<(const Candidate2D& other) const { return score < other.score; }
00102   bool operator>(const Candidate2D& other) const { return score > other.score; }
00103 };
00104 
00105 }  // namespace scan_matching
00106 }  // namespace mapping
00107 }  // namespace cartographer
00108 
00109 #endif  // CARTOGRAPHER_MAPPING_INTERNAL_2D_SCAN_MATCHING_CORRELATIVE_SCAN_MATCHER_2D_H_


cartographer
Author(s): The Cartographer Authors
autogenerated on Thu May 9 2019 02:27:35