# Converting positions from camera matrices to x,y,z and h,p,r values?

I’m using OpenCV and ArUco with Panda and the marker poses are provided in the camera space as matrices. What I need instead is to have one marker for the 0,0,0 position and (0,0,0) Hpr in Panda’s root node and the rest relative to it.

So I have the position of the root node (marker) relative to the OpenCV camera as a matrix and the other markers relative to the camera as matrices as well even though in Panda I need them relative to the root node.

How can I convert these two matrices to the values I need so I can load 3d models in Panda where the markers are detected while having a proper hierarchy for them and not having the camera inside Panda be the same angle and position relative to those marker models as the OpenCV camera is?

If you already have the relationship between the camera (eg. `base.cam`) and the root node (eg. `base.render`) set, you can set matrices relative to the camera like such:

``````marker2.setMat(base.cam, marker2_mat)
``````

Thank you.

This is what I actually have.
I have the position of the AR root marker relative to the OpenCV camera as a matrix.
All the other markers also have their position data same way.
I don’t have the position of the OpenCV camera itself, although it may be possible to be calculated by the position of the root marker relative to it. I don’t know too much about matrix math.

I don’t want the OpenCV camera and base.cam to be the same. In fact I want to see the 3d preview of the markers from different angles with base.cam.

You could either set up a dummy node to represent the OpenCV camera, or you could multiply the marker matrices with the inverse of the OpenCV camera matrix. (Note that matrix multiplication order matters.)

Thank you but if I don’t know the position and rotation of the OpenCV camera, only position of the markers relative to it doesn’t it mean the matrix of OpenCV camera will be all zeroes?

I think i’ve found a solution.

Let me reprhase the question first in case it wasn’t clear.

In OpenCV you don’t know the position of an OpenCV/real life camera relative to a 0,0,0 position like you can in a Panda3D virtual scene. You can only know positions of objects captured by the camera relative to the camera. But in Panda I need the camera relative to something. With the OpenCV AruCo library, we may be able to get data which will allow to get its position relative to a marker placed where we want the (0,0,0) world center to be. However we can only get the position of the marker relative to the camera with OpenCV, not the other way.

I think what I should do is set the position of the Panda camera to (0,0,0) in Panda, get the position of the marker relative to the camera as a matrix in OpenCV and in Panda do marker_node.setMat(matrix) ; base.cam.reparentTo(marker_node); and then do marker_node.setPos(base.cam.getPos(render)) . This I think should move the marker_node to 0,0,0 but keep the relative positioning between the camera and the marker_node the same as base.cam is child of the markr node. Am I right?

I’m not quite sure what you intend that sequence of commands to do, but in your case it would do nothing, as the camera’s relative position to its parent will be `(0, 0, 0)` and so therefore `base.cam.getPos(render) == marker_node.getPos()` already.

Ah, I think I understand your situation a bit better now, though. Sorry for misunderstanding earlier.

If I understand correctly, you have one “root marker” that represents your `(0, 0, 0)` position, ie. your frame of reference, and you want to measure the position of the other markers relative to that. One way to go ahead is to lay out your scene like so:

``````render
- OpenCV camera
- marker1
- marker2
``````

So, we have constructed a dummy node representing the OpenCV camera, and as children of that a dummy node for each of the non-root markers. When we get updated matrices from OpenCV, what you do is this:

• You assign the inverse of the root marker matrix to the dummy OpenCV camera node using `setMat(panda3d.core.invert(mat))`.
• You assign each marker matrix for the other markers to each marker node using `setMat(mat)`.

Now, it is easy to get the “absolute” (ie. relative to the “root marker”) position of each other marker; you can just call eg. `marker2.getPos(render)`. As a free bonus, you now also have a dummy node representing the OpenCV camera’s position and orientation, perhaps for parenting debug visualisation to.

Note that depending on how OpenCV is giving you the matrices, it may be necessary not to invert the root marker matrix but instead to invert all the other marker matrices instead. I’m not 100% sure about that; you’ll need to try to see which one works and which one doesn’t. Additional conversion (eg. a transpose operation) may also be necessary if the OpenCV matrix is laid out in column-major instead of row-major order.

Thank you again.

I just want the root marker to be at (0,0,0) in render’s coordinate space inside Panda.
The root marker and other markers don’t have positional data I can fetch relative to each other, they are all relative to the real world camera’s position.
If this is what you understood and your reply is according to this info, then great.

I think what I thought of did the same but without the need of matrix math. But maybe I’m wrong. Let me try again with proper code tags this time.

``````marker_node.setMat(matrix) # set marker position (which in real world is relative to the physical camera)
base.cam.reparentTo(marker_node) # attach the camera inside Panda to the marker's coordinate space

marker_node.setPos(base.cam.getPos(render)) # the cams position relative to render is (0,0,0), so if we move the marker there instead, the camera (which is now attached to the marker) will be offset accordingly
``````

Now we have the root marker at (0,0,0) as I wanted with the camera offsetted accordingly instead. So other markers can be positioned relative to it (the camera) but in proper position relative to the root marker (which is now at (0,0,0)) as well. Am I wrong here?

Yes, this is what I understood and what my reply is assuming.

Again, I’m not sure what you are expecting your snippet to accomplish. If `base.cam` is at `(0, 0, 0)` before running that code, then `base.cam.getPos(render)` will return the same thing as `marker_node.getPos()` after the reparent, because `reparentTo` does nothing to keep the absolute position of the camera the same before and after the reparent (but also see `wrtReparentTo` if that is what you intended to do).

Oh, yes of course, I was thinking of wrtReparentTo(). it has been awhile since I’ve used these methods and forgot which was which. My idea was to wrtReparentTo() camera to the marker so after the marker is repositioned to (0,0,0) relative to render the camera would be offsetted accordingly.