• 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.

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.
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.
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```
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)