diff --git a/.travis.yml b/.travis.yml
index d75249e65a5971b7b9c333a4f9b182ff6f9ccb9d..41ee059e9f012be906727327139b2b71d8e3c7cc 100644
--- a/.travis.yml
+++ b/.travis.yml
@@ -23,6 +23,8 @@ matrix:
             - libboost-all-dev
             - libassimp-dev
             - libeigen3-dev
+            - python-lxml
+            - python3-lxml
 
     - name: "Xenial - Release - g++"
       env: BUILD_TYPE=Release
@@ -36,6 +38,8 @@ matrix:
             - libassimp-dev
             - libeigen3-dev
             - liboctomap-dev
+            - python-lxml
+            - python3-lxml
 
     - name: "Bionic - Release - g++"
       env: BUILD_TYPE=Release
@@ -49,6 +53,8 @@ matrix:
             - libassimp-dev
             - libeigen3-dev
             - liboctomap-dev
+            - python-lxml
+            - python3-lxml
 
     - name: "Bionic - Debug - g++"
       env: BUILD_TYPE=Debug
@@ -62,6 +68,8 @@ matrix:
             - libassimp-dev
             - libeigen3-dev
             - liboctomap-dev
+            - python-lxml
+            - python3-lxml
 
     - name: "OSX - Release - clang"
       env: BUILD_TYPE=Release
@@ -79,12 +87,11 @@ before_install:
 script:
   # Create build directory
   - which python
-  - which python2
   - mkdir build
   - cd build
 
   # Configure
-  - cmake -DCMAKE_BUILD_TYPE=$BUILD_TYPE -DCMAKE_CXX_FLAGS=-w -DCMAKE_CXX_FLAGS_DEBUG=${CXX_FLAGS_DEBUG} -DPYTHON_EXECUTABLE=$(which python2) ..
+  - cmake -DCMAKE_BUILD_TYPE=$BUILD_TYPE -DCMAKE_CXX_FLAGS=-w -DCMAKE_CXX_FLAGS_DEBUG=${CXX_FLAGS_DEBUG} -DPYTHON_EXECUTABLE=$(which python) ..
 
   # Build
   - make
diff --git a/CMakeLists.txt b/CMakeLists.txt
index d0a563a67cc399948e83761b27552fea7e6d2725..7f0420233708e63e76aaa77897f10119ede0ac89 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -41,8 +41,9 @@ set(PROJECT_DESCRIPTION
   )
 SET(PROJECT_USE_CMAKE_EXPORT TRUE)
 
-SET(CMAKE_C_STANDARD 99)
-SET(CMAKE_CXX_STANDARD 98)
+IF(NOT DEFINED CMAKE_CXX_STANDARD)
+  SET(CMAKE_CXX_STANDARD 98)
+ENDIF()
 
 # Do not support CMake older than 2.8.12
 CMAKE_POLICY(SET CMP0022 NEW)
diff --git a/doc/Doxyfile.extra.in b/doc/Doxyfile.extra.in
index 16c2c547b955dd182889e191dba47249f0ec6b49..61570774331d950ab23f3d03298d1336bd59f191 100644
--- a/doc/Doxyfile.extra.in
+++ b/doc/Doxyfile.extra.in
@@ -1 +1,2 @@
 FILE_PATTERNS          = *.h  *.hh  *.hxx
+GENERATE_XML           = YES
diff --git a/doc/python/doxygen-boost.hh b/doc/python/doxygen-boost.hh
new file mode 100644
index 0000000000000000000000000000000000000000..763b308be32b75ae15e829af05b01178564f5036
--- /dev/null
+++ b/doc/python/doxygen-boost.hh
@@ -0,0 +1,42 @@
+#ifndef DOXYGEN_BOOST_DOC_HH
+#define DOXYGEN_BOOST_DOC_HH
+
+#ifndef DOXYGEN_DOC_HH
+# error "You should have included doxygen.hh first."
+#endif // DOXYGEN_DOC_HH
+
+#include <boost/python.hpp>
+
+namespace doxygen
+{
+
+namespace visitor
+{
+
+template <typename function_type>
+struct member_func_impl : boost::python::def_visitor<member_func_impl<function_type> >
+{
+  member_func_impl(const char* n, function_type f) : name(n), function(f) {}
+  template <class classT>
+  inline void visit(classT& c) const
+  {
+    c.def(name, function, doxygen::member_func_doc(function));
+  }
+
+  const char* name;
+  function_type function;
+};
+
+// TODO surprisingly, this does not work when defined here but it works when
+// defined after the generated files are included.
+template <typename function_type>
+inline member_func_impl<function_type> member_func (const char* name, function_type function)
+{
+  return member_func_impl<function_type>(name, function);
+}
+
+} // namespace visitor
+
+} // namespace doxygen
+
+#endif // DOXYGEN_BOOST_DOC_HH
diff --git a/doc/python/doxygen.hh b/doc/python/doxygen.hh
new file mode 100644
index 0000000000000000000000000000000000000000..20a0bb3e25c889f5527767e3d4d5f6ffa33eeab2
--- /dev/null
+++ b/doc/python/doxygen.hh
@@ -0,0 +1,107 @@
+#ifndef DOXYGEN_DOC_HH
+#define DOXYGEN_DOC_HH
+
+#include <boost/preprocessor/repetition.hpp>
+#include <boost/preprocessor/punctuation/comma_if.hpp>
+
+#ifndef DOXYGEN_DOC_MAX_NUMBER_OF_ARGUMENTS_IN_CONSTRUCTOR
+#define DOXYGEN_DOC_MAX_NUMBER_OF_ARGUMENTS_IN_CONSTRUCTOR 10
+#endif
+
+namespace doxygen
+{
+template <typename _class>
+struct class_doc_impl
+{
+static inline const char* run ()
+{
+  return "";
+}
+static inline const char* attribute (const char*)
+{
+  return "";
+}
+};
+
+template <typename _class>
+inline const char* class_doc ()
+{
+  return class_doc_impl<_class>::run();
+}
+
+template <typename _class>
+inline const char* class_attrib_doc (const char* name)
+{
+  return class_doc_impl<_class>::attribute(name);
+}
+
+template <typename FuncPtr>
+inline const char* member_func_doc (FuncPtr)
+{
+  return "";
+}
+
+#define DOXYGEN_DOC_DECLARE_CONSTRUCTOR(z,nargs,unused)                        \
+template <                                                                     \
+  typename Class                                                               \
+  BOOST_PP_COMMA_IF(nargs)                                                     \
+  BOOST_PP_ENUM_PARAMS(nargs, class Arg)>                                      \
+struct constructor_doc_##nargs##_impl {                                        \
+static inline const char* run ()                                               \
+{                                                                              \
+  return "";                                                                   \
+}                                                                              \
+};                                                                             \
+                                                                               \
+template <                                                                     \
+  typename Class                                                               \
+  BOOST_PP_COMMA_IF(nargs)                                                     \
+  BOOST_PP_ENUM_PARAMS(nargs, class Arg)>                                      \
+inline const char* constructor_doc ()                                          \
+{                                                                              \
+  return constructor_doc_##nargs##_impl<                                       \
+    Class                                                                      \
+    BOOST_PP_COMMA_IF(nargs)                                                   \
+    BOOST_PP_ENUM_PARAMS(nargs, Arg)>::run();                                  \
+}
+
+BOOST_PP_REPEAT(DOXYGEN_DOC_MAX_NUMBER_OF_ARGUMENTS_IN_CONSTRUCTOR, DOXYGEN_DOC_DECLARE_CONSTRUCTOR, ~)
+
+/*
+template <typename Class>
+inline const char* constructor_doc ()
+{
+  return "";
+}
+*/
+
+template <typename Class>
+struct destructor_doc_impl
+{
+static inline const char* run ()
+{
+  return "";
+}
+};
+
+template <typename Class>
+inline const char* destructor_doc ()
+{
+  return destructor_doc_impl<Class>::run();
+}
+
+/* TODO class attribute can be handled by
+
+template <typename Class, typename AttributeType>
+const char* attribute_doc (AttributeType Class::* ptr)
+{
+  // Body looks like
+  // if (ptr == &Class::attributeName)
+  //   return "attrib documentation";
+  return "undocumented";
+}
+*/
+
+} // namespace doxygen
+
+#endif // DOXYGEN_DOC_HH
diff --git a/doc/python/doxygen_xml_parser.py b/doc/python/doxygen_xml_parser.py
new file mode 100755
index 0000000000000000000000000000000000000000..448d6a271f013712373f62569993ee0dcf9f6c5e
--- /dev/null
+++ b/doc/python/doxygen_xml_parser.py
@@ -0,0 +1,630 @@
+#!/usr/bin/python3
+
+from __future__ import print_function
+from lxml import etree
+from os import path
+from xml_docstring import XmlDocString
+import sys
+
+template_file_header = \
+"""#ifndef DOXYGEN_AUTODOC_{header_guard}
+#define DOXYGEN_AUTODOC_{header_guard}
+
+#include "{path}/doxygen.hh"
+"""
+template_file_footer = \
+"""
+#endif // DOXYGEN_AUTODOC_{header_guard}
+"""
+
+template_class_doc = \
+"""
+template <{tplargs}>
+struct class_doc_impl< {classname} >
+{{
+static inline const char* run ()
+{{
+  return "{docstring}";
+}}
+static inline const char* attribute (const char* attrib)
+{{{attributes}
+  (void)attrib; // turn off unused parameter warning.
+  return "";
+}}
+}};"""
+template_class_attribute_body = \
+"""
+  if (strcmp(attrib, "{attribute}") == 0)
+    return "{docstring}";"""
+template_constructor_doc = \
+"""
+template <{tplargs}>
+struct constructor_doc_{nargs}_impl< {classname_prefix}{comma}{argsstring} >
+{{
+static inline const char* run ()
+{{
+  return "{docstring}";
+}}
+}};"""
+template_destructor_doc = \
+"""
+template <{tplargs}>
+struct destructor_doc_impl < {classname_prefix} >
+{{
+static inline const char* run ()
+{{
+  return "{docstring}";
+}}
+}};"""
+template_member_func_doc = \
+"""
+{template}inline const char* member_func_doc ({rettype} ({classname_prefix}*function_ptr) {argsstring})
+{{{body}
+  return "";
+}}"""
+template_member_func_doc_body = \
+"""
+  if (function_ptr == static_cast<{rettype} ({classname_prefix}*) {argsstring}>(&{classname_prefix}{membername}))
+    return "{docstring}";"""
+template_static_func_doc = \
+"""
+{template}inline const char* member_func_doc ({rettype} (*function_ptr) {argsstring})
+{{{body}
+  return "";
+}}"""
+template_static_func_doc_body = \
+"""
+  if (function_ptr == static_cast<{rettype} (*) {argsstring}>(&{namespace}::{membername}))
+    return "{docstring}";"""
+template_open_namespace = \
+"""namespace {namespace} {{"""
+template_close_namespace = \
+"""}} // namespace {namespace}"""
+template_include_intern = \
+"""#include "{filename}"
+"""
+template_include_extern = \
+"""#include <{filename}>
+"""
+
+def _templateParamToDict (param):
+    type = param.find('type')
+    declname = param.find('declname')
+    defname  = param.find('defname')
+    # FIXME type may contain references in two ways:
+    # - the real param type
+    # - the name of the template argument is recognized as the name of a type...
+    if defname is None and declname is None:
+        typetext = type.text
+        for c in type.iter():
+            if c == type: continue
+            if c.text is not None: typetext += c.text
+            if c.tail is not None: typetext += c.tail
+        if typetext.startswith ("typename") or typetext.startswith ("class"):
+            s = typetext.split(maxsplit=1)
+            assert len(s) == 2
+            return { "type": s[0].strip(), "name": s[1].strip() }
+        else:
+            return { "type": type.text, "name": "" }
+    else:
+        assert defname.text == declname.text
+        return { "type": type.text, "name": defname.text }
+
+def makeHeaderGuard (filename):
+    import os
+    return filename.upper().replace('.', '_').replace(os.path.sep, '_')
+
+def format_description (brief, detailed):
+    b = [ el.text.strip() for el in brief   .iter() if el.text ] if brief    is not None else []
+    d = [ el.text.strip() for el in detailed.iter() if el.text ] if detailed is not None else []
+    text = "".join(b)
+    if d:
+        text += '\n' + "".join(d)
+    return text
+
+class Reference(object):
+    def __init__ (self, index, id=None, name=None):
+        self.id = id
+        self.name = name
+        self.index = index
+
+    def xmlToType (self, node, array=None, parentClass=None, tplargs=None):
+        """
+        - node:
+        - parentClass: a class
+        - tplargs: if one of the args is parentClass and no template arguments are provided,
+                   set the template arguments to this value
+        - array: content of the sibling tag 'array'
+        """
+        if node.text is not None:
+            t = node.text.strip()
+        else:
+            t = ""
+        for c in node.iterchildren():
+            if c.tag == "ref":
+                refid = c.attrib["refid"]
+                if parentClass is not None and refid == parentClass.id:
+                    t += " " + parentClass.name
+                    if c.tail is not None and c.tail.lstrip()[0] != '<':
+                        t += tplargs
+                elif self.index.hasref(refid):
+                    t += " " + self.index.getref(refid).name
+                else:
+                    self.index.output.warn ("Unknown reference: ", c.text, refid)
+                    t += " " + c.text.strip()
+            else:
+                if c.text is not None:
+                    t += " " + c.text.strip()
+            if c.tail is not None:
+                t += " " + c.tail.strip()
+        if array is not None:
+            t += array.text
+        return t
+
+# Only for function as of now.
+class MemberDef(Reference):
+    def __init__ (self, index, memberdefxml, parent):
+        super().__init__ (index=index,
+                id = memberdefxml.attrib["id"],
+                name = memberdefxml.find("definition").text)
+        self.parent = parent
+
+        self.xml = memberdefxml
+        self.const = (memberdefxml.attrib['const']=="yes")
+        self.static = (memberdefxml.attrib['static']=="yes")
+        self.rettype = memberdefxml.find('type')
+        self.params = tuple( [ (param.find('type'), param.find('array')) for param in self.xml.findall("param") ] )
+        self.special = self.rettype.text is None and len(self.rettype.getchildren())==0
+        #assert self.special or len(self.rettype.text) > 0
+
+        self._templateParams (self.xml.find('templateparamlist'))
+
+    def _templateParams (self, tpl):
+        if tpl is not None:
+            self.template_params = tuple ([ _templateParamToDict(param) for param in tpl.iterchildren(tag="param") ])
+        else:
+            self.template_params = tuple()
+
+    def prototypekey (self):
+        prototype = (
+                self.xmlToType(self.rettype),
+                tuple( [ tuple(t.items()) for t in self.template_params ]),
+                tuple( [ self.xmlToType(param.find('type')) for param in self.xml.findall("param") ] ),
+                self.const,
+                )
+        return prototype
+
+    def s_prototypeArgs (self):
+        return "({0}){1}".format (self.s_args(), " const" if self.const else "")
+
+    def s_args (self):
+        # If the class is templated, check if one of the argument is the class itself.
+        # If so, we must add the template arguments to the class (if there is none)
+
+        if len(self.parent.template_params) > 0:
+            tplargs = " <" + ", ".join([ d['name'] for d in self.parent.template_params ]) + " > "
+            args = ", ".join(
+                    [ self.xmlToType(type, array, parentClass=self.parent, tplargs=tplargs) for type,array in self.params])
+        else:
+            args = ", ".join([ self.xmlToType(type, array) for type, array in self.params])
+        return args
+
+    def s_tpldecl (self):
+        if len(self.template_params) == 0: return ""
+        return ", ".join([ d['type'] + " " + d['name'] for d in self.template_params ])
+
+    def s_rettype (self):
+        assert not self.special
+        return self.xmlToType(self.rettype)
+
+    def s_name (self):
+        return self.xml.find('name').text.strip()
+
+    def s_docstring (self):
+        return self.index.xml_docstring.getDocString (
+                self.xml.find('briefdescription'),
+                self.xml.find('detaileddescription'),
+                self.index.output)
+
+    def include (self):
+        import os.path
+        loc = self.xml.find('location')
+        # The location is based on $CMAKE_SOURCE_DIR. Remove first directory.
+        return loc.attrib['file'].split(os.path.sep,1)[1]
+
+class CompoundBase(Reference):
+    def __init__ (self, compound, index):
+        self.compound = compound
+        self.filename = path.join (index.directory, compound.attrib["refid"]+".xml")
+        self.tree = etree.parse (self.filename)
+        self.definition = self.tree.getroot().find("compounddef")
+        super().__init__ (index,
+                id = self.definition.attrib['id'],
+                name = self.definition.find("compoundname").text)
+
+class NamespaceCompound (CompoundBase):
+    def __init__ (self, *args):
+        super().__init__ (*args)
+        self.typedefs = []
+        self.enums = []
+        self.static_funcs = []
+        self.template_params = tuple()
+
+        # Add references
+        for section in self.definition.iterchildren("sectiondef"):
+            assert "kind" in section.attrib
+            kind = section.attrib["kind"]
+            if kind == "enum":
+                self.parseEnumSection (section)
+            elif kind == "typedef":
+                self.parseTypedefSection (section)
+            elif kind == "func":
+                self.parseFuncSection (section)
+
+    def parseEnumSection (self, section):
+        for member in section.iterchildren("memberdef"):
+            ref = Reference (index=self.index,
+                    id=member.attrib["id"],
+                    name= self.name + "::" + member.find("name").text)
+            self.index.registerReference (ref)
+            self.enums.append(member)
+            for value in member.iterchildren("enumvalue"):
+                ref = Reference (index=self.index,
+                        id=value.attrib["id"],
+                        name= self.name + "::" + member.find("name").text)
+
+    def parseTypedefSection (self, section):
+        for member in section.iterchildren("memberdef"):
+            ref = Reference (index=self.index,
+                    id=member.attrib["id"],
+                    name= self.name + "::" + member.find("name").text)
+            self.index.registerReference (ref)
+            self.typedefs.append(member)
+
+    def parseFuncSection (self, section):
+        for member in section.iterchildren("memberdef"):
+            self.static_funcs.append (MemberDef (self.index, member, self))
+
+    def innerNamespace (self):
+        return self.name
+
+    def write (self, output):
+        pass
+
+class ClassCompound (CompoundBase):
+    def __init__ (self, *args):
+        super().__init__ (*args)
+        self.member_funcs = list()
+        self.static_funcs = list()
+        self.special_funcs = list()
+        self.attributes = list()
+
+        self.struct = (self.compound.attrib['kind'] == "struct")
+        self.public = (self.definition.attrib['prot'] == "public")
+        self.template_specialization = (self.name.find('<') > 0)
+
+        # Handle templates
+        self._templateParams (self.definition.find('templateparamlist'))
+        for memberdef in self.definition.iter(tag="memberdef"):
+            if memberdef.attrib['prot'] != "public":
+                continue
+            if memberdef.attrib['kind'] == "variable":
+                self._attribute (memberdef)
+            elif memberdef.attrib['kind'] == "typedef":
+                ref = Reference (index=self.index,
+                        id=memberdef.attrib["id"],
+                        name= self.name + "::" + memberdef.find("name").text)
+                self.index.registerReference (ref)
+            elif memberdef.attrib['kind'] == "enum":
+                ref = Reference (index=self.index,
+                        id=memberdef.attrib["id"],
+                        name= self.name + "::" + memberdef.find("name").text)
+                self.index.registerReference (ref)
+                for value in memberdef.iterchildren("enumvalue"):
+                    ref = Reference (index=self.index,
+                            id=value.attrib["id"],
+                            name= self.name + "::" + memberdef.find("name").text)
+                    self.index.registerReference (ref)
+            elif memberdef.attrib['kind'] == "function":
+                self._memberfunc (memberdef)
+
+    def _templateParams (self, tpl):
+        if tpl is not None:
+            self.template_params = tuple([ _templateParamToDict(param) for param in tpl.iterchildren(tag="param") ])
+        else:
+            self.template_params = tuple()
+
+    def _templateDecl (self):
+        if not hasattr(self, "template_params") or len(self.template_params) == 0:
+            return ""
+        return ", ".join([ d['type'] + " " + d['name'] for d in self.template_params ])
+
+    def _className (self):
+        if not hasattr(self, "template_params") or len(self.template_params) == 0:
+            return self.name
+        return self.name + " <" + ", ".join([ d['name'] for d in self.template_params ]) + " >"
+
+    def innerNamespace (self):
+        return self._className()
+
+    def _memberfunc (self, member):
+        m = MemberDef (self.index, member, self)
+        if m.special:
+            self.special_funcs.append (m)
+        elif m.static:
+            self.static_funcs.append (m)
+        else:
+            self.member_funcs.append (m)
+
+    def _writeClassDoc (self, output):
+        docstring = self.index.xml_docstring.getDocString (
+                self.definition.find('briefdescription'),
+                self.definition.find('detaileddescription'),
+                self.index.output)
+        attribute_docstrings = ""
+        for member in self.attributes:
+            _dc = self.index.xml_docstring.getDocString(
+                member.find('briefdescription'),
+                member.find('detaileddescription'),
+                self.index.output)
+            if len(_dc) == 0: continue
+            attribute_docstrings += template_class_attribute_body.format (
+                    attribute = member.find('name').text,
+                    docstring = _dc,
+                    )
+        if len(docstring) == 0 and len(attribute_docstrings) == 0: return
+        output.out (template_class_doc.format (
+            tplargs = self._templateDecl(),
+            classname = self._className(),
+            docstring = docstring,
+            attributes = attribute_docstrings,
+            ))
+
+    def write (self, output):
+        if not self.public: return
+        if self.template_specialization:
+            output.warn ("Disable class {} because template argument are not resolved for templated class specialization.".format(self.name))
+            return
+
+        include = self.definition.find('includes')
+        output.open (include.text)
+        output.out (template_include_extern.format (filename=include.text))
+        output.out (template_open_namespace.format (namespace="doxygen"))
+
+        # Write class doc
+        self._writeClassDoc(output)
+
+        # Group member function by prototype
+        member_funcs = dict()
+        for m in self.member_funcs:
+            prototype = m.prototypekey()
+            if prototype in member_funcs:
+                member_funcs[prototype].append (m)
+            else:
+                member_funcs[prototype] = [ m, ]
+
+        classname_prefix = self._className() + "::"
+
+        for member in self.special_funcs:
+            docstring = member.s_docstring()
+            if len(docstring) == 0: continue
+            if member.s_name()[0] == '~':
+                output.out (template_destructor_doc.format (
+                    tplargs = self._templateDecl(),
+                    classname_prefix = self._className(),
+                    docstring = docstring,
+                    ))
+            else:
+                output.out (template_constructor_doc.format (
+                    tplargs = ", ".join([ d['type'] + " " + d['name'] for d in self.template_params + member.template_params ]),
+                    nargs = len(member.params),
+                    comma = ", " if len(member.params) > 0 else "",
+                    classname_prefix = self._className(),
+                    argsstring = member.s_args(),
+                    docstring = docstring,
+                    ))
+
+        for prototype, members in member_funcs.items():
+            # remove undocumented members
+            documented_members = []
+            docstrings = []
+            for member in members:
+                docstring = member.s_docstring()
+                if len(docstring) == 0: continue
+                documented_members.append (member)
+                docstrings.append (docstring)
+            if len(documented_members) == 0: continue
+
+            body = "".join([
+                template_member_func_doc_body.format (
+                    classname_prefix = classname_prefix,
+                    membername = member.s_name(),
+                    docstring = docstring,
+                    rettype = member.s_rettype(),
+                    argsstring = member.s_prototypeArgs(),
+                    )
+                for member, docstring in zip(documented_members,docstrings) ])
+
+            member = members[0]
+            tplargs = ", ".join([ d['type'] + " " + d['name'] for d in self.template_params + member.template_params ])
+            output.out (template_member_func_doc.format (
+                template = "template <{}>\n".format (tplargs) if len(tplargs) > 0 else "",
+                rettype = member.s_rettype(),
+                classname_prefix = classname_prefix,
+                argsstring = member.s_prototypeArgs(),
+                body = body
+                ))
+
+        output.out (template_close_namespace.format (namespace="doxygen"))
+        output.close()
+
+    def _attribute (self, member):
+        self.attributes.append (member)
+
+class Index:
+    """
+    This class is responsible for generating the list of all C++-usable documented elements.
+    """
+    def __init__ (self, input, output):
+        self.tree = etree.parse (input)
+        self.directory = path.dirname (input)
+        self.xml_docstring = XmlDocString (self)
+        self.compounds = list()
+        self.references = dict()
+        self.output = output
+
+    def parseCompound (self):
+        for compound in self.tree.getroot().iterchildren ("compound"):
+            if compound.attrib['kind'] in ["class", "struct"]:
+                obj = ClassCompound (compound, self)
+            elif compound.attrib['kind'] == "namespace":
+                obj = NamespaceCompound (compound, self)
+            if obj.id not in self.compounds:
+                self.compounds.append (obj.id)
+            self.registerReference (obj)
+
+    def write (self):
+        # Header
+        from os.path import abspath, dirname
+        from time import asctime
+
+        self.output.open ("doxygen_xml_parser_for_cmake.hh")
+        self.output.out ("// Generated on {}".format (asctime()))
+        self.output.close()
+
+        # Implement template specialization for classes and member functions
+        for id in self.compounds:
+            compound = self.references[id]
+            compound.write(self.output)
+
+        self.output.open ("functions.h")
+
+        # Implement template specialization for static functions
+        static_funcs = dict()
+        prototypes = list()
+        includes = list()
+        for id in self.compounds:
+            compound = self.references[id]
+            for m in compound.static_funcs:
+                include = m.include()
+                if include not in includes:
+                    includes.append (include)
+                docstring = m.s_docstring()
+                if len(docstring) == 0: continue
+                prototype = m.prototypekey()
+                if prototype in static_funcs:
+                    static_funcs[prototype].append ( (m, docstring) )
+                else:
+                    static_funcs[prototype] = [ (m, docstring) , ]
+                    prototypes.append (prototype)
+
+        self.output.out (
+                "".join([ template_include_intern.format (filename=filename)
+                    for filename in includes]))
+
+        self.output.out (template_open_namespace.format (namespace="doxygen"))
+
+        for prototype in prototypes:
+            member_and_docstring_s = static_funcs[prototype]
+            body = "".join([
+                template_static_func_doc_body.format (
+                    namespace = member.parent.innerNamespace(),
+                    membername = member.s_name(),
+                    docstring = docstring,
+                    rettype = member.s_rettype(),
+                    argsstring = member.s_prototypeArgs(),
+                    )
+                for member, docstring in member_and_docstring_s ])
+
+            member = member_and_docstring_s[0][0]
+            # TODO fix case of static method in templated class.
+            tplargs = ", ".join([ d['type'] + " " + d['name'] for d in member.parent.template_params + member.template_params ])
+            self.output.out (template_static_func_doc.format (
+                template = "template <{}>\n".format (tplargs) if len(tplargs) > 0 else "",
+                rettype = member.s_rettype(),
+                argsstring = member.s_prototypeArgs(),
+                body = body
+                ))
+
+        self.output.out (template_close_namespace.format (namespace="doxygen"))
+        self.output.close ()
+
+    def registerReference (self, obj, overwrite=True):
+        if obj.id in self.references:
+            if obj.name != self.references[obj.id].name:
+                self.output.warn ("!!!! Compounds " + obj.id + " already exists.", obj.name, self.references[obj.id].name)
+            else:
+                self.output.warn ("Reference " + obj.id + " already exists.", obj.name)
+            if not overwrite: return
+        self.references[obj.id] = obj
+
+    def hasref (self, id):
+        return (id in self.references)
+
+    def getref (self, id):
+        return self.references[id]
+
+class OutputStreams(object):
+    def __init__ (self, output_dir, warn, error, errorPrefix = ""):
+        self.output_dir = output_dir
+        self._out = None
+        self._warn = warn
+        self._err = error
+        self.errorPrefix = errorPrefix
+
+        self._created_files = dict()
+
+    def open (self, name):
+        assert self._out == None, "You did not close the previous file"
+        import os
+        fullname = os.path.join(self.output_dir, name)
+        dirname = os.path.dirname(fullname)
+        if not os.path.isdir (dirname): os.makedirs (dirname)
+
+        if name in self._created_files:
+            self._out = self._created_files[name]
+        else:
+            self._out = open(fullname, mode='w')
+            self._created_files[name] = self._out
+
+            # Header
+            self.out(template_file_header.format (
+                path = os.path.dirname(os.path.abspath(__file__)),
+                header_guard = makeHeaderGuard (name),
+                ))
+
+    def close (self):
+        self._out = None
+
+    def writeFooterAndCloseFiles (self):
+        for n, f in self._created_files.items():
+            # Footer
+            self._out = f
+            self.out(template_file_footer.format(
+                header_guard = makeHeaderGuard (n),
+                ))
+            f.close()
+        self._created_files.clear()
+        self._out = None
+
+    def out(self, *args):
+        print (*args, file=self._out)
+    def warn(self, *args):
+        print (self.errorPrefix, *args, file=self._warn)
+    def err(self, *args):
+        print (self.errorPrefix, *args, file=self._err)
+
+if __name__ == "__main__":
+    import argparse
+
+    parser = argparse.ArgumentParser(description='Process Doxygen XML documentation and generate C++ code.')
+    parser.add_argument('doxygen_index_xml', type=str, help='the Doxygen XML index.')
+    parser.add_argument('output_directory', type=str, help='the output directory.')
+    args = parser.parse_args()
+
+    index = Index (input = sys.argv[1],
+            output = OutputStreams (args.output_directory, sys.stdout, sys.stderr))
+    index.parseCompound()
+    index.write()
+    index.output.writeFooterAndCloseFiles()
+    assert index.output._out == None
diff --git a/doc/python/xml_docstring.py b/doc/python/xml_docstring.py
new file mode 100644
index 0000000000000000000000000000000000000000..2ff48dff1ff5c3bb2b7095d3348340b9d423fa14
--- /dev/null
+++ b/doc/python/xml_docstring.py
@@ -0,0 +1,131 @@
+class XmlDocString (object):
+    def __init__ (self, index):
+        self.index = index
+        self.tags = {
+                "para": self.para,
+                "ref": self.ref,
+                "briefdescription": self.otherTags,
+                "detaileddescription": self.otherTags,
+                "parameterlist": self.parameterlist,
+                "parameterdescription": self.otherTags,
+                "emphasis": self.emphasis,
+                "simplesect": self.simplesect,
+                }
+        self.unkwownTags = set()
+        self.unkwownReferences = dict()
+        self._linesep = "\\n\"\n\""
+
+    def clear (self):
+        self.lines = []
+        self.unkwownTags.clear()
+        self.unkwownReferences.clear()
+
+    def writeErrors (self, output):
+        ret = False
+        for t in self.unkwownTags:
+            output.warn ("Unknown tag: ", t)
+            ret = True
+        for ref,node in self.unkwownReferences.items():
+            output.warn ("Unknown reference: ", ref, node.text)
+            ret = True
+        return ret
+
+    def _write (self, str):
+        nlines=str.split(sep="\n")
+        if len(self.lines)==0:
+            self.lines += nlines
+        else:
+            self.lines[-1] += nlines[0]
+            self.lines += nlines[1:]
+            #self.lines += nlines[1:]
+
+    def _newline (self,n=1):
+        self.lines.extend (["",] * n)
+
+    def _clean(self):
+        s = 0
+        for l in self.lines:
+            if len(l.strip())==0: s+=1
+            else: break
+        e = len(self.lines)
+        for l in reversed(self.lines):
+            if len(l.strip())==0: e-=1
+            else: break
+        self.lines = self.lines[s:e]
+
+    def getDocString (self, brief, detailled, output):
+        self.clear()
+        if brief is not None:
+            self.visit (brief)
+        if detailled is not None and len(detailled.getchildren()) > 0:
+            if brief is not None: self._newline ()
+            self.visit (detailled)
+        from sys import stdout, stderr
+        self.writeErrors(output)
+        self._clean()
+        return self._linesep.join(self.lines)
+
+    def visit (self, node):
+        assert isinstance(node.tag, str)
+        tag = node.tag
+        if tag not in self.tags:
+            self.unknownTag (node)
+        else:
+            self.tags[tag](node)
+
+    def unknownTag (self, node):
+        self.unkwownTags.add (node.tag)
+        self.otherTags (node)
+
+    def otherTags (self, node):
+        if node.text:
+            self._write (node.text.strip())
+        for c in node.iterchildren():
+            self.visit (c)
+            if c.tail: self._write (c.tail.strip())
+
+    def emphasis (self, node):
+        self._write ("*")
+        self.otherTags(node)
+        self._write ("*")
+
+    def simplesect (self, node):
+        self._write (node.attrib["kind"].title()+": ")
+        self.otherTags (node)
+
+    def para (self, node):
+        if node.text: self._write (node.text)
+        for c in node.iterchildren():
+            self.visit (c)
+            if c.tail: self._write (c.tail)
+        self._newline()
+
+    def ref (self, node):
+        refid = node.attrib["refid"]
+        if self.index.hasref(refid):
+            self._write (self.index.getref(refid).name)
+        else:
+            self.unkwownReferences[refid] = node
+            self._write (node.text)
+        assert len(node.getchildren()) == 0
+
+    def parameterlist (self, node):
+        self._newline()
+        self._write (node.attrib["kind"].title())
+        self._newline()
+        for item in node.iterchildren("parameteritem"):
+            self.parameteritem (item)
+
+    def parameteritem (self, node):
+        indent = "  "
+        self._write (indent + "- ")
+        # should contain two children
+        assert len(node.getchildren()) == 2
+        namelist = node.find ("parameternamelist")
+        desc     = node.find ("parameterdescription")
+        sep = ""
+        for name in namelist.iterchildren("parametername"):
+            self._write (sep + name.text)
+            sep = ", "
+        self._write (" ")
+        self.visit (desc)
diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt
index 1730b56118dbcf504bc0220fcb7692a08e3be09c..0c55085c34a0b857f4de8c0d81a1882a12c10751 100644
--- a/python/CMakeLists.txt
+++ b/python/CMakeLists.txt
@@ -40,9 +40,37 @@ SET(LIBRARY_NAME hppfcl)
 
 INCLUDE_DIRECTORIES("${Boost_INCLUDE_DIRS}" ${PYTHON_INCLUDE_DIRS})
 
+INCLUDE_DIRECTORIES("${CMAKE_SOURCE_DIR}/src")
+INCLUDE_DIRECTORIES("${CMAKE_CURRENT_BINARY_DIR}")
+
 SET(${LIBRARY_NAME}_HEADERS
   fcl.hh
-  ) 
+  )
+
+IF(NOT DOXYGEN_FOUND OR (APPLE AND PYTHON_VERSION_MAJOR LESS 3))
+  SET(ENABLE_DOXYGEN_AUTODOC FALSE)
+ELSE()
+  SET(ENABLE_DOXYGEN_AUTODOC TRUE)
+  IF(APPLE)
+    SET(PY_PREFIX ${PYTHON_EXECUTABLE})
+  ENDIF()
+ENDIF()
+IF(ENABLE_DOXYGEN_AUTODOC)
+  ADD_CUSTOM_TARGET(generate_doxygen_cpp_doc
+    COMMAND ${PY_PREFIX} ${CMAKE_SOURCE_DIR}/doc/python/doxygen_xml_parser.py
+    ${CMAKE_BINARY_DIR}/doc/doxygen-xml/index.xml
+    ${CMAKE_CURRENT_BINARY_DIR}/doxygen_autodoc > ${CMAKE_CURRENT_BINARY_DIR}/doxygen_autodoc.log
+    BYPRODUCTS
+    ${CMAKE_CURRENT_BINARY_DIR}/doxygen_autodoc/doxygen_xml_parser_for_cmake.hh
+    ${CMAKE_CURRENT_BINARY_DIR}/doxygen_autodoc.log
+    COMMENT "Generating Doxygen C++ documentation"
+    )
+  ADD_DEPENDENCIES(generate_doxygen_cpp_doc doc)
+
+  LIST(APPEND ${LIBRARY_NAME}_HEADERS
+    ${CMAKE_CURRENT_BINARY_DIR}/doxygen_autodoc/doxygen_xml_parser_for_cmake.hh
+    )
+ENDIF()
 
 SET(${LIBRARY_NAME}_SOURCES
   version.cc
@@ -57,6 +85,10 @@ ADD_LIBRARY(${LIBRARY_NAME} SHARED ${${LIBRARY_NAME}_SOURCES} ${${LIBRARY_NAME}_
 ADD_DEPENDENCIES(python ${LIBRARY_NAME})
 ADD_HEADER_GROUP(${LIBRARY_NAME}_HEADER)
 ADD_SOURCE_GROUP(${LIBRARY_NAME}_SOURCES)
+IF(ENABLE_DOXYGEN_AUTODOC)
+  ADD_DEPENDENCIES(${LIBRARY_NAME} generate_doxygen_cpp_doc)
+  TARGET_COMPILE_DEFINITIONS(${LIBRARY_NAME} PRIVATE -DHAS_DOXYGEN_AUTODOC)
+ENDIF()
 
 TARGET_LINK_BOOST_PYTHON(${LIBRARY_NAME} PUBLIC)
 TARGET_LINK_LIBRARIES(${LIBRARY_NAME} PUBLIC ${PROJECT_NAME} ${BOOST_system_LIBRARY})
diff --git a/python/collision-geometries.cc b/python/collision-geometries.cc
index b0a6f55938a281dd45d72ed03a689fd523cf7784..a3b90cda9eb1bc9bcb410963f5f2c0e54a612d91 100644
--- a/python/collision-geometries.cc
+++ b/python/collision-geometries.cc
@@ -43,6 +43,13 @@
 #include <hpp/fcl/shape/convex.h>
 #include <hpp/fcl/BVH/BVH_model.h>
 
+#ifdef HAS_DOXYGEN_AUTODOC
+#include "doxygen_autodoc/hpp/fcl/BVH/BVH_model.h"
+#include "doxygen_autodoc/hpp/fcl/shape/geometric_shapes.h"
+#endif
+
+#include "../doc/python/doxygen.hh"
+
 using namespace boost::python;
 
 using namespace hpp::fcl;
@@ -71,7 +78,7 @@ void exposeBVHModel (const std::string& bvname)
 
   std::string type = "BVHModel" + bvname;
   class_ <BVHModel_t, bases<BVHModelBase>, shared_ptr<BVHModel_t> >
-    (type.c_str(), init<>())
+    (type.c_str(), doxygen::class_doc<BVHModel_t>(), init<>(doxygen::constructor_doc<BVHModel_t>()))
     ;
 }
 
@@ -108,12 +115,12 @@ struct ConvexWrapper
 void exposeShapes ()
 {
   class_ <ShapeBase, bases<CollisionGeometry>, shared_ptr<ShapeBase>, noncopyable>
-    ("ShapeBase", no_init)
+    ("ShapeBase", doxygen::class_doc<ShapeBase>(), no_init)
     //.def ("getObjectType", &CollisionGeometry::getObjectType)
     ;
 
   class_ <Box, bases<ShapeBase>, shared_ptr<Box> >
-    ("Box", init<>())
+    ("Box", doxygen::class_doc<ShapeBase>(), init<>())
     .def (init<FCL_REAL,FCL_REAL,FCL_REAL>())
     .def (init<Vec3f>())
     .add_property("halfSide",
@@ -122,19 +129,19 @@ void exposeShapes ()
     ;
 
   class_ <Capsule, bases<ShapeBase>, shared_ptr<Capsule> >
-    ("Capsule", init<FCL_REAL, FCL_REAL>())
+    ("Capsule", doxygen::class_doc<Capsule>(), init<FCL_REAL, FCL_REAL>())
     .def_readwrite ("radius", &Capsule::radius)
     .def_readwrite ("halfLength", &Capsule::halfLength)
     ;
 
   class_ <Cone, bases<ShapeBase>, shared_ptr<Cone> >
-    ("Cone", init<FCL_REAL, FCL_REAL>())
+    ("Cone", doxygen::class_doc<Cone>(), init<FCL_REAL, FCL_REAL>())
     .def_readwrite ("radius", &Cone::radius)
     .def_readwrite ("halfLength", &Cone::halfLength)
     ;
 
   class_ <ConvexBase, bases<ShapeBase>, shared_ptr<ConvexBase >, noncopyable>
-    ("ConvexBase", no_init)
+    ("ConvexBase", doxygen::class_doc<ConvexBase>(), no_init)
     .def_readonly ("center", &ConvexBase::center)
     .def_readonly ("num_points", &ConvexBase::num_points)
     .def ("points", &ConvexBaseWrapper::points)
@@ -142,19 +149,19 @@ void exposeShapes ()
     ;
 
   class_ <Convex<Triangle>, bases<ConvexBase>, shared_ptr<Convex<Triangle> >, noncopyable>
-    ("Convex", no_init)
+    ("Convex", doxygen::class_doc< Convex<Triangle> >(), no_init)
     .def_readonly ("num_polygons", &Convex<Triangle>::num_polygons)
     .def ("polygons", &ConvexWrapper<Triangle>::polygons)
     ;
 
   class_ <Cylinder, bases<ShapeBase>, shared_ptr<Cylinder> >
-    ("Cylinder", init<FCL_REAL, FCL_REAL>())
+    ("Cylinder", doxygen::class_doc<Cylinder>(), init<FCL_REAL, FCL_REAL>())
     .def_readwrite ("radius", &Cylinder::radius)
     .def_readwrite ("halfLength", &Cylinder::halfLength)
     ;
 
   class_ <Halfspace, bases<ShapeBase>, shared_ptr<Halfspace> >
-    ("Halfspace", "The half-space is defined by {x | n * x < d}.", init<const Vec3f&, FCL_REAL>())
+    ("Halfspace", doxygen::class_doc<Halfspace>(), init<const Vec3f&, FCL_REAL>())
     .def (init<FCL_REAL,FCL_REAL,FCL_REAL,FCL_REAL>())
     .def (init<>())
     .def_readwrite ("n", &Halfspace::n)
@@ -162,7 +169,7 @@ void exposeShapes ()
     ;
 
   class_ <Plane, bases<ShapeBase>, shared_ptr<Plane> >
-    ("Plane", "The plane is defined by {x | n * x = d}.", init<const Vec3f&, FCL_REAL>())
+    ("Plane", doxygen::class_doc<Plane>(), init<const Vec3f&, FCL_REAL>())
     .def (init<FCL_REAL,FCL_REAL,FCL_REAL,FCL_REAL>())
     .def (init<>())
     .def_readwrite ("n", &Plane::n)
@@ -170,12 +177,12 @@ void exposeShapes ()
     ;
 
   class_ <Sphere, bases<ShapeBase>, shared_ptr<Sphere> >
-    ("Sphere", init<FCL_REAL>())
-    .def_readwrite ("radius", &Sphere::radius)
+    ("Sphere", doxygen::class_doc<Sphere>(), init<FCL_REAL>(doxygen::constructor_doc<Sphere>()))
+    .def_readwrite ("radius", &Sphere::radius, doxygen::class_attrib_doc<Sphere>("radius"))
     ;
 
   class_ <TriangleP, bases<ShapeBase>, shared_ptr<TriangleP> >
-    ("TriangleP", init<const Vec3f&, const Vec3f&, const Vec3f&>())
+    ("TriangleP", doxygen::class_doc<TriangleP>(), init<const Vec3f&, const Vec3f&, const Vec3f&>())
     .def_readwrite ("a", &TriangleP::a)
     .def_readwrite ("b", &TriangleP::b)
     .def_readwrite ("c", &TriangleP::c)
@@ -187,7 +194,7 @@ boost::python::tuple AABB_distance_proxy(const AABB & self, const AABB & other)
 {
   Vec3f P,Q;
   FCL_REAL distance = self.distance(other,&P,&Q);
-  return boost::python::tuple((distance,P,Q));
+  return boost::python::make_tuple(distance,P,Q);
 }
 
 void exposeCollisionGeometries ()
diff --git a/python/math.cc b/python/math.cc
index b392b0aab89e862131bbd2cc1dc844f53176b381..a1fcecd3a18017ee76609797000c7fcee11e8bc1 100644
--- a/python/math.cc
+++ b/python/math.cc
@@ -42,8 +42,14 @@
 
 #include "fcl.hh"
 
-using namespace boost::python;
+#ifdef HAS_DOXYGEN_AUTODOC
+#include "doxygen_autodoc/hpp/fcl/math/transform.h"
+#endif
+
+#include "../doc/python/doxygen.hh"
+#include "../doc/python/doxygen-boost.hh"
 
+using namespace boost::python;
 using namespace hpp::fcl;
 
 struct TriangleWrapper
@@ -69,30 +75,32 @@ void exposeMaths ()
   if(!eigenpy::register_symbolic_link_to_registered_type<Eigen::AngleAxisd>())
     eigenpy::exposeAngleAxis();
 
-  class_ <Transform3f> ("Transform3f", init<>("Default constructor."))
-    .def (init<Matrix3f, Vec3f>())
-    .def (init<Quaternion3f, Vec3f>())
-    .def (init<Matrix3f>())
-    .def (init<Quaternion3f>())
-    .def (init<Vec3f>())
-    .def (init<Transform3f>(args("self","other"),"Copy constructor."))
+  eigenpy::enableEigenPySpecific<Matrix3f>();
+  eigenpy::enableEigenPySpecific<Vec3f   >();
 
-    .def ("getQuatRotation", &Transform3f::getQuatRotation)
-    .def ("getTranslation", &Transform3f::getTranslation, return_value_policy<copy_const_reference>())
+  class_ <Transform3f> ("Transform3f", doxygen::class_doc<Transform3f>(), init<>(doxygen::constructor_doc<Transform3f>()))
+    .def (init<Matrix3f, Vec3f>    (doxygen::constructor_doc<Transform3f, const Matrix3f::MatrixBase&, const Vec3f::MatrixBase&>()))
+    .def (init<Quaternion3f, Vec3f>(doxygen::constructor_doc<Transform3f, const Quaternion3f&        , const Vec3f::MatrixBase&>()))
+    .def (init<Matrix3f>           (doxygen::constructor_doc<Transform3f, const Matrix3f&                                      >()))
+    .def (init<Quaternion3f>       (doxygen::constructor_doc<Transform3f, const Quaternion3f&                                  >()))
+    .def (init<Vec3f>              (doxygen::constructor_doc<Transform3f, const Vec3f&                                         >()))
+
+    .def ("getQuatRotation", &Transform3f::getQuatRotation, doxygen::member_func_doc(&Transform3f::getQuatRotation))
+    .def ("getTranslation", &Transform3f::getTranslation, doxygen::member_func_doc(&Transform3f::getTranslation), return_value_policy<copy_const_reference>())
     .def ("getRotation", &Transform3f::getRotation, return_value_policy<copy_const_reference>())
     .def ("isIdentity", &Transform3f::setIdentity)
 
-    .def ("setQuatRotation", &Transform3f::setQuatRotation)
+    .def (doxygen::visitor::member_func("setQuatRotation", &Transform3f::setQuatRotation))
     .def ("setTranslation", &Transform3f::setTranslation<Vec3f>)
     .def ("setRotation", &Transform3f::setRotation<Matrix3f>)
-    .def ("setTransform", &Transform3f::setTransform<Matrix3f,Vec3f>)
-    .def ("setTransform", static_cast<void (Transform3f::*)(const Quaternion3f&, const Vec3f&)>(&Transform3f::setTransform))
-    .def ("setIdentity", &Transform3f::setIdentity)
-
-    .def ("transform", &Transform3f::transform<Vec3f>)
-    .def ("inverseInPlace", &Transform3f::inverseInPlace, return_internal_reference<>())
-    .def ("inverse", &Transform3f::inverse)
-    .def ("inverseTimes", &Transform3f::inverseTimes)
+    .def (doxygen::visitor::member_func("setTransform", &Transform3f::setTransform<Matrix3f,Vec3f>))
+    .def (doxygen::visitor::member_func("setTransform", static_cast<void (Transform3f::*)(const Quaternion3f&, const Vec3f&)>(&Transform3f::setTransform)))
+    .def (doxygen::visitor::member_func("setIdentity", &Transform3f::setIdentity))
+
+    .def (doxygen::visitor::member_func("transform", &Transform3f::transform<Vec3f>))
+    .def ("inverseInPlace", &Transform3f::inverseInPlace, return_internal_reference<>(), doxygen::member_func_doc(&Transform3f::inverseInPlace))
+    .def (doxygen::visitor::member_func("inverse", &Transform3f::inverse))
+    .def (doxygen::visitor::member_func("inverseTimes", &Transform3f::inverseTimes))
 
     .def (self * self)
     .def (self *= self)