using namespace Eigen
Massively clean up the code here. But only do this in .cpp files