Flann - k-d Trees

[joshhting]

Fixes #7 using flann submodule

[justbuchanan]

One thing to keep in mind when working on this is the flexibility of the rrt library to be used for things other than robocup. The library is designed to be generic in that it can work with an arbitrary number of dimensions and different kinds of state spaces as long as you provide your custom type T (i.e. Vector2d) and a StateSpace subclass that overrides the right methods to work with T.

In a few places of the library we do make the assumption that T is a Vector2f, but this is only in PlaneStateSpace and it's subclasses, which aren't required to be used with the RRT. It would be easy to create a new 3d state space and use eigen's Vector3d class to solve 3-dimensional search problems.

It looks like right now this PR changes RRT::Tree itself to assume that we're using Vector2d as our state and only working in 2d. This works fine for how it's being used in robocup, but does take away a lot of the flexibility.

If the trade-off isn't a problem for you guys, then go for it, just wanted to point it out in case it hadn't been discussed. I think with some refactoring though, there's a way to use the kd-tree for state spaces where it's relevant, but default to the old implementation otherwise so that the RRT can still be used for other things.

[joshhting]

Where does RRT::Tree assume Vector2d and a 2d space? I'm not seeing anything in Tree.hpp that requires Vector2d

[justbuchanan]

The main issue is that within Tree.hpp, there are a couple places where the state type T is cast to a double* and treated as an array of doubles. It doesn't explicitly require that T is a Vector2d, but it looks like it will only work with types that are of the same form (i.e. an array of doubles). Be sure to be explicit about which types of state it will work with.

Also, you're right that the dimensions are not hard-coded - I misread that the first time through. I think 3d would work just fine the way you have it setup. Might be a good thing to add a small unit test for. Also, I don't see the dimensions parameter to Tree's constructor used anywhere.

[joshhting]

So I added some optional function pointer parameters to the Tree so that for types that can't be automatically converted between double* and T, it will use those functions to make the conversion, if provided.

Regarding creating a test for 3d spaces, that would involve modifying GridStateSpace.cpp and PlaneStateSpace.cpp to handle both 2 and 3 dimensions, as the test would involve passing in a 3d state space to the tree. I feel these changes are probably significant/unrelated enough to go in a separate pull request.

[jgkamat]

Getting this error when using flann 1.9.1:

FAILED: rrt-viewer 
: && ccache /usr/lib/ccache/bin/c++   -std=c++11 -fPIC -g   CMakeFiles/rrt-viewer.dir/src/rrt-viewer/main.cpp.o CMakeFiles/rrt-viewer.dir/src/rrt-viewer/RRTWidget.cpp.o CMakeFiles/rrt-viewer.dir/qrc_main.cpp.o CMakeFiles/rrt-viewer.dir/rrt-viewer_automoc.cpp.o  -o rrt-viewer  -rdynamic /usr/lib64/libQt5Widgets.so.5.7.1 /usr/lib64/libQt5Quick.so.5.7.1 libRRT.a /usr/lib64/libQt5Qml.so.5.7.1 /usr/lib64/libQt5Network.so.5.7.1 /usr/lib64/libQt5Gui.so.5.7.1 /usr/lib64/libQt5Core.so.5.7.1 && :
/usr/lib/gcc/x86_64-pc-linux-gnu/5.4.0/../../../../x86_64-pc-linux-gnu/bin/ld: CMakeFiles/rrt-viewer.dir/src/rrt-viewer/RRTWidget.cpp.o: undefined reference to symbol 'LZ4_resetStreamHC'
/usr/lib64/liblz4.so.1: error adding symbols: DSO missing from command line
collect2: error: ld returned 1 exit status
ninja: build stopped: subcommand failed.

On ubuntu xenial's version (1.8.4) I don't see this issue though. Weird.

[joshhting]

Uh it appears I broke something when I pulled your branch

[jgkamat]

I'll fix it, one sec

[joshhting]

Great! :D Thanks for your guys' patience, I was in Vegas last week so I didn't have time to work on this then. PR is finally ready for code review again

[joshhting]

Memory leaks and such should be gone now.

[joshhting]

Bump @ashaw596 @JNeiger