This notebook is a class for camera projections, common used function like solvePNP or draw or read camera info are all in this class, just create an object than you can use, this notebook below have two part: function part and example part.

Function define part

class camera_projection[source]

camera_projection()

Testing example

we will move to 'data' directory to start all examples, this directory have been added to '.gitignoe' , so is convenient to test in local and light in cloud

cd data

/home/arg/arg_robotics_tools/data

prepare data 

id = '1DN5tiV_WII_aJM3DLijPnq5OLtI0HAq1'
filename = "ViperX_apriltags"
gdown_unzip(id, filename)

Downloading...
From: https://drive.google.com/u/1/uc?id=1DN5tiV_WII_aJM3DLijPnq5OLtI0HAq1
To: /home/arg/arg_robotics_tools/data/ViperX_apriltags.zip
100%|███████████████████████████████████████████████████████| 48.1M/48.1M [00:00<00:00, 103MB/s]

create an object

all function in this notebook are use as class members, so create an object first

cam_project = camera_projection()

function calling

we can simply call read_camera_info, read_images(id) to get the img

apriltag_detection() to get apriltag detect and solvePNP use self.detection_results[0].corners as input img point

cam_project.read_camera_info()
cam_project.read_images(300)
cam_project.apriltag_detection()
cam_project.solvePnP()

[INFO] detecting AprilTags...
[INFO] 1 total AprilTags detected

r = R.from_euler('x', 180, degrees=True)
r_tran = np.identity(4)
r_tran[:3,:3] = r.as_matrix()

with open('ViperX_apriltags/pose/' + str(300) + '.yaml', "r") as stream:
    try:
      data = yaml.safe_load(stream)
    except yaml.YAMLError as exc:
      print(exc)

tag_2 = np.array(data['Tag_2_pose'][0]['transformation_matrix'][0])
tag_2 = tag_2.reshape(4, 4)
tag_2_inv = np.matmul(tag_2, r_tran)
tag_2_inv = inv(tag_2_inv)

draw_image = cam_project.draw_point(tag_2_inv, np.identity(4))

figsize = 15 # param larger is bigger, adjust as needed
plt.rcParams['figure.figsize'] = (figsize, figsize)
plt.imshow(draw_image, cmap = 'brg')
plt.show()

joints = ['Joint_1_pose', 'Joint_2_pose', 
          'Joint_3_pose', 'Joint_4_pose', 
          'Joint_5_pose', 'Joint_6_pose']

for joint in joints:
    joint_data = np.array(data[joint][0]['transformation_matrix'][0])
    joint_data = joint_data.reshape(4, 4)
    if joints.index(joint) > 0:
      joint_data = np.matmul(previous_data, joint_data)
    draw_image = cam_project.draw_point(tag_2_inv, joint_data)
    previous_data = joint_data
    
plt.imshow(draw_image, cmap = 'brg')
plt.show()

ee = np.array(data['EE_pose'][0]['transformation_matrix'][0])
ee = ee.reshape(4, 4)
draw_image = cam_project.draw_point(tag_2_inv, ee)

plt.imshow(draw_image, cmap = 'brg')
plt.show()