def test_rodrigues_to_quaternion(self):
imp = ImageProcessor(self.TEST_JETSON_SINGLE_MARKER.img_path)
rvecs, _ = imp._find_pose(imp._find_fiducial_markers()[0])
# test value
test_quat = imp.rodrigues_to_quaternion(rvecs[0])
# true value
true_quat = pyquaternion.Quaternion(w=-0.060,
x=0.746,
y=-0.648,
z=-0.140)
# assert that the str representations are equal (components identical up to 3 decimal places)
self.assertEqual(str(test_quat), str(true_quat))
def test_rodrigues_to_quaternion_has_identical_rotation(self):
"""
Confirms the following case:
http://stackoverflow.com/questions/12933284/rodrigues-into-eulerangles-and-vice-versa
"""
imp = ImageProcessor(self.TEST_JETSON_SINGLE_MARKER.img_path)
rvecs, _ = imp._find_pose(imp._find_fiducial_markers()[0])
test_quat = imp.rodrigues_to_quaternion(rvecs[0])
# get rotation matrices
rot_mat_orig = cv2.Rodrigues(rvecs[0])[0]
rot_mat_quat = test_quat.rotation_matrix
rot_mat = cv2.multiply(rot_mat_orig, cv2.transpose(rot_mat_quat))
# test not working for some reason; just test equality of rotation matrices
# self.assertTrue(np.allclose(rot_mat,
# np.identity(3),
# rtol=1e-02))
self.assertTrue(np.allclose(rot_mat_orig, rot_mat_quat, rtol=1e-02))
def test_draw_axis(self):
imp = ImageProcessor(self.TEST_JETSON_SINGLE_MARKER.img_path)
rvecs, tvecs = imp._find_pose(imp._find_fiducial_markers()[0])
img = imp.draw_axis(imp.rodrigues_to_quaternion(rvecs[0]), tvecs[0])
self.assertIsNotNone(img)
Helper script for testing purposes.
This file can be called from the python3 shell as such:
exec(open("ImP/startImP.py").read())
"""
from ImP.imageProcessing.imageProcessingInterface import ImageProcessor
from ImP.imageProcessing.cameraCalibration import CameraCalibration
from ImP.imageProcessing.settings import ImageProcessingSettings
import cv2
from cv2 import aruco
import numpy as np
# img_path = "ImP/imageProcessing/test_files/jetson_test1.jpg"
# img_path = "ImP/imageProcessing/test_files/capstone_class_photoshoot/AC_0_125_DOCT.JPG"
img_path = "ImP/imageProcessing/test_files/capstone_class_photoshoot/SPACE_0.JPG"
imp = ImageProcessor(img_path)
corners, ids = imp._find_fiducial_markers()
rvecs, tvecs = imp._find_pose(corners)
quaternions = [imp.rodrigues_to_quaternion(r) for r in rvecs]
[print(q == q.normalised) for q in quaternions]
print(corners)
print(quaternions)
print(tvecs)
img = aruco.drawAxis(np.copy(imp._img_mat), imp._cal.CAMERA_MATRIX, imp._cal.DIST_COEFFS, rvecs[-1], tvecs[-1], ImageProcessingSettings.get_marker_length())
cv2.imwrite("ImP/output_files/OUTPUT_1.jpg", img)
img = imp.draw_aerocube_markers()
cv2.imwrite("ImP/output_files/OUTPUT.jpg", img)
img = imp.draw_aerocubes()
cv2.imwrite("ImP/output_files/OUTPUT_CUBE.jpg", img)