Generalized some functions with VertexSE2 to OptimizableGraph::Vertex