for 4x4 matrices implement the special algorithm that Markos proposed, falling back to the general algorithm in the bad case.
for 4x4 matrices implement the special algorithm that Markos proposed,
falling back to the general algorithm in the bad case.

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