def importFromExternalOrientationTextfile(filename, delimiter, imageCollection, rotationSequence, observedPosition, observedOrientation, camera): poses = np.loadtxt(filename, dtype=str, delimiter=delimiter, skiprows=1) imageCollection.observedPosition = observedPosition imageCollection.observedOrientation = observedOrientation for row in range(0, poses.shape[0]): id = poses[row, 0] translation = np.zeros((3, 1)) eulerAngles = np.zeros(3) translation[0, 0] = float(poses[row, 1]) translation[1, 0] = float(poses[row, 2]) translation[2, 0] = float(poses[row, 3]) eulerAngles[0] = float(poses[row, 4]) eulerAngles[1] = float(poses[row, 5]) eulerAngles[2] = float(poses[row, 6]) pose = geometry.Pose() pose.setRotationEuler(conv.degrees2Radians(eulerAngles), rotationSequence) pose.setTranslation(translation) image = geometry.Image() image.setPose(pose) image.setCamera(camera) image.rotationSequence = rotationSequence imageCollection.addImage(image, id)
def _edge_pose_generator(self, pose, num_steps): step_size = 2 * numpy.pi / num_steps theta = -numpy.pi while theta < numpy.pi: yield geometry.Pose(pose.x + numpy.cos(theta) * self._radius, pose.y + numpy.sin(theta) * self._radius, pose.theta) theta += step_size
def __init__(self, map_file, render_lidar, use_noise, lidar_dist_measure_sigma, lidar_theta_step_sigma, lidar_num_ranges_noise, odom_trans_sigma, odom_rot_sigma, width, mbot_max_trans_speed, mbot_max_angular_speed, real_time_factor): # Model self._map_file = map_file self._use_noise = use_noise self._lidar_dist_measure_sigma = lidar_dist_measure_sigma self._lidar_theta_step_sigma = lidar_theta_step_sigma self._lidar_num_ranges_noise = lidar_num_ranges_noise self._odom_trans_sigma = odom_trans_sigma self._odom_rot_sigma = odom_rot_sigma self._mbot_max_trans_speed = mbot_max_trans_speed self._mbot_max_angular_speed = mbot_max_angular_speed self._map = None self._mbot = None self._lidar = None self._odom_pose = geometry.Pose(0, 0, 0) self._last_pose = geometry.Pose(0, 0, 0) # Controller self._lcm = lcm.LCM("udpm://239.255.76.67:7667?ttl=2") # LCM update rate self._lcm_handle_rate = 100 # LCM channel names self._odometry_channel = 'ODOMETRY' self._motor_command_channel = 'MBOT_MOTOR_COMMAND' # Subscribe to lcm topics self._lcm.subscribe(self._motor_command_channel, self._motor_command_handler) # LCM callback thread self._lcm_thread = threading.Thread(target=self._handle_lcm) self._real_time_factor = real_time_factor # View self._render_lidar = render_lidar self._running = True self._display_surf = None self._width = width self._sprites = pygame.sprite.RenderUpdates() self._max_frame_rate = 50 self._space_converter = None
def __init__(self, world_map, max_trans_speed, max_angular_speed, real_time_factor): super(Mbot, self).__init__() # Model self._map = world_map self._pose = geometry.Pose(0, 0, 0) stop = mbot_motor_command_t() stop.utime = int(0) self._current_motor_commands = [stop] self._radius = 0.1 # Initialize trajectory to all 0 poses from now - trajectory_length to now for every trajectory step in time self._trajectory_length = 10.0 # Seconds self._trajectory_step = 0.005 # Seconds num_steps = int(self._trajectory_length / self._trajectory_step) start_time = time.perf_counter() - (num_steps * self._trajectory_step) self._trajectory = [ Mbot.State(geometry.Pose(0, 0, 0), geometry.Twist(0, 0, 0), start_time + (i * self._trajectory_step)) for i in range(num_steps) ] # TODO(???): Properly model the dependence between translation and angular speed, e.g. if at max angular speed # then translation speed must be zero self._max_trans_speed = max_trans_speed self._max_angular_speed = max_angular_speed self._moving = False # View self._primary_color = pygame.Color(0, 0, 255) self._secondary_color = pygame.Color(255, 0, 0) self._dir_indicator_arc_length = math.pi / 3 self.image = pygame.Surface([10, 10]) self.image.set_colorkey((0, 0, 0)) self.rect = self.image.get_rect() # Control self._trajectory_lock = threading.Lock() self._real_time_factor = real_time_factor
def _const_vel_motion(self, state, dt, angular_velocity_tolerance=1e-5): """! @brief Model constant velocity motion Equations of motion: dx = int_ti^tf trans_v * cos(vtheta * (t - ti) + theta_i) dt = vtheta != 0 --> (trans_v / vtheta) * (sin(vtheta * (t - ti) + theta_i)) - sin(theta_i) = vtheta == 0 --> trans_v * cos(theta_i) * (tf - ti) dy = int_ti^tf tans_v * sin(vtheta * (t - ti) + theta_i) dt = vtheta != 0 --> (-trans_v / vtheta) * (cos(vtheta * (t - ti) + theta_i)) - cos(theta_i) = vtheta == 0 --> trans_v * sin(theta_i) * (tf - ti) dtheta = vtheta * (t - ti) @param state The state at the start of the motion @param dt Delta time @param angular_velocity_tolerance Any angular velocity magnitude less than this is considered zero for numerical stability """ # Calculate the updates to x y and theta dx = 0 dy = 0 dtheta = state.twist.vtheta * dt # Small values are numerically unstable so treat as 0 if numpy.abs(state.twist.vtheta) <= angular_velocity_tolerance: dx = state.twist.vx * dt dy = state.twist.vy * dt dtheta = 0 else: trans_over_ang = numpy.sqrt(state.twist.vx**2 + state.twist.vy**2) / state.twist.vtheta dx = trans_over_ang * ( numpy.sin(state.twist.vtheta * dt + state.pose.theta) - numpy.sin(state.pose.theta)) dy = -trans_over_ang * ( numpy.cos(state.twist.vtheta * dt + state.pose.theta) - numpy.cos(state.pose.theta)) return geometry.Pose(dx, dy, dtheta)
import numpy as np import conversions as conv import geometry import sfmio objectPoints = np.array([[30, 0, 100], [35, 0, 100]]) cameraMatrix = np.matrix([[5000, 0, 0], [0, 5000, 0], [0, 0, 1]]) projectionCenter1 = np.array([0.0, 0.0, 0.0]) projectionCenter2 = np.array([20.0, 0.0, 0.0]) projectionCenter1 = projectionCenter1[:, np.newaxis] projectionCenter2 = projectionCenter2[:, np.newaxis] pose1 = geometry.Pose() pose2 = geometry.Pose() camera1 = geometry.PinholeCamera(pixelSizeMilimeters=0.004, numberOfRows=4000, numberOfColumns=6000, principalDistanceMilimeters=12.0) pose1.setRotationEuler(conv.degrees2Radians(np.array([25.0, -3.0, -0.75])), 'xyz') pose1.setTranslation(projectionCenter1) pose2.setRotationEuler(conv.degrees2Radians(np.array([0.0, -1.0, -0.55])), 'xyz') pose2.setTranslation(projectionCenter2) print("getting transformation matrix for pose 1") print(pose1.T)
import numpy as np import geometry as geom myObjectPoints = geom.ObjectPointCollection(collectionId = 1) myObjectPoints.reserve(7) myObjectPoints.insertPoint(0, 0, 0, 101, 0) myObjectPoints.insertPoint(-12, 12, 0, 102, 1) myObjectPoints.insertPoint(0, 12, 0, 103, 2) myObjectPoints.insertPoint(12, 12, 0, 104, 3) myObjectPoints.insertPoint(-12, -12, 0, 105, 4) myObjectPoints.insertPoint(0, -12, 0, 106, 5) myObjectPoints.insertPoint(12, -12, 0, 107, 6) myCamera = geom.PinholeCamera(pixelSizeMilimeters=0.006, numberOfRows=6000, numberOfColumns=4000, principalDistanceMilimeters=20 ) myPose = geom.Pose() translation = np.zeros((3,1)) translation[0,0] = 0.0 translation[1,0] = 0.0 translation[2,0] = 18.0 myPose.setTranslation(translation) myPose.setRotationEuler(np.array([0.0,0.0,0.0]), "xyz") myImage = geom.Image() myImage.setPose(myPose) myImage.setCamera(myCamera) geom.projectImagePointCollectionToImage(myImage,myObjectPoints,0,7)
def createImageCollectionFromXtrelReport(filename, collectionId): reportFile = open(filename,"r") lines = reportFile.readlines() numberOfImages = 0 #looking for line with number of images: for line in lines: if len(line) < 28: continue if line[0:27] == "Number of images :": elements = line.split() numberOfImages = int(elements[len(elements)-1]) positionOfCameras = lines.index("Cameras assignments and number of points:\n") cameraAssignments = {} cameraFiles = set() #reading file names of cameras for lineId in range(positionOfCameras+2, positionOfCameras + 2 + numberOfImages): elements = lines[lineId].split() cameraAssignments[elements[0]] = elements[1] cameraFiles.add(elements[1]) cameras = {} for cameraFile in cameraFiles: try: camera = readCameraFromCamFile(cameraFile) cameras[cameraFile] = camera except: print("Error while reading camera from cam file") #reading coordinates of projection centers positionOfCoordinatesStart = lines.index("Coordinates:\n") projectionCenterCoordinates = {} for lineId in range(positionOfCoordinatesStart+2, positionOfCoordinatesStart + 2 + numberOfImages): elements = lines[lineId].split() imageId = elements[1] coordinates = np.zeros((3,1)) coordinates[0,0] = float(elements[2]) coordinates[1,0] = float(elements[3]) coordinates[2,0] = float(elements[4]) projectionCenterCoordinates[imageId] = coordinates #reading rotation positionOfRotationsStart = lines.index("Rotation:\n") anglesRad = {} parametrization = {} for lineId in range(positionOfRotationsStart+2, positionOfRotationsStart + 2 + numberOfImages): elements = lines[lineId].split() imageId = elements[1] angles = np.zeros(3) angles[0] = conv.degrees2Radians(float(elements[3])) angles[1] = conv.degrees2Radians(float(elements[4])) angles[2] = conv.degrees2Radians(float(elements[5])) anglesRad[imageId] = angles parametrization[imageId] = elements[2] reportFile.close() #building collection: mapRotationSequenceToName = {"om-fi-ka" : "xyz", "al-ni-ka" : "zxz" } collectionOfImages = geometry.ImageCollection(collectionId = collectionId) for imageId, position in projectionCenterCoordinates.items(): pose = geometry.Pose() pose.setTranslation(position) pose.setRotationEuler(anglesRad[imageId], mapRotationSequenceToName[parametrization[imageId]] ) image = geometry.Image() image.setPose(pose) image.setCamera(cameras[cameraAssignments[imageId]]) image.rotationSequence = mapRotationSequenceToName[parametrization[imageId]] collectionOfImages.addImage(image, imageId) return collectionOfImages