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,