Thrown away Eigen dependency in the whole optimization process
Removed all Eigen datastructures used internally