def test_it_should_set_translation(self):
transform = Transform()
transform.set_translation(10, 1, 2.2)
truth = np.array([[1, 0, 0, 10],
[0, 1, 0, 1],
[0, 0, 1, 2.2],
[0, 0, 0, 1]])
np.testing.assert_almost_equal(transform.matrix, truth)
def detect(self, img):
if len(img.shape) > 2:
raise Exception("ChessboardDetector uses gray image as input")
detection = None
ret, corners = cv2.findChessboardCorners(img, self.chess_shape, None)
if ret:
ret, rvec, tvec = cv2.solvePnP(self.obj_points, corners,
self.camera.matrix(),
np.array([0, 0, 0, 0, 0]))
# invert axis convention
rvec[1] = -rvec[1]
rvec[2] = -rvec[2]
tvec[1] = -tvec[1]
tvec[2] = -tvec[2]
detection = Transform()
detection.matrix[0:3, 0:3] = cv2.Rodrigues(rvec)[0]
detection.set_translation(tvec[0] / 1000, tvec[1] / 1000,
tvec[2] / 1000)
return detection