Use eigen version depending on what KDL uses

* Now matching KDL include pathes to find out if KDL uses eigen2 or eigen3 (when KDL is found)
* Finding and including the same eigen version as KDL (if no KDL, use eigen2)
* Adding EIGEN2_SUPPORT when using eigen3, until CBF is migrated to eigen3

fixes #2