constraint_builder_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_INTERNAL_CONSTRAINTS_CONSTRAINT_BUILDER_3D_H_
00018 #define CARTOGRAPHER_MAPPING_INTERNAL_CONSTRAINTS_CONSTRAINT_BUILDER_3D_H_
00019 
00020 #include <array>
00021 #include <deque>
00022 #include <functional>
00023 #include <limits>
00024 #include <vector>
00025 
00026 #include "Eigen/Core"
00027 #include "Eigen/Geometry"
00028 #include "absl/synchronization/mutex.h"
00029 #include "cartographer/common/fixed_ratio_sampler.h"
00030 #include "cartographer/common/histogram.h"
00031 #include "cartographer/common/lua_parameter_dictionary.h"
00032 #include "cartographer/common/math.h"
00033 #include "cartographer/common/task.h"
00034 #include "cartographer/common/thread_pool.h"
00035 #include "cartographer/mapping/3d/submap_3d.h"
00036 #include "cartographer/mapping/internal/3d/scan_matching/ceres_scan_matcher_3d.h"
00037 #include "cartographer/mapping/internal/3d/scan_matching/fast_correlative_scan_matcher_3d.h"
00038 #include "cartographer/mapping/pose_graph_interface.h"
00039 #include "cartographer/mapping/proto/pose_graph/constraint_builder_options.pb.h"
00040 #include "cartographer/mapping/trajectory_node.h"
00041 #include "cartographer/metrics/family_factory.h"
00042 #include "cartographer/sensor/compressed_point_cloud.h"
00043 #include "cartographer/sensor/internal/voxel_filter.h"
00044 #include "cartographer/sensor/point_cloud.h"
00045 
00046 namespace cartographer {
00047 namespace mapping {
00048 namespace constraints {
00049 
00050 // Asynchronously computes constraints.
00051 //
00052 // Intermingle an arbitrary number of calls to 'MaybeAddConstraint',
00053 // 'MaybeAddGlobalConstraint', and 'NotifyEndOfNode', then call 'WhenDone' once.
00054 // After all computations are done the 'callback' will be called with the result
00055 // and another MaybeAdd(Global)Constraint()/WhenDone() cycle can follow.
00056 //
00057 // This class is thread-safe.
00058 class ConstraintBuilder3D {
00059  public:
00060   using Constraint = mapping::PoseGraphInterface::Constraint;
00061   using Result = std::vector<Constraint>;
00062 
00063   ConstraintBuilder3D(const proto::ConstraintBuilderOptions& options,
00064                       common::ThreadPoolInterface* thread_pool);
00065   ~ConstraintBuilder3D();
00066 
00067   ConstraintBuilder3D(const ConstraintBuilder3D&) = delete;
00068   ConstraintBuilder3D& operator=(const ConstraintBuilder3D&) = delete;
00069 
00070   // Schedules exploring a new constraint between 'submap' identified by
00071   // 'submap_id', and the 'compressed_point_cloud' for 'node_id'.
00072   //
00073   // 'global_node_pose' and 'global_submap_pose' are initial estimates of poses
00074   // in the global map frame, i.e. both are gravity aligned.
00075   //
00076   // The pointees of 'submap' and 'compressed_point_cloud' must stay valid until
00077   // all computations are finished.
00078   void MaybeAddConstraint(const SubmapId& submap_id, const Submap3D* submap,
00079                           const NodeId& node_id,
00080                           const TrajectoryNode::Data* const constant_data,
00081                           const transform::Rigid3d& global_node_pose,
00082                           const transform::Rigid3d& global_submap_pose);
00083 
00084   // Schedules exploring a new constraint between 'submap' identified by
00085   // 'submap_id' and the 'compressed_point_cloud' for 'node_id'.
00086   // This performs full-submap matching.
00087   //
00088   // 'global_node_rotation' and 'global_submap_rotation' are initial estimates
00089   // of roll and pitch, i.e. their yaw is essentially ignored.
00090   //
00091   // The pointees of 'submap' and 'compressed_point_cloud' must stay valid until
00092   // all computations are finished.
00093   void MaybeAddGlobalConstraint(
00094       const SubmapId& submap_id, const Submap3D* submap, const NodeId& node_id,
00095       const TrajectoryNode::Data* const constant_data,
00096       const Eigen::Quaterniond& global_node_rotation,
00097       const Eigen::Quaterniond& global_submap_rotation);
00098 
00099   // Must be called after all computations related to one node have been added.
00100   void NotifyEndOfNode();
00101 
00102   // Registers the 'callback' to be called with the results, after all
00103   // computations triggered by 'MaybeAdd*Constraint' have finished.
00104   // 'callback' is executed in the 'ThreadPool'.
00105   void WhenDone(const std::function<void(const Result&)>& callback);
00106 
00107   // Returns the number of consecutive finished nodes.
00108   int GetNumFinishedNodes();
00109 
00110   // Delete data related to 'submap_id'.
00111   void DeleteScanMatcher(const SubmapId& submap_id);
00112 
00113   static void RegisterMetrics(metrics::FamilyFactory* family_factory);
00114 
00115  private:
00116   struct SubmapScanMatcher {
00117     const HybridGrid* high_resolution_hybrid_grid = nullptr;
00118     const HybridGrid* low_resolution_hybrid_grid = nullptr;
00119     std::unique_ptr<scan_matching::FastCorrelativeScanMatcher3D>
00120         fast_correlative_scan_matcher;
00121     std::weak_ptr<common::Task> creation_task_handle;
00122   };
00123 
00124   // The returned 'grid' and 'fast_correlative_scan_matcher' must only be
00125   // accessed after 'creation_task_handle' has completed.
00126   const SubmapScanMatcher* DispatchScanMatcherConstruction(
00127       const SubmapId& submap_id, const Submap3D* submap)
00128       EXCLUSIVE_LOCKS_REQUIRED(mutex_);
00129 
00130   // Runs in a background thread and does computations for an additional
00131   // constraint.
00132   // As output, it may create a new Constraint in 'constraint'.
00133   void ComputeConstraint(const SubmapId& submap_id, const NodeId& node_id,
00134                          bool match_full_submap,
00135                          const TrajectoryNode::Data* const constant_data,
00136                          const transform::Rigid3d& global_node_pose,
00137                          const transform::Rigid3d& global_submap_pose,
00138                          const SubmapScanMatcher& submap_scan_matcher,
00139                          std::unique_ptr<Constraint>* constraint)
00140       LOCKS_EXCLUDED(mutex_);
00141 
00142   void RunWhenDoneCallback() LOCKS_EXCLUDED(mutex_);
00143 
00144   const proto::ConstraintBuilderOptions options_;
00145   common::ThreadPoolInterface* thread_pool_;
00146   absl::Mutex mutex_;
00147 
00148   // 'callback' set by WhenDone().
00149   std::unique_ptr<std::function<void(const Result&)>> when_done_
00150       GUARDED_BY(mutex_);
00151 
00152   // TODO(gaschler): Use atomics instead of mutex to access these counters.
00153   // Number of the node in reaction to which computations are currently
00154   // added. This is always the number of nodes seen so far, even when older
00155   // nodes are matched against a new submap.
00156   int num_started_nodes_ GUARDED_BY(mutex_) = 0;
00157 
00158   int num_finished_nodes_ GUARDED_BY(mutex_) = 0;
00159 
00160   std::unique_ptr<common::Task> finish_node_task_ GUARDED_BY(mutex_);
00161 
00162   std::unique_ptr<common::Task> when_done_task_ GUARDED_BY(mutex_);
00163 
00164   // Constraints currently being computed in the background. A deque is used to
00165   // keep pointers valid when adding more entries. Constraint search results
00166   // with below-threshold scores are also 'nullptr'.
00167   std::deque<std::unique_ptr<Constraint>> constraints_ GUARDED_BY(mutex_);
00168 
00169   // Map of dispatched or constructed scan matchers by 'submap_id'.
00170   std::map<SubmapId, SubmapScanMatcher> submap_scan_matchers_
00171       GUARDED_BY(mutex_);
00172 
00173   common::FixedRatioSampler sampler_;
00174   scan_matching::CeresScanMatcher3D ceres_scan_matcher_;
00175 
00176   // Histograms of scan matcher scores.
00177   common::Histogram score_histogram_ GUARDED_BY(mutex_);
00178   common::Histogram rotational_score_histogram_ GUARDED_BY(mutex_);
00179   common::Histogram low_resolution_score_histogram_ GUARDED_BY(mutex_);
00180 };
00181 
00182 }  // namespace constraints
00183 }  // namespace mapping
00184 }  // namespace cartographer
00185 
00186 #endif  // CARTOGRAPHER_MAPPING_INTERNAL_CONSTRAINTS_CONSTRAINT_BUILDER_3D_H_


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