Using Matrix instead of Vetor from Eigen library