Move some function to Eigen