slam_toolbox_lifelong.hpp
Go to the documentation of this file.
1 /*
2  * slam_toolbox
3  * Copyright (c) 2019, Samsung Research America
4  *
5  * THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE
6  * COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY
7  * COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS
8  * AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED.
9  *
10  * BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO
11  * BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS
12  * CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND
13  * CONDITIONS.
14  *
15  */
16 
17 /* Author: Steven Macenski */
18 
19 #ifndef SLAM_TOOLBOX_SLAM_TOOLBOX_LIFELONG_H_
20 #define SLAM_TOOLBOX_SLAM_TOOLBOX_LIFELONG_H_
21 
23 
24 namespace slam_toolbox
25 {
26 
27 using namespace ::karto;
28 
30 {
31 public:
34 
35  // computation metrics
36  double computeObjectiveScore(const double& intersect_over_union, const double& area_overlap, const double& reading_overlap, const int& num_constraints, const double& initial_score, const int& num_candidates) const;
37  static double computeIntersect(LocalizedRangeScan* s1, LocalizedRangeScan* s2);
38  static double computeIntersectOverUnion(LocalizedRangeScan* s1, LocalizedRangeScan* s2);
39  static double computeAreaOverlapRatio(LocalizedRangeScan* ref_scan, LocalizedRangeScan* candidate_scan);
40  static double computeReadingOverlapRatio(LocalizedRangeScan* ref_scan, LocalizedRangeScan* candidate_scan);
41  static void computeIntersectBounds(LocalizedRangeScan* s1, LocalizedRangeScan* s2, double& x_l, double& x_u, double& y_l, double& y_u);
42 
43 protected:
44  virtual void laserCallback(
45  const sensor_msgs::LaserScan::ConstPtr& scan) override final;
46  virtual bool deserializePoseGraphCallback(
47  slam_toolbox_msgs::DeserializePoseGraph::Request& req,
48  slam_toolbox_msgs::DeserializePoseGraph::Response& resp) override final;
49 
50  void evaluateNodeDepreciation(LocalizedRangeScan* range_scan);
51  void removeFromSlamGraph(Vertex<LocalizedRangeScan>* vertex);
52  double computeScore(LocalizedRangeScan* reference_scan, Vertex<LocalizedRangeScan>* candidate, const double& initial_score, const int& num_candidates);
53  ScoredVertices computeScores(Vertices& near_scans, LocalizedRangeScan* range_scan);
54  Vertices FindScansWithinRadius(LocalizedRangeScan* scan, const double& radius);
55  void updateScoresSlamGraph(const double& score, Vertex<LocalizedRangeScan>* vertex);
56  void checkIsNotNormalized(const double& value);
57 
58  bool use_tree_;
59  double iou_thresh_;
64  double iou_match_;
66 };
67 
68 }
69 
70 #endif //SLAM_TOOLBOX_SLAM_TOOLBOX_LIFELONG_H_
void removeFromSlamGraph(Vertex< LocalizedRangeScan > *vertex)
std::vector< karto::Vertex< karto::LocalizedRangeScan > * > Vertices
double computeScore(LocalizedRangeScan *reference_scan, Vertex< LocalizedRangeScan > *candidate, const double &initial_score, const int &num_candidates)
static void computeIntersectBounds(LocalizedRangeScan *s1, LocalizedRangeScan *s2, double &x_l, double &x_u, double &y_l, double &y_u)
double computeObjectiveScore(const double &intersect_over_union, const double &area_overlap, const double &reading_overlap, const int &num_constraints, const double &initial_score, const int &num_candidates) const
std::vector< ScoredVertex > ScoredVertices
void evaluateNodeDepreciation(LocalizedRangeScan *range_scan)
static double computeReadingOverlapRatio(LocalizedRangeScan *ref_scan, LocalizedRangeScan *candidate_scan)
void updateScoresSlamGraph(const double &score, Vertex< LocalizedRangeScan > *vertex)
ScoredVertices computeScores(Vertices &near_scans, LocalizedRangeScan *range_scan)
virtual bool deserializePoseGraphCallback(slam_toolbox_msgs::DeserializePoseGraph::Request &req, slam_toolbox_msgs::DeserializePoseGraph::Response &resp) override final
Vertices FindScansWithinRadius(LocalizedRangeScan *scan, const double &radius)
static double computeAreaOverlapRatio(LocalizedRangeScan *ref_scan, LocalizedRangeScan *candidate_scan)
virtual void laserCallback(const sensor_msgs::LaserScan::ConstPtr &scan) override final
void checkIsNotNormalized(const double &value)
static double computeIntersect(LocalizedRangeScan *s1, LocalizedRangeScan *s2)
Definition: Karto.h:86
static double computeIntersectOverUnion(LocalizedRangeScan *s1, LocalizedRangeScan *s2)


slam_toolbox
Author(s): Steve Macenski
autogenerated on Mon Feb 28 2022 23:46:49