Algorithm improvements to KD-Tree module: applications to ICP (Pranjal Kumar Rai)

[jlblancoc]

Initial description

Nanoflann is a C++ library provided by MRPT for building KD-Trees. The library is a fork of the widely-used FLANN C++ library and is mostly optimized for lower dimensional point clouds. Most ICP algorithms for robot SLAM and localization require finding nearest neighbor in 2D/3D point clouds. Support for dynamic point clouds and nearest neighbour query in non flat spaces are two paramount features which are missing in the current nanoflann library.

See attached PDF.

5589797390254080_1491229396_MRPT_GSOC_Proposal.pdf

Final results:

• nanoflann PR 48: Pull request with all code contributions by @pranjal-rai for GSoC2017

[jlblancoc]

I updated this here. Now the build issue is fixed. Thank you very much for your help.

Great!! You're welcome ;-)

Should a git commit message include information about all the changes I made or just the major changes?

As long as messages are not like "changes", "progress", etc. and you give a reasonable overview of what's the most important things you are trying to do with the commit, it'll be fine. Imagine yourself, a few months from now, looking back through the commit logs. The message should be clear enough to make you remember what were you doing today. That would be my advise...

[pranjal-rai]

I have now added test files for real datasets for 4 libraries namely nanoflann, flann, fastann, libkdtree and added all these libraries except nanoflann as 3rdparty libraries. These 4 files basically compute kd-tree insertion time and query time for real datasets with different sizes. I will now move on to tests on randomly generated datasets. I will then integrate these files to make the final benchmarking tool. In my proposal I had divided benchmarking task between second and third month and had planned to add support for nearest neighbor query in non flat spaces during the second month. I would like to complete the entire benchmarking task during the second month itself and then move to non-flat spaces task in third month. Is this change in proposal acceptable?

[pranjal-rai]

Hi @jlblancoc

The following are the results for comparison on real 3d datasets. I have clipped the time taken for libkdtree because it was quite high in comparison to other three libraries.

The general trend for k-d tree build performance is - libkdtree << flann < nanoflann < fastann The general trend for one 3d query performance is - libkdtree << fastann < flann < nanoflann

Please let me know if there is any clarification required.

[pranjal-rai]

The following are the results for comparison on random 3d datasets. The plot just includes the results for nanoflann and flann (as fastann and libkdtree are EXTREMELY slow for point clouds of size lager than 10^4).

The general trend of k-d tree build and query performance is - libkdtree < fastann << flann < nanoflann

[pranjal-rai]

Further my branch is up to date in case if you want to repeat any of the results.

[jlblancoc]

Great results, @pranjal-rai ! Thanks for the plots, they really help understanding the trends. It looks great for nanoflann 👍

Just minor details to improve about the graphs, thinking of a potential follow-up work in which we could try to publish some these results:

• try using loglog() or semilogy() in Matlab/octave to use a logarithmic scale in the x and/or y axes. In this way, the very large values for the slower alternatives will still fit. Just an idea, perhaps it's not worth but we could try....
• Scale time values such that they are in units that don't need to be displayed with many decimal places, e.g. milliseconds, microseconds, etc.
• any "serious" analysis like this one should include error bars in each data point. For that, several (>10) executions of each test will be done, then we will extract the mean and standard deviation of those values. The idea is visually representing whether the difference between close plots are statistically significant.
• Color blind guys like myself find hard to identify some of the plots by looking at the legend :-) To solve it, I always use combinations of color, line thickness, and style (dash/dotted/solid....) to make even clearer the different between all plots.

So far... all good... great work! 👍

[jlblancoc]

PS: Also, the time of operations like "single queries", which are quite fast, are better measured by executing it N times, then taking the average (perhaps you're already doing it, I didn't read that code yet). That is independent of the, again, outer loop M repetitions for getting the error bar statistics.

[pranjal-rai]

I will start working on all the points. Thanks for the suggestions.

[pranjal-rai]

Hi @jlblancoc

I have updated the plots as per the pointers. I have started working on the GUI part to integrate all the results. I have attached the results for tests on real dataset.

[pranjal-rai]

PS: Also, the time of operations like "single queries", which are quite fast, are better measured by executing it N times

I am already doing this.

[jlblancoc]

They look fantastic... good!

In the first one, the yaxis says "(ms)" but... are the numbers actually in ms, or in seconds?

It's also weird that the variance bars are so changing... how many experiments are you averaging here? It seems doing a few more repetitions might help to converge...

[pranjal-rai]

In the first one, the yaxis says "(ms)" but... are the numbers actually in ms, or in seconds?

The time on yaxis is in milliseconds, it is very small because the above test was run on dataset of order 10^3. The time is higher for larger datasets. The following are the results for random dataset of order 10^6 for flann and nanoflann.

It's also weird that the variance bars are so changing... how many experiments are you averaging here?

I am considering around 10 experiments. For random tests I am running the executable with different dataset for each experiment. This might be a possible reason for the weird behaviour in variance bars. Should I run the experiments with the same dataset each time?

[jlblancoc]

Should I run the experiments with the same dataset each time?

Yes please. It's easy to do by setting srand() to the same number ;-)

[pranjal-rai]

It works much better with around 50 experiments and same dataset. I removed srand().

[pranjal-rai]

Attached are some screenshots of the GUI tool. Please suggest what all should we add in the tool. I have committed all the changes to my branch.

[jlblancoc]

It looks great! 👍

Good use of tk for quickly laying out a simple GUI.

Some comments:

• Please, add X & Y grid lines to the plots if that's easy.

• "Relative size of point cloud" might be better mapped to real numbers, i.e. the number of points. If they are spaced logarithmically, feel free of using a log x scale.

• For random datasets, you said

  I removed srand(). That actually doesn't make the comparison fair, unless you call the executable once for each tested library... calling "srand(i)" at the beginning of the i-th repetition (for any library) would be the ideal to ensure the repeatability of results.

Cheers!

[pranjal-rai]

I have added X & Y grid lines to the plots and replaced "Relative Size of point cloud" with actual size. Here is a demo.

That actually doesn't make the comparison fair, unless you call the executable once for each tested library...

I did not get this point. I am not sure how it is unfair. Can you please explain it in a bit detail.

[jlblancoc]

Graphs are now perfect, thanks! :+1:

I did not get this point. I am not sure how it is unfair. Can you please explain it in a bit detail.

Sorry, perhaps I explained it wrong: I'm referring only to the case of random point clouds. If srand() is not called, each pointcloud will be generated with different XYZ coordinates, so we're not comparing two libraries against the same pointcloud, but different libraries against different pointclouds... which is not equally significant. By calling srand(i) with a fixed "i" value before the generation of each pointcloud, we are 100% sure that the pointcloud each lib will be working on, is exactly the same, despite being "random"... don't you agree?

[pranjal-rai]

Hi @jlblancoc

Thank you for the clarification. I fixed the issue and now I am using the fixed seed value i for a particular experiment for all the libraries. The results are same as before and the error bars also look okay.

[raghavendersahdev]

please ignore my reference, I by mistake referenced it by writing # 1 without space

[pranjal-rai]

Hi @jlblancoc

I have now added support for selecting libraries and using desired number of points for random tests.

The GUI now looks something like -

Please suggest if there are any more features which can be added.

[pranjal-rai]

Hi @jlblancoc

My branch Benchmarking is up to date with all the changes. The benchmarking tool is complete with support for 4 libraries. Do I need to create a pull request at this stage?

[jlblancoc]

Hi @pranjal-rai !

Sorry for the delay in answering to your project comments... As you might guess, filling in the evaluation without writing here in advance means that, overall, progress is good... yay! :+1:

I have a few comments on minor improvements on the the benchmark tool, and a proposal for what's next:

First, on the benchmark:

• The name "tool" is rather generic... what about a more descriptive name? This also applies to the window caption, etc. The name "nanoflann" could be shown somewhere to put it in context (e.g. the window caption might be "nanoflann KD-tree libraries benchmark" or something alike).
• Together with the two large buttons "real" and "random", it would be of much help to show a small text field with a short description of what does the application do, what is expected from the user (e.g. "press XXX to run..."), etc.
• Since nanoflann is a small project, it's not required to make the Pull Request now; you can delay it until the end of GSoC is closer.
• All main()s accepting a variable number of arguments (e.g. this one, but it applies to all of them) should follow some good practices:
  • Check the actual number of parameters! Otherwise, accessing argv[2] may lead to a crash.
  • If the number of actual parameters != expected parameters, print a short description with details on how to call the program. This is done by any decent command-line program. For instance, execute gcc, git, link, etc. etc. to see example of error messages. It's not needed to implement a "--help" argument, just "show the help" always that parameters do not look correct.

[jlblancoc]

Next, on next steps:

One of the scheduled tasks we discussed during the preparation time was the extension of nanoflann to non-flat spaces like SO(2), SE(2), SO(3) and SE(3). Please, read ch. 9 of this doc if you need clarification or a refresh on the notation -- see also this Wiki page on SO(2).

Why? Well... querying a KD-tree of robot poses (pose=position + orientation, in 2D or 3D) is a core operation in many motion planning algorithms, and in fact it takes for a large portion of their cost. So it's worth optimizing this operation.

In MRPT, there is a TO-DO task related to this topic. Read this and the calls to getNearestNode() in that same file. Those are the calls we want to optimize!!.

For now, don't waste too much time digging into MRPT sources. First, let's focus on how to extend nanoflann to allow a mix of metric spaces.

I have my own idea on how to approach this, but I'll leave you to come up with a proposal so you have time to realize what is the set of minimal required changes that would allow using SE(2) or SE(3) poses as "points" in the KD-tree. Think about how "points" are stored, compared, etc. and let me know what are your ideas on how to approach the problem (if you feel blocked and don't make progress in a few days, just let me know, don't worry).

Just a hint: a fundamental ingredient are metrics. Metrics for SO(2) are sort of trivial, while for SO(3) there are multiple valid alternatives (having two of them would be more than enough). I think I already sent you some material on them time ago, right? Please, let me know if you find them.

Cheers!

PS: Think of this in this way: nanoflann should be able to find the closest SE(2) pose (x,y,phi) among a set of "points" (=poses), but also, it would be great to allow state spaces in which several coordinates are angles (think of the configuration state vector of an articulated robot arm). Possible implementations could use std::tuple, variadic templates, etc.

[pranjal-rai]

Hi @jlblancoc

The name "tool" is rather generic... what about a more descriptive name?

I have named the tool to Nanoflann Benchmarking Tool

Together with the two large buttons "real" and "random", it would be of much help to show a small text field with a short description

I have added instructions to run the tests on the open screen of the tool.

All main()s accepting a variable number of arguments (e.g. this one, but it applies to all of them) should follow some good practices:

Fixed.

I have updated my branch and will merge it with master once you approve. I will start working on the next part now.

[pranjal-rai]

Hi @jlblancoc

Metrics for SO(2) are sort of trivial, while for SO(3) there are multiple valid alternatives (having two of them would be more than enough). I think I already sent you some material on them time ago, right?

I found this link from our earlier conversations. http://ingmec.ual.es/~jlblanco/papers/blanco2013tutorial-manifolds-introduction-robotics.pdf

So I guess, we need to add support for Frobenius norm and Riemannian distance, right?

[pranjal-rai]

Is there any specific parametrization which has to be used to store the poses or I can use any of these 3 parametrizations: yaw-pitch-roll, quaternion, transformation matrices?

[jlblancoc]

Thanks, much better now!

You can merge that branch.

El 31 jul. 2017 9:41 a. m., "Pranjal Rai" notifications@github.com escribió:

Hi @jlblancoc https://github.com/jlblancoc

The name "tool" is rather generic... what about a more descriptive name?

I have named the tool to Nanoflann Benchmarking Tool

Together with the two large buttons "real" and "random", it would be of much help to show a small text field with a short description

I have added instructions to run the tests on the open screen of the tool.

[image: openscreen] https://user-images.githubusercontent.com/6441513/28767329-da525bdc-75f0-11e7-850d-2afe3a04439a.png

All main()s accepting a variable number of arguments (e.g. this one, but it applies to all of them) should follow some good practices:

Fixed.

I have updated my branch https://github.com/pranjal-rai/nanoflann/tree/Benchmarking and I will merge it with master once you approve. I will start working on the next part now.

— You are receiving this because you were mentioned. Reply to this email directly, view it on GitHub https://github.com/MRPT/GSoC2017-discussions/issues/1#issuecomment-318992874, or mute the thread https://github.com/notifications/unsubscribe-auth/AFPj2kVXzk2qWQEOrGKWDaMgp3ZZfO6Bks5sTYVDgaJpZM4NRWSn .

[jlblancoc]

Exactly, those two metrics are quite common so it's good to have both of them.

On what parameterization of so(3) we shall support... I'm not sure. On the one hand, forcing users to store full 3x3 matrices will be the most efficient (you'll check this while implementing the metrics...), since I think any other parameterization requires passing to matrix form during comparison, which will be a waste of time.

Give it a thought and let me know if you agree or find some better alternative? (We could add a transparent cache of 3x3 matrices, but it'll be too complicate for the first implementation,I guess )

El 2 ago. 2017 2:02 a. m., "Pranjal Rai" notifications@github.com escribió:

Is there any specific parametrization which has to be used to store the poses or I can use any of these 3 parametrizations: yaw-pitch-roll, quaternion, transformation matrices?

— You are receiving this because you were mentioned. Reply to this email directly, view it on GitHub https://github.com/MRPT/GSoC2017-discussions/issues/1#issuecomment-319530037, or mute the thread https://github.com/notifications/unsubscribe-auth/AFPj2ohy7lg_dgTqqW5fPCJtV7RTRbAoks5sT7x_gaJpZM4NRWSn .

[jlblancoc]

@jbriales sorry to bother you with this other gsoc project... but since you're the expert in hot stuff... :-)

Do you recommend us any other metric for so(3), apart of Frobenius norm and Riemannian distance? We'll use them for kd-tree building and querying. Thanks!

[pranjal-rai]

Hi @jlblancoc

I read some portion of this paper and concluded that quaternion parametrization is a good choice for 3d rotations. Further there is a metric mentioned in the paper, Inner Product of Unit Quaternions (section 3.4) defined over the quaternions which seems a good option for metric choice because as shown in the comparison section it takes the least number of operations to evaluate distance between two quaternions (4 multiplications, 1 arccos, 1 comparison). What is your view on this?

[jlblancoc]

I'm glad you investigated it! Ok, start assuming rotations are always stored as unit quaternions; it's quite common and compact.

What about the idea of using a template argument of tuple<> to describe the type of metric of each part of the state vectors, do you have a sketch on which we can further discuss?

Cheers.

[pranjal-rai]

So right now, I am planning to implement the following two metrics which are defined over quaternions q1 and q2 as:

• Φ3(q1,q2) = arccos(|q1 · q2|) (4 multiplications, 1 arccos, 1 comparison) Range : [0,π/2]

• Φ4(q1,q2) = 1 − |q1 · q2| (4 multiplications, 1 comparison) Range : [0, 1]

In the paper they have compared 6 different metrics and these are the two most optimal metric choice for quaternions.

I will assume that rotations are stored as quaternions and perform all the operations on them. I will start reading the code and try to come up with a solution which makes minimal changes to it. Does this look reasonable?

What about the idea of using a template argument of tuple<>

If I am not wrong do you mean to say that I should be able to pass the type of metric to be used as a template parameter, right? I will keep this in mind while coding.

[pranjal-rai]

Along with this I can also add a function to convert rotation matrix to quaternions, to support rotation matrix as well.

[jlblancoc]

Ok to go on with those two metrics (we need "names" for them! ;-).

Along with this I can also add a function to convert rotation matrix to quaternions, to support rotation matrix as well.

I would prefer not to do it, at least for now. Let's focus on one particular parameterization... It's the "Parsimony principle" of designing library APIs ;-)

On the metrics and tuples, this is what I have in my mind:

The final user should be able to use as metric class (similar, more complete examples shall be included in the library docs/README) in any nanoflann index class:

• state space = q \in R^n (real numbers, dimension = n):

L2_Simple_Adaptor<...>

• state space = R^3 * UnitQuaternion:

std::tuple< L2_Simple_Adaptor<....,¿3?,> , SO3MetricQuat<...> >

Some ideas:

• Each part calling to the metric class/object, should be converted into a call to a new auxiliary function to be able to detect whether metric is a single class, or a std::tuple. See this example

[pranjal-rai]

We can name Φ3, Φ4 metrics as acosInnerProdQuat_Adaptor and InnerProdQuat_Adaptor respectively.

I would prefer not to do it, at least for now.

Okay.

In the current implementation, the distance adaptors are implemented as separate structs. For example there are structs named L1_Adaptor, L2_Adaptor and L2_Simple_Adaptor. So I will go ahead and code similar structures for acosInnerProdQuat_Adaptor and InnerProdQuat_Adaptor.

I had some doubts.

• Do we need to handle only SO(3) or both SO(3) and SE(3)?

• std::tuple< L2_Simple_Adaptor<....,¿3?,> , SO3MetricQuat<...> >

If I am right, this tuple is required to handle SE(3). In this case L2_Simple_Adaptor will be applied to the translation part and SO3MetricQuat (which will either be acosInnerProdQuat_Adaptor or InnerProdQuat_Adaptor) will be applied to the rotaional part of SE(3). Now suppose I have the distance in both translation and rotational part. How do I merge this to compute the final distance for SE(3) group?

[pranjal-rai]

Hi @jlblancoc

I have created a new branch and I will keep it updated with all the changes. I will merge this branch to master once I am done.

[jlblancoc]

Great!

If I am right, this tuple is required to handle SE(3). In this case L2_Simple_Adaptor will be applied to the translation part and SO3MetricQuat (which will either be acosInnerProdQuat_Adaptor or InnerProdQuat_Adaptor) will be applied to the rotaional part of SE(3).

That's exactly what I had in mind, you got it perfectly 👍

Now suppose I have the distance in both translation and rotational part. How do I merge this to compute the final distance for SE(3) group?

Sure, I thought of that but didn't want to complicate things right from the beginning: for now, just add all metrics for all the vector segments. Later, we can add an optional scaling / weighting factor to each of the metric parts, e.g. SO(3) weights 10x more than the translational part, etc.

Sorry for the late answers!

[pranjal-rai]

Hi @jlblancoc

I had a doubt in the code. What will be the accum_dist function for metric InnerProdQuat_Adaptor and acosInnerProdQuat_Adaptor?

InnerProdQuat_Adaptor 1 − |q1 · q2|

acosInnerProdQuat_Adaptor arccos(|q1 · q2|)

[jbriales]

Hi everyone! Sorry for the very late answer, I couldn't check the mail for almost two weeks and the mail stack grew out of control. Anyway, I saw you already solved the issue quite satisfactorily. As @pranjal-rai found out, I think metrics related to quaternions are probably the most efficient ones. I have no experience with those, as I usually employ the Frobenius norm for its simplicity in terms of mathematical complexity (still, less effective as it involves more operations). Just in case, another interesting work is "Rotation Averaging", https://link.springer.com/article/10.1007/s11263-012-0601-0. I would say at the end all of them should behave quite similarly, so I would keep the most efficient one as you did.

Cheers!

[pranjal-rai]

Thanks @jbriales , I will read the paper you suggested.

@jlblancoc

I found out that accum_dist is the partial distance, which is calculated using just one dimension and is used by kd-tree while traversal. So what will be the partial distance for our metrics? Source : flann code

[jlblancoc]

Thanks Jesus! I know you took a loooong flight, thanks for your time ;-)

[jlblancoc]

Hi Pranjal!

Yes, that function for accumulating distances now should be refactoredj that's probably the most difficult part of what needs to be done to accomplish the generality of state space topologies...

I think we should transform the calls to that functions, iterated over N dimensions, to one call per "topology group/type", then do the loop inside the metric-specific class.

Does it make sense to you? Thoughts? Let me know if you find something else where you need a hand...

El 8 ago. 2017 3:46 p. m., "Pranjal Rai" notifications@github.com escribió:

Thanks @jbriales https://github.com/jbriales , I will read the paper you suggested.

@jlblancoc https://github.com/jlblancoc

I found out that accum_dist https://github.com/jlblancoc/nanoflann/blob/master/include/nanoflann.hpp#L374 is the partial distance, which is calculated using just one dimension and is used by kd-tree while traversal. So what will be the partial distance for our metrics? Source : flann code https://github.com/mariusmuja/flann/blob/master/src/cpp/flann/algorithms/dist.h#L180

— You are receiving this because you were mentioned. Reply to this email directly, view it on GitHub https://github.com/MRPT/GSoC2017-discussions/issues/1#issuecomment-320960695, or mute the thread https://github.com/notifications/unsubscribe-auth/AFPj2k2xOANS4hVzl31ntBX336m6bxifks5sWGa5gaJpZM4NRWSn .

[pranjal-rai]

I think we should transform the calls to that functions, iterated over N dimensions, to one call per "topology group/type", then do the loop inside the metric-specific class.

But there are instances in the code where we are calling accum_dist for just a single dimension. For example, to calculate cut_dist while traversing the tree. What should be returned in this case?

[pranjal-rai]

Hi @jlblancoc

I am stuck with the accumulating distance part. I tried looking up about it but did not find anything. Can you please help me out?

[jlblancoc]

Hi!

I assume your latest changes are in your fork and uploaded to the latest branch, right? I'll take a look tonight and help you out of this, don't worry.

BTW: To put you in context of why this nanoflann subproject is useful within MRPT (ICP aside), here is one glowing place where a SE(2) kd-tree would be fantastic to have: https://github.com/MRPT/mrpt/blob/master/libs/nav/src/planners/PlannerRRT_SE2_TPS.cpp

(search for getNearestNode()).

[pranjal-rai]

I have updated my fork. I still need to clean up the code.

[jlblancoc]

Hi! Just some a minor thing you can work on in the meanwhile: since we now have many "projects" (CMake targets) inside the nanoflann repo, it's great to organize them in so called solution folders

It's easy to do:

# near the beginning:
set_property(GLOBAL PROPERTY USE_FOLDERS ON)
set_property(GLOBAL PROPERTY PREDEFINED_TARGETS_FOLDER "CMakeTargets")

# for each target, classify it:
set_target_properties(my_executable_target PROPERTIES FOLDER "examples")
...
set_target_properties(my_test_target PROPERTIES FOLDER "tests")
...

At least, this is very useful for Visual Studio, I don't know if any Linux IDE uses the folder organization as well...

[pranjal-rai]

Hi @jlblancoc

I have added the solution folders here. I was not able to test it as I am a linux user. I will now start documenting and cleaning all the codes. Meanwhile please suggest to me if there is any other task which can be done. Also Is there any update on the accumulating distance part?

[jlblancoc]

👍 to clean-up and documenting!

Sorry about the accumulating distance task... I got 100% absorbed by the tons of bugs/tasks in the main MRPT project (sigh). Will look at your topic now.