From e8959f56b833ac9d436513db31749f13a48166d5 Mon Sep 17 00:00:00 2001
From: Alejandro Lopez Gestoso <alopez@iri.upc.edu>
Date: Mon, 15 Nov 2021 12:21:52 +0100
Subject: [PATCH] Now it removes also features when relocalization. Added clear
of landmarks
---
src/adc_landmarks_slam_solver_alg.cpp | 2 ++
src/adc_landmarks_slam_solver_alg_node.cpp | 6 ++++++
2 files changed, 8 insertions(+)
diff --git a/src/adc_landmarks_slam_solver_alg.cpp b/src/adc_landmarks_slam_solver_alg.cpp
index 20b8cd3..7a1a93f 100644
--- a/src/adc_landmarks_slam_solver_alg.cpp
+++ b/src/adc_landmarks_slam_solver_alg.cpp
@@ -64,6 +64,7 @@ bool AdcLandmarksSlamSolverAlgorithm::load_landmarks(const std::string& _landmar
if (!landmarks_file.good())
return false;
std::map<double, LandmarkInfo>().swap(this->landmarks_);
+ this->landmarks_.clear();
//Read the file
double id, l1, l2, l3;
int t;
@@ -97,6 +98,7 @@ bool AdcLandmarksSlamSolverAlgorithm::load_from_xodr(const std::string& _landmar
road.set_min_road_length(0.01);
road.load(_landmarks_file);
std::map<double, LandmarkInfo>().swap(this->landmarks_);
+ this->landmarks_.clear();
for(unsigned int i=0;i<road.get_num_segments();i++)
{
diff --git a/src/adc_landmarks_slam_solver_alg_node.cpp b/src/adc_landmarks_slam_solver_alg_node.cpp
index 9f3dabb..6b8eb70 100644
--- a/src/adc_landmarks_slam_solver_alg_node.cpp
+++ b/src/adc_landmarks_slam_solver_alg_node.cpp
@@ -264,6 +264,12 @@ void AdcLandmarksSlamSolverAlgNode::mainNodeThread(void)
//Remove candidates
std::list<LandmarkCandidate>().swap(this->landmarks_candidates_);
this->landmarks_candidates_.clear();
+ std::vector<FeatureInfo>().swap(this->front_features_);
+ this->front_features_.clear();
+ std::vector<FeatureInfo>().swap(this->rear_features_);
+ this->rear_features_.clear();
+ std::vector<FeatureInfo>().swap(this->features_);
+ this->features_.clear();
this->new_initialpose_ = false;
this->new_estimated_pose_ = true;
--
GitLab