Add CostFunctionToFunctor.

CostFunctionToFunctor wraps a CostFunction, and makes it available
as a templated functor that can be called from other templated
functors. This is useful for when one wants to mix automatic,
numeric and analytic differentiated functions.

Also a bug fix in autodiff.h

Change-Id: If8ba281a89fda976ef2ce10a5844a74c4ac7b84a