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
