Fix octree sampling

[boxanm]

I accidentally noticed this bug when unit testing an NDT-inspired octree sampling method. The current octree sampling implementation takes into account the situation when the indexed point was already processed; see this example:

std::size_t j = d;
//retrieve index from lookup table if sampling in already switched element
if(std::size_t(d)<idx)
   j = mapidx[d];

However, we don't know whether mapidx[d] doesn't again point to an index that was also processed. What actually needs to be done, is to propagate all the changes that were made on all previously used indexes of mapidx. So in this PR, I replaced the above code example and similar by

std::size_t j = d;
//retrieve index from lookup table if sampling in already switched element
while(j < idx)
   j = mapidx[j];

I haven't had time to check other Octree implementations and see how they deal with this situation. Naturally, the use of the while cycle slows down the execution. There's probably a more clever solution using pointers?

[simonpierredeschenes]

@boxanm Could you please confirm that this is the same issue as #454?

[boxanm]

The bug I described shouldn't make random parts of the cloud disappear. Instead, the bug causes incorrect sampling inside individual cells. So points, that should be sampled are preserved and the output contains more points, than the number of cells in the octree. Therefore I don't think #454 is related.