[sig.johnnson]

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.

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.

[sig.johnnson]

Update: I was able to do it using this library: https://github.com/HIPS/autograd. But it's really slow. I need to operate on thousands of matrices, so I would much appreciate a better solution.

[Albert]

You can find the source code of a function in C++ that can turn a non-homogeneous matrix into an homogeneous matrix:
https://github.com/RoboDK/RoboDK-API/blo...i.cpp#L354

It should look like something like this in Python:

Code:

def MakeHomogeneous(pose):
   from robolink import robomath
   vx = pose.VX()
   vy = pose.VY()
   # vz = pose.VZ()
   vx = robomath.normalize3(vx)
   vz = robomath.cross(vx, vy)
   vz = robomath.normalize3(vz)
   vy = robomath.cross(vz, vx)
   vy = robomath.normalize3(vy)
   poseH = Mat(pose)
   poseH.setVX(vx)
   poseH.setVY(vy)
   poseH.setVZ(vz)
   return poseH

[sig.johnnson]

Hi Albert, thanks for the quick reply -- this works great.

I believe your line 10 of the snippet should be `vy=robomath.normalize3(vy)`. I made this change and things look good.

[Albert]

Great, I just corrected it. Thank you for your feedback.