ArUco relative marker position and rvec/tvec inversion
I am trying to write a program with aruco+openCV. Program detects two ArUco markers, invert the second one and compose them together. Result is another tvec and rvec. I am summing tvec and first markers tvec, multiplying rvec and first markers rvec. And trying to show that with projectPoints and cv.imshow. My tvec/rvec invert operation code:
def inversePerspective(rvec, tvec):
R, _ = cv2.Rodrigues(rvec)
R = np.matrix(R).T
invTvec = np.dot(-R, np.matrix(tvec))
invRvec, _ = cv2.Rodrigues(R)
return invRvec, invTvec
In another question user Tetragramm helped me to understand the basics. You can find the question in: http://answers.opencv.org/question/17...
And I thought I should test my functions. In relative position function:
def relativePosition(rvec1, tvec1, rvec2, tvec2):
rvec1, tvec1 = rvec1.reshape((3, 1)), tvec1.reshape((3, 1))
rvec2, tvec2 = rvec2.reshape((3, 1)), tvec2.reshape((3, 1))
# Inverse the second marker, the right one in the image
invRvec, invTvec = inversePerspective(rvec2, tvec2)
print(rvec2, tvec2, "\n and \n", inversePerspective(invRvec, invTvec))
info = cv2.composeRT(rvec1, tvec1, invRvec, invTvec)
composedRvec, composedTvec = info[0], info[1]
composedRvec = composedRvec.reshape((3, 1))
composedTvec = composedTvec.reshape((3, 1))
return composedRvec, composedTvec
I executed code once and showed 2 markers that near to each other. Normally I should expect that in print function, it should print almost the same matrices. The output was:
rvec: [[-0.08162009]
[-0.07777238]
[-1.46936788]] tvec: [[-0.05844654]
[ 0.01511464]
[ 0.00109817]]
and
[[-0.08162009]
[-0.07777238]
[-1.46936788]] [[-0.05844654]
[ 0.01511464]
[ 0.00109817]]
It means that my invert code is ok. And I am using composeRT to compose two rvec/tvecs. Is there anything I did wrong ? My current code is in github and I really accept any kind of help. I am searching for days and days. Thanks in advance for the answers, here is the codebase: https://github.com/aliyasineser/Gradu...
Edit: I print wrong test code. I corrected that.