Make the filtering of duplicate segments more efficient

[rdube]

@exodaniel @HannesSommer I'd like to make the filterDuplicateSegmentsOfTargetMap() function more efficient:

https://github.com/ethz-asl/segmatch/blob/feature/incremental_estimator/segmatch/src/segmatch.cpp#L466

The current idea is that we get a cloud of all target segments centroids and loop over all segments centroid of the cloud to add and check if we should remove the segment from the target cloud.

Creating the target segments cloud centroid can take a few ms (50-100 ms on a big KITTI map). Brute force knn to this cloud currently takes a lot of time (500ms+). I will change by first applying a cylindrical filter to that cloud, according on the trajectory pose they are linked to.

The best would be to first grab the "near collectors" as we had before and then build the local cloud without having to iterate over all segments but only over the ones which are linked to a near collector. We could potentially achieve the same by modifying the definition of valid_segments_ in SegmentedCloud from :

std::vector<Segment> valid_segments_; to:

typedef std::pair<SE3, std::vector<Segment> > PoseAndSegmentsPair; std::vector<PoseAndSegmentsPair> valid_segments_;

But that would require some refactoring. Any thoughts?

[rdube]

Update: this is the expensive function:

https://github.com/ethz-asl/segmatch/blob/master/segmatch/src/segmented_cloud.cpp#L236

I'm gonna switch to an unordered_map

[danieldugas]

Has the efficiency improved or worsen without collectors?

I would assume that this is a case where working with limited neighborhoods is efficient, but I don't see how you would do that if all segments are in the same non-ordered data structure. Any thoughts on a sort of Octomap for segment poses? It might not make much sense if segments in some area all have pretty much the same pose, otherwise it could gain you some efficiency.

Bon courage!

[rdube]

@exodaniel it improved and made the integration cleaner. As I mentioned in my last post the removal is the expensive bit. Moving to unordered_map will hopefully solve that. I'll test soon.

[rdube]

Solved in #13

[danieldugas]

Cool ! I have no doubt that segment poses are a big improvement over the collectors overall. I was wondering about this specific case, since you're working with a single big structure. Good job on solving this!

[rdube]

Thanks! :-) It works fine for the current datasets. It might require improvement on bigger datasets though. The main issue was the deletion in a vector which is not efficient.