I got a method that adds adjacent Voxels to a vector. This method uses an vector with all the remaining points (means, they still need to be looked at as they are possible adjacents) and looks on every one of them if it is near enough to be added. If an element is a neighbor it also checks all adjacents of this element to add them too. This happens in a recursive manner.
void remove(std::vector<pcl::PointXYZ> &vec, pcl::PointXYZ p) {
for (int i = 0; i < vec.size(); i++) {
if (vec[i].x == p.x && vec[i].y == p.y && vec[i].z == p.z) {
vec.erase(vec.begin() + i);
break; // as all points should be unique
}
}
}
void addAdjacents(pcl::PointXYZ start, std::vector<pcl::PointXYZ> &newCluster, std::vector<pcl::PointXYZ> &remainingPoints) {
for (pcl::PointXYZ p : remainingPoints) {
if (distance(p, start) < 0.015) {
newCluster.push_back(p);
remove(remainingPoints, p);
if (remainingPoints.size() > 0)
addAdjacents(p, newCluster, remainingPoints);
}
}
}
The problem is, that many points from the remainingPoints
-vector are added to the newCluster
multiple times. I thought this wouldn't happen but it seems like internally it makes copies of the vector in the recursion? If a point is removed in a deeper layer the for-loop in the outer execution is somehow still iterating over this (removed) element.
I am fairly new to c++ so I am not sure how to prevent this. Can anyone help me? Thanks!
For sure I can just write a method addToCluster
which just checks if the vector has this element before adding it but I thought that maybe there is a more elegant way to prevent this happening in the first place.
Edit:
As I understand I am breaking my iterator in the loop. So I would need to somehow update my iterator after calling addAdjacents
. Is this right? Can I do something like that?