def collect_points_6dir(self, tgt):
# must be in CV mode, otherwise the point clouds won't align
scan_config = [0, -1, 2, 1, 4, 5] # front, left, back, right, up, down
points_6dir = np.zeros((0, self.imgwidth, 3), dtype=np.float32)
for k, face in enumerate(scan_config):
if face == 4: # look upwards at tgt
pose = Pose(Vector3r(tgt[0], tgt[1], tgt[2]),
to_quaternion(math.pi / 2, 0, 0)) # up
elif face == 5: # look downwards
pose = Pose(Vector3r(tgt[0], tgt[1], tgt[2]),
to_quaternion(-math.pi / 2, 0,
0)) # down - pitch, roll, yaw
else: # rotate from [-90, 0, 90, 180]
yaw = math.pi / 2 * face
pose = Pose(Vector3r(tgt[0], tgt[1], tgt[2]),
to_quaternion(0, 0, yaw))
self.set_vehicle_pose(pose)
depth_front, _ = self.get_depth_campos()
if depth_front is None:
rospy.logwarn('Missing image {}: {}'.format(k, tgt))
continue
# import ipdb;ipdb.set_trace()
point_array = expo_util.depth_to_point_cloud(depth_front,
self.focal,
self.pu,
self.pv,
mode=k)
points_6dir = np.concatenate((points_6dir, point_array), axis=0)
# reset the pose for fun
pose = Pose(Vector3r(tgt[0], tgt[1], tgt[2]), to_quaternion(0, 0, 0))
self.set_vehicle_pose(pose)
# print 'points:', points_6dir.shape
return points_6dir
def fly(client: airsim.MultirotorClient, args: argparse.Namespace) -> None:
if args.flush:
client.simFlushPersistentMarkers()
if not args.transformation:
poses = [
Pose(record.position, record.orientation)
for record in args.recording.values()
]
positions = [record.position for record in args.recording.values()]
else:
matrix = np.loadtxt(
args.transformation) # load the 4x4 transformation matrix
print(matrix)
poses = []
positions = []
for record in args.recording.values():
pos = Vector3r(*np.matmul(matrix, np.append(record.position, 1)))
poses.append(Pose(pos, record.orientation))
positions = [pos]
if args.axes:
client.simPlotTransforms(poses,
scale=100.0,
thickness=2.5,
is_persistent=True)
else:
client.simPlotPoints(positions,
args.color,
size=10,
is_persistent=True)
if args.lines:
client.simPlotLineStrip(positions,
args.color,
thickness=2.5,
is_persistent=True)
if args.aim:
line_list = []
for pose in poses:
line_list.append(pose.position)
x_axis, _, _ = AirSimNedTransform.local_axes_frame(pose)
line_list.append(pose.position + x_axis * 100)
client.simPlotLineList(line_list,
args.color,
thickness=2.5,
is_persistent=True)
def align_meshroom_to_airsim(meshroom_pose,
meshroom_to_airsim_transform):
# NOTE this transformation is only based on the positions (and not on the orientations)
meshroom_pos = np.append(meshroom_pose.position.to_numpy_array(),
1) # [x, y, z, 1]
airsim_pos = np.matmul(meshroom_to_airsim_transform, meshroom_pos)
return Pose(Vector3r(*airsim_pos), meshroom_pose.orientation)
def convert_2_pose(a):
"""
a: A 7-element array.
"""
return Pose(Vector3r(a[0], a[1], a[2]),
Quaternionr(a[3], a[4], a[5], a[6]))
def convert_2_pose(p, e):
"""
p: A 3-element NumPy array, the position.
q: A 4-element NumPy array, a quaternion witht he last element being w.
"""
return Pose(Vector3r(p[0], p[1], p[2]), to_quaternion(e[0], e[1], e[2]))
def data_sampling(self, positions, trajname):
self.init_folders(trajname)
self.randquaternionsampler.init_random_yaw()
start = time.time()
for k, pose in enumerate(positions):
position = Vector3r(pose[0], pose[1], pose[2])
orientation = self.randquaternionsampler.next_quaternion()
dronepose = Pose(position, orientation)
self.imgclient.setpose(dronepose)
time.sleep(0.02)
self.imgclient.simPause(True)
rgblist, depthlist, seglist, camposelist = self.imgclient.readimgs(
)
self.imgclient.simPause(False)
# save images and poses
imgprefix = str(k).zfill(6) + '_'
for w, camind in enumerate(self.camlist):
camname = self.camlist_name[camind]
# save RGB image
if 'Scene' in self.imgtypelist:
img = rgblist[w] # change bgr to rgb
cv2.imwrite(
join(self.imgdirs[w], imgprefix + camname + '.png'),
img, [cv2.IMWRITE_PNG_COMPRESSION, 0])
# save depth image
if 'DepthPlanner' in self.imgtypelist:
depthimg = depthlist[w]
np.save(
join(self.depthdirs[w],
imgprefix + camname + '_depth.npy'), depthimg)
# save segmentation image
if 'Segmentation' in self.imgtypelist:
segimg = seglist[w]
np.save(
join(self.segdirs[w],
imgprefix + camname + '_seg.npy'), segimg)
# write pose to file
self.posenplist[w].append(np.array(camposelist[w]))
# imgshow = np.concatenate((leftimg,rightimg),axis=1)
print(
' {0}, pose {1}, orientation ({2:.2f},{3:.2f},{4:.2f},{5:.2f})'
.format(k, pose, orientation.x_val, orientation.y_val,
orientation.z_val, orientation.w_val))
# cv2.imshow('img',imgshow)
# cv2.waitKey(1)
for w in range(len(self.camlist)):
# save poses into numpy txt
np.savetxt(self.posefilelist[w], self.posenplist[w])
end = time.time()
print('Trajectory sample time {}'.format(end - start))
def fly(client: airsim.MultirotorClient, args: argparse.Namespace) -> None:
if args.reset:
client.reset()
client.simFlushPersistentMarkers()
plot_pose(client, TEST_POSE)
plot_xyz_axis(client, X, Y, Z, origin=O)
with airsimy.pose_at_simulation_pause(client) as pose:
plot_pose(client, pose)
client.simPlotArrows([pose.position], [LOOK_AT_TARGET],
Rgba.White,
is_persistent=True)
# NOTE use x' = (LOOK_AT_TARGET - p) as the new x-axis (i.e. front vector),
# and project the current up/down vector (z-axis in AirSim) into the plane
# that is normal to x' at point p. This way we can get the remaining right
# vector by computing cross(down, front).
x_prime = LOOK_AT_TARGET - pose.position
_, _, z_axis = AirSimNedTransform.local_axes_frame(pose)
z_prime = airsimy.vector_projected_onto_plane(z_axis,
plane_normal=x_prime)
# NOTE don't forget to normalize! Not doing so will break the orientation below.
x_prime /= x_prime.get_length()
z_prime /= z_prime.get_length()
y_prime = z_prime.cross(x_prime)
plot_xyz_axis(
client,
x_prime * 1.25,
y_prime * 1.25,
z_prime * 1.25,
origin=pose.position,
colors=CMY,
thickness=1.5,
)
# Now, find the orientation that corresponds to the x'-y'-z' axis frame:
new_pose = Pose(
pose.position,
airsimy.quaternion_that_rotates_axes_frame(
source_xyz_axes=(X, Y, Z),
target_xyz_axes=(x_prime, y_prime, z_prime),
),
)
plot_pose(client, new_pose
) # NOTE this should be the same as the plot_xyz_axis above!
if args.set:
client.simSetVehiclePose(new_pose, ignore_collision=True)
def fly(client: MultirotorClient, args: argparse.Namespace) -> None:
reset(client)
rect = cast(Rect, args.rect)
initial_pose = cast(Pose, client.simGetVehiclePose())
closest_corner = rect.closest_corner(initial_pose.position)
if args.verbose:
ff.print_pose(initial_pose, airsim.to_eularian_angles)
ff.log_info(f"Closest corner {ff.to_xyz_str(closest_corner)}")
start_pos = Vector3r(
*ff.to_xyz_tuple(closest_corner)) # NOTE make sure we copy it by value
start_pos.z_val += args.z_offset
start_pos.y_val += args.y_offset
start_pos.x_val += args.x_offset
plot = Plotter(client)
plot.flush_persistent()
plot.points([start_pos], Rgba.Red)
if args.teleport:
ff.log(f"Teleporting to {ff.to_xyz_str(start_pos)}...")
new_pose = Pose(start_pos, initial_pose.orientation)
Controller.teleport(client, to=new_pose, ignore_collision=True)
else:
ff.log(f"Flying to {ff.to_xyz_str(start_pos)}...")
client.moveToPositionAsync(*ff.to_xyz_tuple(start_pos),
velocity=5).join()
ff.log("Capturing grid...")
time.sleep(2)
z = Vector3r(0, 0, args.altitude_shift)
path = [
z + Vector3r(*ff.to_xyz_tuple(corner))
for corner in rect.zigzag(lanes=args.zigzags)
]
if AUGMENT_PATHS:
path = Controller.augment_path(path, AUGMENT_PATHS_MAX_DIST)
plot.points(path, Rgba.White)
plot.lines(path, Rgba.Green)
try:
if RECORD:
client.startRecording()
Controller.fly_path(client, path)
except Exception as e:
raise e
finally:
if RECORD:
client.stopRecording()
def init_exploration(self):
# cloud_msg, cloud_odom_msg = self.get_point_cloud_msg(cam_id=0)
# cam_trans, cam_rot = expo_util.odom_to_trans_and_rot(cloud_odom_msg)
# cam_pose = self.cmd_client.simGetCameraInfo(camera_name=0).pose
cam_info = self.cmd_client.simGetCameraInfo(camera_name=self.camid)
img_res = self.cmd_client.simGetImages(self.img_type)
img_res = img_res[0]
cam_pose = Pose(img_res.camera_position, img_res.camera_orientation)
cam_trans, cam_rot = expo_util.sim_pose_to_trans_and_rot(cam_pose)
self.imgwidth = img_res.width
self.imgheight = img_res.height
self.focal, self.pu, self.pv = expo_util.get_intrinsic(
img_res, cam_info, self.fov)
self.cam_pos = cam_trans
rospy.loginfo('Initialized img ({},{}) focal {}, ({},{})'.format(
self.imgwidth, self.imgheight, self.focal, self.pu, self.pv))
self.publish_lidar_scans_6dir(cam_trans)
time.sleep(5)
def setRandomPose(client, center, offset_range, rotation_range):
random_vector = np.random.random_sample((2, 1))
unit_vector = random_vector / np.linalg.norm(random_vector)
displacement_vector = (offset_range[1] - offset_range[0]) * (
np.random.random_sample()) * unit_vector + offset_range[0]
displacement_vector = displacement_vector.reshape(-1, )
position = Vector3r(center.position.x_val + displacement_vector[0],
center.position.y_val + displacement_vector[1],
center.position.z_val)
rotation = (rotation_range[1] - rotation_range[0]
) * np.random.random_sample() + rotation_range[
0] #2*np.sin(2*np.pi*np.random.random_sample())
orientation = Quaternionr(z_val=rotation)
pose = Pose(position_val=position, orientation_val=orientation)
client.simSetObjectPose("pallet", pose, True)
return pose, displacement_vector, rotation
def capture_LIDAR_depth(self, p, q):
"""
p: A 3-element NumPy array, the position.
q: A 4-element NumPy array, quaternion, w is the last element.
"""
faces = [0, 1, 2, -1] # Front, right, back, left.
depthList = []
q0 = Quaternionr(q[0], q[1], q[2], q[3])
for face in faces:
# Compose a AirSim Pose object.
yaw = np.pi / 2 * face
yq = to_quaternion(0, 0, yaw)
# q = to_quaternion( e[0], e[1], e[2] )
q1 = yq * q0
pose = Pose(Vector3r(p[0], p[1], p[2]), q1)
# Change the vehicle pose.
self.set_vehicle_pose(pose)
# Get the image.
for i in range(self.maxTrial):
depth, _ = self.get_depth_campos()
if depth is None:
print("Fail for trail %d on face %d. " % (i, face))
continue
if (depth is None):
raise Exception("Could not get depth image for face %d. " %
(face))
# Get a valid depth.
depthList.append(depth)
return depthList
def pose_from_meshroom_to_airsim(meshroom_pose):
assert len(meshroom_pose.center) == 3 and len(
meshroom_pose.rotation) == 9, meshroom_pose
xyzw = MeshroomTransform.rotation(meshroom_pose.rotation,
as_xyzw_quaternion=True)
return Pose(Vector3r(*meshroom_pose.center), Quaternionr(*xyzw))
if (z := args.z_offset):
start_pos.z_val -= z # start higher up, to avoid crashing with objects
if (y := args.y_offset): start_pos.y_val += y
if (x := args.x_offset): start_pos.x_val += x
# Plot a point at the start position and go to it
if SHOW_PLOTS:
client.simPlotPoints([start_pos],
color_rgba=Rgba.White,
is_persistent=True)
if args.teleport:
ff.log(f"Teleporting to {ff.to_xyz_str(start_pos)}...")
# NOTE using `nanQuaternionr` for the orientation should work for `simSetVehiclePose`
# not to change it (which is called by `Controller.teleport`)... but it doesn't
new_pose = Pose(start_pos, initial_pose.orientation)
Controller.teleport(client, to=new_pose, ignore_collision=True)
else:
ff.log(f"Flying to {ff.to_xyz_str(start_pos)}...")
client.moveToPositionAsync(*ff.to_xyz_tuple(start_pos),
velocity=5,
timeout_sec=12).join()
# Fly over the region of interest now that we reached the starting position
ff.log("Flying over zone...")
time.sleep(2)
fly_zone(client, args.roi, altitude_shift=args.z_shift)
def fly_zone(client: airsim.MultirotorClient,
zone: Rect,
def fly(client: airsim.MultirotorClient, args: argparse.Namespace) -> None:
initial_pose = client.simGetVehiclePose()
if args.verbose:
ff.print_pose(initial_pose, airsim.to_eularian_angles)
ff.log(client.simGetCameraInfo(camera_name=CAPTURE_CAMERA))
if args.flush or (args.capture_dir and not args.debug):
client.simFlushPersistentMarkers()
camera_poses = []
for camera in args.trajectory:
position = convert_uavmvs_to_airsim_position(
camera.position, translation=args.offset, scaling=args.scale
)
orientation = quaternion_orientation_from_eye_to_look_at(position, LOOK_AT_TARGET)
camera_poses.append(Pose(position, orientation))
if args.debug:
camera_positions = [pose.position for pose in camera_poses]
client.simPlotPoints(camera_positions, Rgba.Blue, is_persistent=True)
client.simPlotLineStrip(camera_positions, Rgba.Cyan, thickness=2.5, is_persistent=True)
airsim_record = []
def do_stuff_at_uavmvs_viewpoint(i, pose):
nonlocal client, camera_poses, airsim_record
log_string = f"({i}/{len(camera_poses)})"
p, q = pose.position, pose.orientation
if args.debug:
log_string += f" position = {to_xyz_str(p)}"
client.simPlotTransforms([pose], scale=100, is_persistent=True)
# client.simPlotArrows([p], [LOOK_AT_TARGET], Rgba.White, thickness=3.0, duration=10)
elif args.capture_dir:
path = f"{args.prefix}pose{args.suffix}_{i:0{len(str(len(camera_poses)))}}.png"
path = os.path.join(args.capture_dir, path)
airsim.write_png(path, AirSimImage.get_mono(client, CAPTURE_CAMERA))
log_string += f' saved image to "{path}"'
record_line = AirSimRecord.make_line_string(p, q, time_stamp=str(i), image_file=path)
airsim_record.append(record_line)
ff.log(log_string)
if IS_CV_MODE:
for i, camera_pose in enumerate(camera_poses):
client.simSetVehiclePose(camera_pose, ignore_collision=True)
do_stuff_at_uavmvs_viewpoint(i, camera_pose)
time.sleep(CV_SLEEP_SEC)
else:
client.moveToZAsync(z=-10, velocity=VELOCITY).join() # XXX avoid colliding on take off
client.hoverAsync().join()
mean_position_error = 0.0
for i, camera_pose in enumerate(camera_poses):
client.moveToPositionAsync(
*to_xyz_tuple(camera_pose.position),
velocity=VELOCITY,
drivetrain=DrivetrainType.MaxDegreeOfFreedom,
yaw_mode=YawMode(is_rate=False, yaw_or_rate=YAW_N),
).join()
with pose_at_simulation_pause(client) as real_pose:
# NOTE when we pre-compute the viewpoint's camera orientation, we use the
# expected drone position, which (should be close, but) is not the actual
# drone position. Hence, we could experiment with using fake orientation:
# quaternion_orientation_from_eye_to_look_at(real_pose.position, LOOK_AT_TARGET)
fake_pose = Pose(real_pose.position, camera_pose.orientation)
client.simSetVehiclePose(fake_pose, ignore_collision=True)
client.simContinueForFrames(1) # NOTE ensures pose change
do_stuff_at_uavmvs_viewpoint(i, fake_pose)
client.simSetVehiclePose(real_pose, ignore_collision=True)
position_error = real_pose.position.distance_to(camera_pose.position)
mean_position_error += position_error
ff.log_debug(f"{position_error = }")
mean_position_error /= len(camera_poses)
ff.log_debug(f"{mean_position_error = }")
if airsim_record:
print_to_file = args.record_path is not None
file = open(args.record_path, "w") if print_to_file else None
print(AirSimRecord.make_header_string(), file=(file if print_to_file else sys.stdout))
for line in airsim_record:
print(line, file=(file if print_to_file else sys.stdout))
if print_to_file:
file.close()
ff.log_info(f'Saved AirSim record to "{args.record_path}"')
def setPose(center, z_rotation):
pose = Pose(Vector3r(center[0], center[1], center[2]),
Quaternionr(z_val=z_rotation))
return pose
O = Vector3r(0, 0, 0)
X = Vector3r(1, 0, 0)
Y = Vector3r(0, 1, 0)
Z = Vector3r(0, 0, 1)
RGB = (Rgba.Red, Rgba.Green, Rgba.Blue)
CMY = (Rgba.Cyan, Rgba.Magenta, Rgba.Yellow)
LOOK_AT_TARGET = data_config.Ned.Cidadela_Statue
TEST_POSE = Pose(
Vector3r(x_val=-13.793405532836914,
y_val=2.257002115249634,
z_val=-6.261967182159424),
Quaternionr(
x_val=-0.020065542310476303,
y_val=-0.14743053913116455,
z_val=-0.02142292633652687,
w_val=0.9886367917060852,
),
)
def plot_xyz_axis(
client: airsim.MultirotorClient,
x_axis: Vector3r,
y_axis: Vector3r,
z_axis: Vector3r,
origin: Vector3r,
colors=RGB,
thickness=2.5,
