Changed code to use Eigen Matrix Library instead of std::vector.