I have a 4x4 matrix that is "nearly homogenous" (M, below). That is to say, `M[0:3,0:3] * M[0:3,0:3].tr()` is very nearly I(3), but not quite.

Each individual element of `M*M.tr()` matches I(3) to 3 places after the decimal, but together they add up to make the matrix non-homogenous in the eyes of `robomath.Mat.isHomogeneous()` since that has a threshold of 1e-4 for the sum of the difference with I(3).

How can I tweak my matrix to make it "more homogeneous"? That is, what algorithm might I use to reach the matrix nearest M that is actually homogeneous according to robomath?

I have tried making the robomath threshold more permissive by directly editing the source, but it seems that RoboDK the app just sets the pose to eye(4) when a non-homogeneous matrix is passed.

An iterative solution for operating on M would be fine; in fact I already wrote a framework for that. I just can't figure out how to determine the direction I need to shift M to make it more homogeneous.

Code:

`M =`

[[ 0.188, -0.958, 0.218, -2360.080 ],

[ -0.982, -0.184, 0.039, 1401.453 ],

[ 0.003, -0.222, -0.975, 2107.839 ],

[ 0.000, 0.000, 0.000, 1.000 ]]

err = M[0:3,0:3]*M[0:3,0:3].tr()

str(err)

[[ 1.000, -0.000, 0.000 ],

[ -0.000, 1.000, -0.000 ],

[ 0.000, -0.000, 1.000 ]]

str(err[0,0])

0.9999118410062409

Each individual element of `M*M.tr()` matches I(3) to 3 places after the decimal, but together they add up to make the matrix non-homogenous in the eyes of `robomath.Mat.isHomogeneous()` since that has a threshold of 1e-4 for the sum of the difference with I(3).

How can I tweak my matrix to make it "more homogeneous"? That is, what algorithm might I use to reach the matrix nearest M that is actually homogeneous according to robomath?

I have tried making the robomath threshold more permissive by directly editing the source, but it seems that RoboDK the app just sets the pose to eye(4) when a non-homogeneous matrix is passed.

An iterative solution for operating on M would be fine; in fact I already wrote a framework for that. I just can't figure out how to determine the direction I need to shift M to make it more homogeneous.