OpenCV AR

Scripting Issues
1

Hello everyone,

I’m working on an OpenCV project. Many of the new functions in the OpenCV 3D reconstruction library calculate a translation vector and a rotation vector ( tvec and rvec) of the camera relative to a visible object

My team has written a program that links OpenCV and Panda with Panda handling the 3D environment and modeling.

The problem we are having is that while we’ve been able to translate tvec to appropriate XYZ in Panda3D, the rotation vector has been very difficult to convert over to something Panda3D can use to orient the model.

The OpenCV tvec to Panda3D XYZ is:
[b]tvec/b = X
[b]tvec/b = -Z
[b]tvec/b = Y

Does anyone know how I can convert the components of rvec to Panda3D’s HPR?

Thanks.

2

OpenCV uses the Rodrigues formula to encode a rotation in three components.

I once wrote the following function to unroll it to something Panda understands:

    def rodrigues(self, r):
        theta = r.length()
        if theta == 0:
            return Mat4.identMat()
        c = math.cos(theta)
        s = math.sin(theta)
        c1 = 1.0 - c

        rx = r[0] / theta
        ry = r[1] / theta
        rz = r[2] / theta

        I = Mat3.identMat()
        rrt = Mat3(rx*rx, rx*ry, rx*rz, rx*ry, ry*ry, ry*rz, rx*rz, ry*rz, rz*rz)
        _r_x_ = Mat3(0, -rz, ry, rz, 0, -rx, -ry, rx, 0)

        R = Mat3()
        for y in range(3):
            for x in range(3):
                R.setCell(y, x, I(y, x) * c + rrt(y, x) * (1.0 - c) + _r_x_(y, x) * s)
        R.transposeInPlace()
        return Mat4(R)

This returns a 4x4 matrix, which you can apply to a NodePath with setMat(). If you really only want the hpr triple, you can then extract that again with nodePath.getHpr().

David

3

Wow that was a fast response. It took longer to try it out than to get the reply. Thanks.

This code is great. I still have a minor issue but this is infinitely better than before.

It seems that OpenCV is assuming a Y-up world while Panda uses a Z-up world. The issue is that when I rotate the marker around in the Y-up axis, the model rotates around its Panda Y axis and not its Z-axis. This seems like it should be a simple problem to solve but at the moment it’s giving me trouble. To try to fix it I created a dummynode and applied the matrix to that dummy and then grabbed its HPR and switched the axes when I apply the HPR to the AR model but it’s not working right. Is there a simpler way to solve this issue?

4

You could put the marker node underneath a dummy parent, and set the 90 degree rotation on the parent to convert the coordinate system.

Or you could just put that 90 degree rotation on render.

Or, you could simply run Panda with:

coordinate-system y-up

in your Config.prc file.

David

5

By the way, lest someone accuse me of plagiarism at some point in the future, let me point out that I am not claiming original authorship of the above function, but rather I lifted it from the OpenCV source code, and translated it to Python. There are doubtless more efficient ways to convert the vector, but this was the most straightforward way at the time I needed to do it.

David