28 if (value < 0.0 || value > 1.0)
30 ROS_FATAL(
"All stores and scales must be in range [0, 1].");
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.");
69 const sensor_msgs::LaserScan::ConstPtr& scan)
80 LaserRangeFinder* laser =
getLaser(scan);
85 " %s; discarding scan", scan->header.frame_id.c_str());
100 LocalizedRangeScan* range_scan)
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;
116 ScoredVertices::iterator it;
117 for (it = scored_verices.begin(); it != scored_verices.end(); ++it)
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());
138 LocalizedRangeScan* scan,
const double& radius)
149 smapper_->getMapper()->GetGraph()->FindNearByVertices(
150 scan->GetSensorName(), scan->GetBarycenterPose(), radius);
155 smapper_->getMapper()->GetGraph()->FindNearLinkedVertices(scan, radius);
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
177 if (intersect_over_union > iou_match_ && num_constraints < 3)
184 double overlap = overlap_scale_ * std::min(area_overlap, reading_overlap);
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);
192 double candidates = num_candidates - 1;
193 double candidate_scale_factor = candidates_scale_ * candidates;
199 initial_score * (1.0 + contraint_scale_factor)
207 ROS_ERROR(
"Objective function calculated for vertex score (%0.4f)"
208 " greater than one! Thresholding to 1.0", score);
217 LocalizedRangeScan* reference_scan,
218 Vertex<LocalizedRangeScan>* candidate,
219 const double& initial_score,
const int& num_candidates)
222 double new_score = initial_score;
223 LocalizedRangeScan* candidate_scan = candidate->GetObject();
228 int num_constraints = candidate->GetEdges().size();
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() ||
237 return initial_score;
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);
257 LocalizedRangeScan* range_scan)
261 scored_vertices.reserve(near_scans.size());
267 ScanVector::iterator candidate_scan_it;
269 for (candidate_scan_it = near_scans.begin();
270 candidate_scan_it != near_scans.end(); )
273 (*candidate_scan_it)->GetObject());
274 if (iou <
iou_thresh_ || (*candidate_scan_it)->GetEdges().size() < 2)
276 candidate_scan_it = near_scans.erase(candidate_scan_it);
284 for (candidate_scan_it = near_scans.begin();
285 candidate_scan_it != near_scans.end(); ++candidate_scan_it)
287 ScoredVertex scored_vertex((*candidate_scan_it),
289 (*candidate_scan_it)->GetScore(), near_scans.size()));
290 scored_vertices.push_back(scored_vertex);
292 return scored_vertices;
297 Vertex<LocalizedRangeScan>* vertex)
300 smapper_->getMapper()->RemoveNodeFromGraph(vertex);
301 smapper_->getMapper()->GetMapperSensorManager()->RemoveScan(
302 vertex->GetObject());
303 dataset_->RemoveData(vertex->GetObject());
304 vertex->RemoveObject();
312 Vertex<LocalizedRangeScan>* vertex)
316 vertex->SetScore(score);
321 slam_toolbox_msgs::DeserializePoseGraph::Request& req,
322 slam_toolbox_msgs::DeserializePoseGraph::Response& resp)
325 if (req.match_type == procType::LOCALIZE_AT_POSE)
327 ROS_ERROR(
"Requested a localization deserialization "
328 "in non-localization mode.");
337 LocalizedRangeScan* s1, LocalizedRangeScan* s2,
338 double& x_l,
double& x_u,
double& y_l,
double& y_u)
341 Size2<double> bb1 = s1->GetBoundingBox().GetSize();
342 Size2<double> bb2 = s2->GetBoundingBox().GetSize();
343 Pose2 pose1 = s1->GetBarycenterPose();
344 Pose2 pose2 = s2->GetBarycenterPose();
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);
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);
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);
365 LocalizedRangeScan* s2)
368 double x_l, x_u, y_l, y_u;
370 const double intersect = (y_u - y_l) * (x_u - x_l);
382 LocalizedRangeScan* s2)
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;
396 return intersect / uni;
401 LocalizedRangeScan* ref_scan,
402 LocalizedRangeScan* candidate_scan)
410 Size2<double> bb_candidate = candidate_scan->GetBoundingBox().GetSize();
411 const double candidate_area =
412 bb_candidate.GetHeight() * bb_candidate.GetWidth();
414 return overlap_area / candidate_area;
419 LocalizedRangeScan* ref_scan,
420 LocalizedRangeScan* candidate_scan)
424 const int num_pts = pts.size();
427 double x_l, x_u, y_l, y_u;
430 PointVectorDouble::const_iterator pt_it;
432 for (pt_it = pts.begin(); pt_it != pts.end(); ++pt_it)
434 if (pt_it->GetX() < x_u && pt_it->GetX() > x_l &&
435 pt_it->GetY() < y_u && pt_it->GetY() > y_l)
441 return double(inner_pts) / double(num_pts);