def _ParseCameraConfig(self, camera_config):
# extract serial number
serial_no = camera_config["serial_no"]
# construct the transformation matrix
world_transform = camera_config["world_transform"]
X_WCamera = tf.euler_matrix(world_transform["roll"],
world_transform["pitch"],
world_transform["yaw"])
X_WCamera[:3, 3] = \
[world_transform["x"], world_transform["y"], world_transform["z"]]
# construct the transformation matrix
internal_transform = camera_config["internal_transform"]
X_CameraDepth = tf.euler_matrix(internal_transform["roll"],
internal_transform["pitch"],
internal_transform["yaw"])
X_CameraDepth[:3, 3] = ([internal_transform["x"],
internal_transform["y"],
internal_transform["z"]])
# construct the camera info
camera_info_data = camera_config["camera_info"]
if "fov_y" in camera_info_data:
camera_info = CameraInfo(camera_info_data["width"],
camera_info_data["height"],
camera_info_data["fov_y"])
else:
camera_info = CameraInfo(
camera_info_data["width"], camera_info_data["height"],
camera_info_data["focal_x"], camera_info_data["focal_y"],
camera_info_data["center_x"], camera_info_data["center_y"])
return serial_no, X_WCamera, X_CameraDepth, camera_info
def visualize(self, states, dt):
import meshcat
import meshcat.geometry as g
import meshcat.transformations as tf
import time
# Create a new visualizer
vis = meshcat.Visualizer()
vis.open()
vis["/Cameras/default"].set_transform(
tf.translation_matrix([0, 0, 0]).dot(
tf.euler_matrix(0, np.radians(-30), -np.pi / 2)))
vis["/Cameras/default/rotated/<object>"].set_transform(
tf.translation_matrix([1, 0, 0]))
vis["Quadrotor"].set_object(
g.StlMeshGeometry.from_file('systems/crazyflie2.stl'))
while True:
for state in states:
vis["Quadrotor"].set_transform(
tf.translation_matrix([state[0], state[1], state[2]]).dot(
tf.quaternion_matrix(state[6:10])))
time.sleep(dt)