pass objects as const refs when needed, and avoid casting to MatrixXd at each step