slam_toolbox_lifelong.cpp
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 
20 
21 namespace slam_toolbox
22 {
23 
24 /*****************************************************************************/
26 /*****************************************************************************/
27 {
28  if (value < 0.0 || value > 1.0)
29  {
30  ROS_FATAL("All stores and scales must be in range [0, 1].");
31  exit(-1);
32  }
33 }
34 
35 /*****************************************************************************/
37 : SlamToolbox(nh)
38 /*****************************************************************************/
39 {
41  nh.param("lifelong_search_use_tree", use_tree_, false);
42  nh.param("lifelong_minimum_score", iou_thresh_, 0.10);
43  nh.param("lifelong_iou_match", iou_match_, 0.85);
44  nh.param("lifelong_node_removal_score", removal_score_, 0.10);
45  nh.param("lifelong_overlap_score_scale", overlap_scale_, 0.5);
46  nh.param("lifelong_constraint_multiplier", constraint_scale_, 0.05);
47  nh.param("lifelong_nearby_penalty", nearby_penalty_, 0.001);
48  nh.param("lifelong_candidates_scale", candidates_scale_, 0.03);
49 
57 
58  ROS_WARN("Lifelong mapping mode in SLAM Toolbox is considered "
59  "experimental and should be understood before proceeding. Please visit: "
60  "https://github.com/SteveMacenski/slam_toolbox/wiki/"
61  "Experimental-Lifelong-Mapping-Node for more information.");
62 
63  // in lifelong mode, we cannot have interactive mode enabled
65 }
66 
67 /*****************************************************************************/
69  const sensor_msgs::LaserScan::ConstPtr& scan)
70 /*****************************************************************************/
71 {
72  // no odom info
73  Pose2 pose;
74  if(!pose_helper_->getOdomPose(pose, scan->header.stamp))
75  {
76  return;
77  }
78 
79  // ensure the laser can be used
80  LaserRangeFinder* laser = getLaser(scan);
81 
82  if(!laser)
83  {
84  ROS_WARN_THROTTLE(5., "Failed to create laser device for"
85  " %s; discarding scan", scan->header.frame_id.c_str());
86  return;
87  }
88 
89  // LTS additional bounded node increase parameter (rate, or total for run or at all?)
90  // LTS pseudo-localization mode. If want to add a scan, but not deleting a scan, add to local buffer?
91  // LTS if (eval() && dont_add_more_scans) {addScan()} else {localization_add_scan()}
92  // LTS if (eval() && ctr / total < add_rate_scans) {addScan()} else {localization_add_scan()}
93  karto::LocalizedRangeScan* range_scan = addScan(laser, scan, pose);
94  evaluateNodeDepreciation(range_scan);
95  return;
96 }
97 
98 /*****************************************************************************/
100  LocalizedRangeScan* range_scan)
101 /*****************************************************************************/
102 {
103  if (range_scan)
104  {
105  boost::mutex::scoped_lock lock(smapper_mutex_);
106 
107  const BoundingBox2& bb = range_scan->GetBoundingBox();
108  const Size2<double> bb_size = bb.GetSize();
109  double radius = sqrt(bb_size.GetWidth()*bb_size.GetWidth() +
110  bb_size.GetHeight()*bb_size.GetHeight()) / 2.0;
111  Vertices near_scan_vertices = FindScansWithinRadius(range_scan, radius);
112 
113  ScoredVertices scored_verices =
114  computeScores(near_scan_vertices, range_scan);
115 
116  ScoredVertices::iterator it;
117  for (it = scored_verices.begin(); it != scored_verices.end(); ++it)
118  {
119  if (it->GetScore() < removal_score_)
120  {
121  ROS_INFO("Removing node %i from graph with score: %f and "
122  "old score: %f.", it->GetVertex()->GetObject()->GetUniqueId(),
123  it->GetScore(), it->GetVertex()->GetScore());
124  removeFromSlamGraph(it->GetVertex());
125  }
126  else
127  {
128  updateScoresSlamGraph(it->GetScore(), it->GetVertex());
129  }
130  }
131  }
132 
133  return;
134 }
135 
136 /*****************************************************************************/
138  LocalizedRangeScan* scan, const double& radius)
139 /*****************************************************************************/
140 {
141  // Using the tree will create a Kd-tree and find all neighbors in graph
142  // around the reference scan. Otherwise it will use the graph and
143  // access scans within radius that are connected with constraints to this
144  // node.
145 
146  if (use_tree_)
147  {
148  return
149  smapper_->getMapper()->GetGraph()->FindNearByVertices(
150  scan->GetSensorName(), scan->GetBarycenterPose(), radius);
151  }
152  else
153  {
154  return
155  smapper_->getMapper()->GetGraph()->FindNearLinkedVertices(scan, radius);
156  }
157 }
158 
159 /*****************************************************************************/
161  const double& intersect_over_union,
162  const double& area_overlap,
163  const double& reading_overlap,
164  const int& num_constraints,
165  const double& initial_score,
166  const int& num_candidates) const
167 /*****************************************************************************/
168 {
169  // We have some useful metrics. lets make a new score
170  // intersect_over_union: The higher this score, the better aligned in scope these scans are
171  // area_overlap: The higher, the more area they share normalized by candidate size
172  // reading_overlap: The higher, the more readings of the new scan the candidate contains
173  // num_constraints: The lower, the less other nodes may rely on this candidate
174  // initial_score: Last score of this vertex before update
175 
176  // this is a really good fit and not from a loop closure, lets just decay
177  if (intersect_over_union > iou_match_ && num_constraints < 3)
178  {
179  return -1.0;
180  }
181 
182  // to be conservative, lets say the overlap is the lesser of the
183  // area and proportion of laser returns in the intersecting region.
184  double overlap = overlap_scale_ * std::min(area_overlap, reading_overlap);
185 
186  // if the num_constraints are high we want to stave off the decay, but not override it
187  double contraint_scale_factor = std::min(1.0,
188  std::max(0., constraint_scale_ * (num_constraints - 2)));
189  contraint_scale_factor = std::min(contraint_scale_factor, overlap);
190 
191  //
192  double candidates = num_candidates - 1;
193  double candidate_scale_factor = candidates_scale_ * candidates;
194 
195  // Give the initial score a boost proportional to the number of constraints
196  // Subtract the overlap amount, apply a penalty for just being nearby
197  // and scale the entire additional score by the number of candidates
198  double score =
199  initial_score * (1.0 + contraint_scale_factor)
200  - overlap
201  - nearby_penalty_;
202 
203  //score += (initial_score - score) * candidate_scale_factor;
204 
205  if (score > 1.0)
206  {
207  ROS_ERROR("Objective function calculated for vertex score (%0.4f)"
208  " greater than one! Thresholding to 1.0", score);
209  return 1.0;
210  }
211 
212  return score;
213 }
214 
215 /*****************************************************************************/
217  LocalizedRangeScan* reference_scan,
218  Vertex<LocalizedRangeScan>* candidate,
219  const double& initial_score, const int& num_candidates)
220 /*****************************************************************************/
221 {
222  double new_score = initial_score;
223  LocalizedRangeScan* candidate_scan = candidate->GetObject();
224 
225  // compute metrics for information loss normalized
226  double iou = computeIntersectOverUnion(reference_scan, candidate_scan);
227  double area_overlap = computeAreaOverlapRatio(reference_scan, candidate_scan);
228  int num_constraints = candidate->GetEdges().size();
229  double reading_overlap = computeReadingOverlapRatio(reference_scan, candidate_scan);
230 
231  bool critical_lynchpoint = candidate_scan->GetUniqueId() == 0 ||
232  candidate_scan->GetUniqueId() == 1;
233  int id_diff = reference_scan->GetUniqueId() - candidate_scan->GetUniqueId();
234  if (id_diff < smapper_->getMapper()->getParamScanBufferSize() ||
235  critical_lynchpoint)
236  {
237  return initial_score;
238  }
239 
240  double score = computeObjectiveScore(iou,
241  area_overlap,
242  reading_overlap,
243  num_constraints,
244  initial_score,
245  num_candidates);
246 
247  ROS_INFO("Metric Scores: Initial: %f, IOU: %f,"
248  " Area: %f, Num Con: %i, Reading: %f, outcome score: %f.",
249  initial_score, iou, area_overlap, num_constraints, reading_overlap, score);
250  return score;
251 }
252 
253 /*****************************************************************************/
256  Vertices& near_scans,
257  LocalizedRangeScan* range_scan)
258 /*****************************************************************************/
259 {
260  ScoredVertices scored_vertices;
261  scored_vertices.reserve(near_scans.size());
262 
263  // must have some minimum metric to utilize
264  // IOU will drop sharply with fitment, I'd advise not setting this value
265  // any higher than 0.15. Also check this is a linked constraint
266  // We want to do this early to get a better estimate of local candidates
267  ScanVector::iterator candidate_scan_it;
268  double iou = 0.0;
269  for (candidate_scan_it = near_scans.begin();
270  candidate_scan_it != near_scans.end(); )
271  {
272  iou = computeIntersectOverUnion(range_scan,
273  (*candidate_scan_it)->GetObject());
274  if (iou < iou_thresh_ || (*candidate_scan_it)->GetEdges().size() < 2)
275  {
276  candidate_scan_it = near_scans.erase(candidate_scan_it);
277  }
278  else
279  {
280  ++candidate_scan_it;
281  }
282  }
283 
284  for (candidate_scan_it = near_scans.begin();
285  candidate_scan_it != near_scans.end(); ++candidate_scan_it)
286  {
287  ScoredVertex scored_vertex((*candidate_scan_it),
288  computeScore(range_scan, (*candidate_scan_it),
289  (*candidate_scan_it)->GetScore(), near_scans.size()));
290  scored_vertices.push_back(scored_vertex);
291  }
292  return scored_vertices;
293 }
294 
295 /*****************************************************************************/
297  Vertex<LocalizedRangeScan>* vertex)
298 /*****************************************************************************/
299 {
300  smapper_->getMapper()->RemoveNodeFromGraph(vertex);
301  smapper_->getMapper()->GetMapperSensorManager()->RemoveScan(
302  vertex->GetObject());
303  dataset_->RemoveData(vertex->GetObject());
304  vertex->RemoveObject();
305  delete vertex;
306  vertex = nullptr;
307  // LTS what do we do about the contraints that node had about it?Nothing?Transfer?
308 }
309 
310 /*****************************************************************************/
312  Vertex<LocalizedRangeScan>* vertex)
313 /*****************************************************************************/
314 {
315  // Saved in graph so it persists between sessions and runs
316  vertex->SetScore(score);
317 }
318 
319 /*****************************************************************************/
321  slam_toolbox_msgs::DeserializePoseGraph::Request& req,
322  slam_toolbox_msgs::DeserializePoseGraph::Response& resp)
323 /*****************************************************************************/
324 {
325  if (req.match_type == procType::LOCALIZE_AT_POSE)
326  {
327  ROS_ERROR("Requested a localization deserialization "
328  "in non-localization mode.");
329  return false;
330  }
331 
333 }
334 
335 /*****************************************************************************/
337  LocalizedRangeScan* s1, LocalizedRangeScan* s2,
338  double& x_l, double& x_u, double& y_l, double& y_u)
339 /*****************************************************************************/
340 {
341  Size2<double> bb1 = s1->GetBoundingBox().GetSize();
342  Size2<double> bb2 = s2->GetBoundingBox().GetSize();
343  Pose2 pose1 = s1->GetBarycenterPose();
344  Pose2 pose2 = s2->GetBarycenterPose();
345 
346  const double s1_upper_x = pose1.GetX() + (bb1.GetWidth() / 2.0);
347  const double s1_upper_y = pose1.GetY() + (bb1.GetHeight() / 2.0);
348  const double s1_lower_x = pose1.GetX() - (bb1.GetWidth() / 2.0);
349  const double s1_lower_y = pose1.GetY() - (bb1.GetHeight() / 2.0);
350 
351  const double s2_upper_x = pose2.GetX() + (bb2.GetWidth() / 2.0);
352  const double s2_upper_y = pose2.GetY() + (bb2.GetHeight() / 2.0);
353  const double s2_lower_x = pose2.GetX() - (bb2.GetWidth() / 2.0);
354  const double s2_lower_y = pose2.GetY() - (bb2.GetHeight() / 2.0);
355 
356  x_u = std::min(s1_upper_x, s2_upper_x);
357  y_u = std::min(s1_upper_y, s2_upper_y);
358  x_l = std::max(s1_lower_x, s2_lower_x);
359  y_l = std::max(s1_lower_y, s2_lower_y);
360  return;
361 }
362 
363 /*****************************************************************************/
364 double LifelongSlamToolbox::computeIntersect(LocalizedRangeScan* s1,
365  LocalizedRangeScan* s2)
366 /*****************************************************************************/
367 {
368  double x_l, x_u, y_l, y_u;
369  computeIntersectBounds(s1, s2, x_l, x_u, y_l, y_u);
370  const double intersect = (y_u - y_l) * (x_u - x_l);
371 
372  if (intersect < 0.0)
373  {
374  return 0.0;
375  }
376 
377  return intersect;
378 }
379 
380 /*****************************************************************************/
381 double LifelongSlamToolbox::computeIntersectOverUnion(LocalizedRangeScan* s1,
382  LocalizedRangeScan* s2)
383 /*****************************************************************************/
384 {
385  // this is a common metric in machine learning used to determine
386  // the fitment of a set of bounding boxes. Its response sharply
387  // drops by box matches.
388 
389  const double intersect = computeIntersect(s1, s2);
390 
391  Size2<double> bb1 = s1->GetBoundingBox().GetSize();
392  Size2<double> bb2 = s2->GetBoundingBox().GetSize();
393  const double uni = (bb1.GetWidth() * bb1.GetHeight()) +
394  (bb2.GetWidth() * bb2.GetHeight()) - intersect;
395 
396  return intersect / uni;
397 }
398 
399 /*****************************************************************************/
401  LocalizedRangeScan* ref_scan,
402  LocalizedRangeScan* candidate_scan)
403 /*****************************************************************************/
404 {
405  // ref scan is new scan, candidate scan is potential for decay
406  // so we want to find the ratio of space of the candidate scan
407  // the reference scan takes up
408 
409  double overlap_area = computeIntersect(ref_scan, candidate_scan);
410  Size2<double> bb_candidate = candidate_scan->GetBoundingBox().GetSize();
411  const double candidate_area =
412  bb_candidate.GetHeight() * bb_candidate.GetWidth();
413 
414  return overlap_area / candidate_area;
415 }
416 
417 /*****************************************************************************/
419  LocalizedRangeScan* ref_scan,
420  LocalizedRangeScan* candidate_scan)
421 /*****************************************************************************/
422 {
423  const PointVectorDouble& pts = candidate_scan->GetPointReadings(true);
424  const int num_pts = pts.size();
425 
426  // get the bounds of the intersect area
427  double x_l, x_u, y_l, y_u;
428  computeIntersectBounds(ref_scan, candidate_scan, x_l, x_u, y_l, y_u);
429 
430  PointVectorDouble::const_iterator pt_it;
431  int inner_pts = 0;
432  for (pt_it = pts.begin(); pt_it != pts.end(); ++pt_it)
433  {
434  if (pt_it->GetX() < x_u && pt_it->GetX() > x_l &&
435  pt_it->GetY() < y_u && pt_it->GetY() > y_l)
436  {
437  inner_pts++;
438  }
439  }
440 
441  return double(inner_pts) / double(num_pts);
442 }
443 
444 } // end namespace
slam_toolbox::SlamToolbox
Definition: slam_toolbox_common.hpp:55
slam_toolbox::LifelongSlamToolbox::nearby_penalty_
double nearby_penalty_
Definition: slam_toolbox_lifelong.hpp:65
slam_toolbox::LifelongSlamToolbox::computeScores
ScoredVertices computeScores(Vertices &near_scans, LocalizedRangeScan *range_scan)
Definition: slam_toolbox_lifelong.cpp:255
slam_toolbox::SlamToolbox::getLaser
karto::LaserRangeFinder * getLaser(const sensor_msgs::LaserScan::ConstPtr &scan)
Definition: slam_toolbox_common.cpp:289
slam_toolbox::LifelongSlamToolbox::deserializePoseGraphCallback
virtual bool deserializePoseGraphCallback(slam_toolbox_msgs::DeserializePoseGraph::Request &req, slam_toolbox_msgs::DeserializePoseGraph::Response &resp) override final
Definition: slam_toolbox_lifelong.cpp:320
ROS_WARN_THROTTLE
#define ROS_WARN_THROTTLE(period,...)
slam_toolbox::SlamToolbox::smapper_
std::unique_ptr< mapper_utils::SMapper > smapper_
Definition: slam_toolbox_common.hpp:123
slam_toolbox::SlamToolbox::deserializePoseGraphCallback
virtual bool deserializePoseGraphCallback(slam_toolbox_msgs::DeserializePoseGraph::Request &req, slam_toolbox_msgs::DeserializePoseGraph::Response &resp)
Definition: slam_toolbox_common.cpp:736
karto::LocalizedRangeScan
Definition: Karto.h:5505
slam_toolbox::LifelongSlamToolbox::candidates_scale_
double candidates_scale_
Definition: slam_toolbox_lifelong.hpp:63
slam_toolbox::LifelongSlamToolbox::FindScansWithinRadius
Vertices FindScansWithinRadius(LocalizedRangeScan *scan, const double &radius)
Definition: slam_toolbox_lifelong.cpp:137
toolbox_types::ScoredVertices
std::vector< ScoredVertex > ScoredVertices
Definition: toolbox_types.hpp:72
slam_toolbox::SlamToolbox::enable_interactive_mode_
bool enable_interactive_mode_
Definition: slam_toolbox_common.hpp:120
slam_toolbox::LifelongSlamToolbox::use_tree_
bool use_tree_
Definition: slam_toolbox_lifelong.hpp:58
slam_toolbox::SlamToolbox::smapper_mutex_
boost::mutex smapper_mutex_
Definition: slam_toolbox_common.hpp:137
toolbox_types::Vertices
std::vector< karto::Vertex< karto::LocalizedRangeScan > * > Vertices
Definition: toolbox_types.hpp:73
ROS_WARN
#define ROS_WARN(...)
slam_toolbox::LifelongSlamToolbox::checkIsNotNormalized
void checkIsNotNormalized(const double &value)
Definition: slam_toolbox_lifelong.cpp:25
slam_toolbox::LifelongSlamToolbox::computeIntersectOverUnion
static double computeIntersectOverUnion(LocalizedRangeScan *s1, LocalizedRangeScan *s2)
Definition: slam_toolbox_lifelong.cpp:381
slam_toolbox::LifelongSlamToolbox::evaluateNodeDepreciation
void evaluateNodeDepreciation(LocalizedRangeScan *range_scan)
Definition: slam_toolbox_lifelong.cpp:99
ROS_FATAL
#define ROS_FATAL(...)
karto::PointVectorDouble
std::vector< Vector2< kt_double > > PointVectorDouble
Definition: Karto.h:1283
slam_toolbox::SlamToolbox::loadPoseGraphByParams
void loadPoseGraphByParams(ros::NodeHandle &nh)
Definition: slam_toolbox_common.cpp:223
slam_toolbox::LifelongSlamToolbox::LifelongSlamToolbox
LifelongSlamToolbox(ros::NodeHandle &nh)
Definition: slam_toolbox_lifelong.cpp:36
slam_toolbox::LifelongSlamToolbox::laserCallback
virtual void laserCallback(const sensor_msgs::LaserScan::ConstPtr &scan) override final
Definition: slam_toolbox_lifelong.cpp:68
slam_toolbox::LifelongSlamToolbox::computeScore
double computeScore(LocalizedRangeScan *reference_scan, Vertex< LocalizedRangeScan > *candidate, const double &initial_score, const int &num_candidates)
Definition: slam_toolbox_lifelong.cpp:216
slam_toolbox::LifelongSlamToolbox::computeAreaOverlapRatio
static double computeAreaOverlapRatio(LocalizedRangeScan *ref_scan, LocalizedRangeScan *candidate_scan)
Definition: slam_toolbox_lifelong.cpp:400
slam_toolbox::SlamToolbox::addScan
virtual karto::LocalizedRangeScan * addScan(karto::LaserRangeFinder *laser, const sensor_msgs::LaserScan::ConstPtr &scan, karto::Pose2 &karto_pose)
Definition: slam_toolbox_common.cpp:476
slam_toolbox::LifelongSlamToolbox::iou_thresh_
double iou_thresh_
Definition: slam_toolbox_lifelong.hpp:59
slam_toolbox::SlamToolbox::dataset_
std::unique_ptr< karto::Dataset > dataset_
Definition: slam_toolbox_common.hpp:124
slam_toolbox::LifelongSlamToolbox::removal_score_
double removal_score_
Definition: slam_toolbox_lifelong.hpp:60
ROS_ERROR
#define ROS_ERROR(...)
slam_toolbox::LifelongSlamToolbox::computeIntersectBounds
static void computeIntersectBounds(LocalizedRangeScan *s1, LocalizedRangeScan *s2, double &x_l, double &x_u, double &y_l, double &y_u)
Definition: slam_toolbox_lifelong.cpp:336
slam_toolbox::SlamToolbox::pose_helper_
std::unique_ptr< pose_utils::GetPoseHelper > pose_helper_
Definition: slam_toolbox_common.hpp:129
ros::NodeHandle::param
T param(const std::string &param_name, const T &default_val) const
slam_toolbox
Definition: slam_toolbox_lifelong.hpp:24
slam_toolbox::LifelongSlamToolbox::overlap_scale_
double overlap_scale_
Definition: slam_toolbox_lifelong.hpp:61
slam_toolbox::LifelongSlamToolbox::computeReadingOverlapRatio
static double computeReadingOverlapRatio(LocalizedRangeScan *ref_scan, LocalizedRangeScan *candidate_scan)
Definition: slam_toolbox_lifelong.cpp:418
slam_toolbox_lifelong.hpp
ROS_INFO
#define ROS_INFO(...)
slam_toolbox::LifelongSlamToolbox::constraint_scale_
double constraint_scale_
Definition: slam_toolbox_lifelong.hpp:62
slam_toolbox::LifelongSlamToolbox::iou_match_
double iou_match_
Definition: slam_toolbox_lifelong.hpp:64
slam_toolbox::LifelongSlamToolbox::removeFromSlamGraph
void removeFromSlamGraph(Vertex< LocalizedRangeScan > *vertex)
Definition: slam_toolbox_lifelong.cpp:296
slam_toolbox::LifelongSlamToolbox::computeIntersect
static double computeIntersect(LocalizedRangeScan *s1, LocalizedRangeScan *s2)
Definition: slam_toolbox_lifelong.cpp:364
slam_toolbox::LifelongSlamToolbox::updateScoresSlamGraph
void updateScoresSlamGraph(const double &score, Vertex< LocalizedRangeScan > *vertex)
Definition: slam_toolbox_lifelong.cpp:311
ros::NodeHandle
slam_toolbox::LifelongSlamToolbox::computeObjectiveScore
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
Definition: slam_toolbox_lifelong.cpp:160


slam_toolbox
Author(s): Steve Macenski
autogenerated on Thu Jan 11 2024 03:37:56