The matrices being stored here seem to be sparse much of the time. Using Eigen's sparse matrix representation may speed things up.