1

I'm creating a university project with OpenCV Python and ArUco markers, where I would like to get a (relatively) robust pose estimation for the movement of the camera. I plan on using this for indoor drone flight graphing. For this, I have to transform the camera pose to world coordinates defined by the first seen marker.

I know there must be a transformation matrix between the markers, but I can't seem to figure out, what it is. I am trying with the difference of respective rvecs.

The code for the function in Python:

def TransformBetweenMarkers(tvec_m, tvec_n, rvec_m, rvec_n):
    tvec_m = np.transpose(tvec_m) # tvec of 'm' marker
    tvec_n = np.transpose(tvec_n) # tvec of 'n' marker
    dtvec = tvec_m - tvec_n # vector from 'm' to 'n' marker in the camera's coordinate system

    # get the markers' rotation matrices respectively
    R_m = cv2.Rodrigues(rvec_m)[0]
    R_n = cv2.Rodrigues(rvec_n)[0]

    tvec_mm = np.matmul(-R_m.T, tvec_m) # camera pose in 'm' marker's coordinate system
    tvec_nn = np.matmul(-R_n.T, tvec_n) # camera pose in 'n' marker's coordinate system

    # translational difference between markers in 'm' marker's system,
    # basically the origin of 'n'
    dtvec_m = np.matmul(-R_m.T, dtvec)

    # this gets me the same as tvec_mm,
    # but this only works, if 'm' marker is seen
    # tvec_nm = dtvec_m + np.matmul(-R_m.T, tvec_n)

    # something with the rvec difference must give the transformation(???)
    drvec = rvec_m-rvec_n
    drvec_m = np.transpose(np.matmul(R_m.T, np.transpose(drvec))) # transformed to 'm' marker
    dR_m = cv2.Rodrigues(drvec_m)[0]

    # I want to transform tvec_nn with a single matrix,
    # so it would be interpreted in 'm' marker's system
    tvec_nm = dtvec_m + np.matmul(dR_m.T, tvec_nn)

    # objective: tvec_mm == tvec_nm

This is the best I could get, but there is still an error value of +-0.03 meters between the tvec_mm and tvec_nm translation values.

Is it possible to get any better with this? Is this even a legit transformation or just a huge coincidence, that it gives approximately the same values? Any ideas?

Thank you!

Szepy
  • 11
  • 2
  • Frankly, I am not familiar with `rvec` calculations and transformations, but am familiar with the rotation matrices. `dR_m = R_m.dot(R_n.T)` should give you the correct transformation in terms of the rotation matrices. Can you check if this improves anything? Also, you do not need to use `np.matmul(x,y)`, using `x.dot(y)` is much easier. For example, you can do things like `x.dot(y).dot(z)` if you want to do `x * y * z`. – Kani Oct 15 '19 at 00:38
  • @Kani Thank you, it works! (Also thanks for the `.dot()` multiplication!) I had the idea for this, but I couldn't prove myself, that the `dR_m = R_m.dot(R_n.T)` gives a constant matrix, which I need for future calculations. Tested it, now it can jump from marker to marker with this matrix, thanks! – Szepy Oct 17 '19 at 08:28

0 Answers0