submap_3d.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_3D_SUBMAP_3D_H_
00018 #define CARTOGRAPHER_MAPPING_3D_SUBMAP_3D_H_
00019 
00020 #include <memory>
00021 #include <string>
00022 #include <vector>
00023 
00024 #include "Eigen/Geometry"
00025 #include "cartographer/common/port.h"
00026 #include "cartographer/mapping/3d/hybrid_grid.h"
00027 #include "cartographer/mapping/3d/range_data_inserter_3d.h"
00028 #include "cartographer/mapping/id.h"
00029 #include "cartographer/mapping/proto/3d/submaps_options_3d.pb.h"
00030 #include "cartographer/mapping/proto/serialization.pb.h"
00031 #include "cartographer/mapping/proto/submap_visualization.pb.h"
00032 #include "cartographer/mapping/submaps.h"
00033 #include "cartographer/sensor/range_data.h"
00034 #include "cartographer/transform/rigid_transform.h"
00035 #include "cartographer/transform/transform.h"
00036 
00037 namespace cartographer {
00038 namespace mapping {
00039 
00040 proto::SubmapsOptions3D CreateSubmapsOptions3D(
00041     common::LuaParameterDictionary* parameter_dictionary);
00042 
00043 class Submap3D : public Submap {
00044  public:
00045   Submap3D(float high_resolution, float low_resolution,
00046            const transform::Rigid3d& local_submap_pose,
00047            const Eigen::VectorXf& rotational_scan_matcher_histogram);
00048 
00049   explicit Submap3D(const proto::Submap3D& proto);
00050 
00051   proto::Submap ToProto(bool include_probability_grid_data) const override;
00052   void UpdateFromProto(const proto::Submap& proto) override;
00053 
00054   void ToResponseProto(const transform::Rigid3d& global_submap_pose,
00055                        proto::SubmapQuery::Response* response) const override;
00056 
00057   const HybridGrid& high_resolution_hybrid_grid() const {
00058     return *high_resolution_hybrid_grid_;
00059   }
00060   const HybridGrid& low_resolution_hybrid_grid() const {
00061     return *low_resolution_hybrid_grid_;
00062   }
00063   const Eigen::VectorXf& rotational_scan_matcher_histogram() const {
00064     return rotational_scan_matcher_histogram_;
00065   }
00066 
00067   // Insert 'range_data' into this submap using 'range_data_inserter'. The
00068   // submap must not be finished yet.
00069   void InsertData(const sensor::RangeData& range_data,
00070                   const RangeDataInserter3D& range_data_inserter,
00071                   float high_resolution_max_range,
00072                   const Eigen::Quaterniond& local_from_gravity_aligned,
00073                   const Eigen::VectorXf& scan_histogram_in_gravity);
00074 
00075   void Finish();
00076 
00077  private:
00078   void UpdateFromProto(const proto::Submap3D& submap_3d);
00079 
00080   std::unique_ptr<HybridGrid> high_resolution_hybrid_grid_;
00081   std::unique_ptr<HybridGrid> low_resolution_hybrid_grid_;
00082   Eigen::VectorXf rotational_scan_matcher_histogram_;
00083 };
00084 
00085 // The first active submap will be created on the insertion of the first range
00086 // data. Except during this initialization when no or only one single submap
00087 // exists, there are always two submaps into which range data is inserted: an
00088 // old submap that is used for matching, and a new one, which will be used for
00089 // matching next, that is being initialized.
00090 //
00091 // Once a certain number of range data have been inserted, the new submap is
00092 // considered initialized: the old submap is no longer changed, the "new" submap
00093 // is now the "old" submap and is used for scan-to-map matching. Moreover, a
00094 // "new" submap gets created. The "old" submap is forgotten by this object.
00095 class ActiveSubmaps3D {
00096  public:
00097   explicit ActiveSubmaps3D(const proto::SubmapsOptions3D& options);
00098 
00099   ActiveSubmaps3D(const ActiveSubmaps3D&) = delete;
00100   ActiveSubmaps3D& operator=(const ActiveSubmaps3D&) = delete;
00101 
00102   // Inserts 'range_data_in_local' into the Submap collection.
00103   // 'local_from_gravity_aligned' is used for the orientation of new submaps so
00104   // that the z axis approximately aligns with gravity.
00105   // 'rotational_scan_matcher_histogram_in_gravity' will be accumulated in all
00106   // submaps of the Submap collection.
00107   std::vector<std::shared_ptr<const Submap3D>> InsertData(
00108       const sensor::RangeData& range_data_in_local,
00109       const Eigen::Quaterniond& local_from_gravity_aligned,
00110       const Eigen::VectorXf& rotational_scan_matcher_histogram_in_gravity);
00111 
00112   std::vector<std::shared_ptr<const Submap3D>> submaps() const;
00113 
00114  private:
00115   void AddSubmap(const transform::Rigid3d& local_submap_pose,
00116                  int rotational_scan_matcher_histogram_size);
00117 
00118   const proto::SubmapsOptions3D options_;
00119   std::vector<std::shared_ptr<Submap3D>> submaps_;
00120   RangeDataInserter3D range_data_inserter_;
00121 };
00122 
00123 }  // namespace mapping
00124 }  // namespace cartographer
00125 
00126 #endif  // CARTOGRAPHER_MAPPING_3D_SUBMAP_3D_H_


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