Refactor matrix_function test in preparation of next commit.
Refactor matrix_function test in preparation of next commit.

robustrobotics/eigen,robustrobotics/eigen,robustrobotics/eigen,robustrobotics/eigen