Thread Rating:
  • 0 Vote(s) - 0 Average
  • 1
  • 2
  • 3
  • 4
  • 5

How to make a pose homogeneous

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.

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()
[[ 1.000, -0.000, 0.000 ],
 [ -0.000, 1.000, -0.000 ],
 [ 0.000, -0.000, 1.000 ]]

Each individual element of `M*` 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.
Update: I was able to do it using this library: But it's really slow. I need to operate on thousands of matrices, so I would much appreciate a better solution.
You can find the source code of a function in C++ that can turn a non-homogeneous matrix into an homogeneous matrix:

It should look like something like this in Python:
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)
   return poseH
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.
Great, I just corrected it. Thank you for your feedback.

Users browsing this thread:
1 Guest(s)