def test_stereoCamera(self, params):
return
cam, p = params
camGroup = ct.CameraGroup(cam.projection,
(cam.orientation, ct.SpatialOrientation()))
cam1 = camGroup[0]
cam2 = camGroup[1]
cam1.tilt_deg = 0
cam1.roll_deg = 0
cam1.heading_deg = 0
cam2.tilt_deg = 0
cam2.roll_deg = 0
cam2.heading_deg = 0
cam2.pos_x_m = 10
p = np.array(p)
# transform point
p1, p2 = camGroup.imagesFromSpace(p)
p3 = camGroup.spaceFromImages(p1, p2)
# points behind the camera are allowed to be nan
p1_single = camGroup[0].spaceFromImage(camGroup[0].imageFromSpace(p))
p2_single = camGroup[1].spaceFromImage(camGroup[1].imageFromSpace(p))
p[np.isnan(p1_single)] = np.nan
p[np.isnan(p2_single)] = np.nan
np.testing.assert_almost_equal(
p,
p3,
4,
err_msg=
"Points from two cameras cannot be projected to the space and back"
)
def test_parameterSet(self):
cam = ct.Camera(ct.RectilinearProjection(), ct.SpatialOrientation())
cam.defaults.elevation_m = 99
assert cam.elevation_m == 99
assert cam.defaults.elevation_m == 99
cam.elevation_m = 10
assert cam.elevation_m == 10
assert cam.defaults.elevation_m == 99
cam.elevation_m = None
assert cam.elevation_m == 99
assert cam.defaults.elevation_m == 99
# test if the parameter sets are liked properly
cam.orientation.elevation_m = 900
assert cam.elevation_m == 900
cam.elevation_m = 800
assert cam.orientation.elevation_m == 800
# test non parameter
cam.foo = 99
assert cam.foo == 99
cam.defaults.foo = 99
assert cam.defaults.foo == 99
self.assertRaises(AttributeError, lambda: cam.bla)
self.assertRaises(AttributeError, lambda: cam.parameters.bla)
self.assertRaises(AttributeError, lambda: cam.projection.bla)
self.assertRaises(AttributeError, lambda: cam.orientation.bla)
self.assertRaises(AttributeError, lambda: cam.defaults.bla)
def orientation(draw,
elevation=st.floats(0, 1000),
tilt_deg=st.floats(0, 90),
roll_deg=st.floats(-180, 180),
heading_deg=st.floats(-360, 360),
x_m=st.floats(-100, 100),
y_m=st.floats(-100, 100)):
return ct.SpatialOrientation(draw(elevation), draw(tilt_deg),
draw(roll_deg), draw(heading_deg), draw(x_m),
draw(y_m))
def test_init_cam(self):
# intrinsic camera parameters
f = 6.2
sensor_size = (6.17, 4.55)
image_size = (3264, 2448)
# initialize the camera
cam = ct.Camera(
ct.RectilinearProjection(focallength_mm=f,
image_width_px=image_size[1],
image_height_px=image_size[0],
sensor_width_mm=sensor_size[1],
sensor_height_mm=sensor_size[0]),
ct.SpatialOrientation())
def camera_down_with_world_points(draw,
projection=projection(),
n=st.one_of(st.integers(1, 1000),
st.just(1))):
camera = ct.Camera(projection=draw(projection),
orientation=ct.SpatialOrientation())
n = draw(n)
# the points can either be
if n == 1:
points = draw(
st.tuples(st.floats(-100, 100), st.floats(-100, 100),
st.floats(-100, 100)))
else:
points = draw(
st_np.arrays(dtype="float",
shape=st.tuples(st.just(n), st.just(3)),
elements=st.floats(-100, 100)))
return camera, points
def updateCameraParameters(self):
"""
update camera dictionary for calculation - uses potentially user modified data from gui
"""
# update current cam parameters
self.cam = dotdict()
self.cam.fov_h_deg = getFloat(self.leFOV_horizontal.text())
self.cam.fov_v_deg = getFloat(self.leFOV_vertical.text())
self.cam.sensor_w_mm = getFloat(self.leSensor_width.text())
self.cam.sensor_h_mm = getFloat(self.leSensor_height.text())
self.cam.focallength_mm = getFloat(self.leFocallength.text())
self.cam.img_w_px = getInteger(self.leImage_width.text())
self.cam.img_h_px = getInteger(self.leImage_height.text())
self.cam.center_x_px = self.cam.img_w_px / 2. + getFloat(
self.leOffsetX.text())
self.cam.center_y_px = self.cam.img_h_px / 2. + getFloat(
self.leOffsetY.text())
self.cam.heading_deg = getFloat(self.leCamPan.text())
self.cam.tilt_deg = getFloat(self.leCamTilt.text())
self.cam.roll_deg = getFloat(self.leCamRoll.text())
self.cam.elevation_m = getFloat(self.leCamElevation.text())
self.cam.pos_x_m = getFloat(self.leCamPosX.text())
self.cam.pos_y_m = getFloat(self.leCamPosY.text())
self.cam.projection = self.cbProjection.itemText(
self.cbProjection.currentIndex())
self.cam.gps_lat = getFloat(self.leCamLat.text())
self.cam.gps_lon = getFloat(self.leCamLon.text())
if self.cam.sensor_h_mm is None or self.cam.sensor_w_mm is None:
self.calcSensorDimensionsFromFOV()
# save parameters as options to DB
self.setOption('DM_focallength_mm', self.cam.focallength_mm)
self.setOption('DM_fov_h_deg', self.cam.fov_h_deg)
self.setOption('DM_fov_v_deg', self.cam.fov_v_deg)
self.setOption('DM_sensor_w_mm', self.cam.sensor_w_mm)
self.setOption('DM_sensor_h_mm', self.cam.sensor_h_mm)
self.setOption('DM_img_w_px', self.cam.img_w_px)
self.setOption('DM_img_h_px', self.cam.img_h_px)
self.setOption('DM_offset_x_px', getFloat(self.leOffsetX.text()))
self.setOption('DM_offset_y_px', getFloat(self.leOffsetY.text()))
print("Camera:")
print(json.dumps(self.cam, indent=4, sort_keys=True))
self.position = dotdict()
self.position.cam_elevation = getFloat(self.leCamElevation.text())
self.position.plane_elevation = getFloat(self.lePlaneElevation.text())
# save parameters to options
self.setOption('DM_cam_elevation', self.position.cam_elevation)
self.setOption('DM_plane_elevation', self.position.plane_elevation)
print("Position:")
print(json.dumps(self.position, indent=4, sort_keys=True))
print(self.cam)
# update camera parameters
# self.camera = ct.CameraTransform(self.cam.focallength_mm, [self.cam.sensor_w_mm, self.cam.sensor_h_mm],[self.cam.img_w_px, self.cam.img_h_px])
orientation = ct.SpatialOrientation(heading_deg=self.cam.heading_deg,
tilt_deg=self.cam.tilt_deg,
roll_deg=self.cam.roll_deg,
elevation_m=self.cam.elevation_m,
pos_x_m=self.cam.pos_x_m,
pos_y_m=self.cam.pos_y_m)
if self.cam.projection == "Cylindrical":
projection = ct.CylindricalProjection(
focallength_mm=self.cam.focallength_mm,
image_width_px=self.cam.img_w_px,
image_height_px=self.cam.img_h_px,
sensor_width_mm=self.cam.sensor_w_mm,
sensor_height_mm=self.cam.sensor_h_mm,
center_x_px=self.cam.center_x_px,
center_y_px=self.cam.center_y_px)
elif self.cam.projection == "Equirectangular":
projection = ct.EquirectangularProjection(
focallength_mm=self.cam.focallength_mm,
image_width_px=self.cam.img_w_px,
image_height_px=self.cam.img_h_px,
sensor_width_mm=self.cam.sensor_w_mm,
sensor_height_mm=self.cam.sensor_h_mm,
center_x_px=self.cam.center_x_px,
center_y_px=self.cam.center_y_px)
else: # default to rectilinear projection
print("Defaulting to rectiliniear")
projection = ct.RectilinearProjection(
focallength_mm=self.cam.focallength_mm,
image_width_px=self.cam.img_w_px,
image_height_px=self.cam.img_h_px,
sensor_width_mm=self.cam.sensor_w_mm,
sensor_height_mm=self.cam.sensor_h_mm,
center_x_px=self.cam.center_x_px,
center_y_px=self.cam.center_y_px)
self.camera = ct.Camera(orientation=orientation, projection=projection)
self.camera.setGPSpos(lat=self.cam.gps_lat,
lon=self.cam.gps_lon,
elevation=self.cam.elevation_m)
def __init__(self, camera_params):
self.bridge = CvBridge()
for camera_id in range(camera_params['n_cameras']):
setattr(self, 'last_image_%02i' % (camera_id, ),
[[None, None, None]])
self.last_model_states = ModelStates()
self.spawned_objects = []
# Camera IDs and corresponding variables holding images
self.camera_type = camera_params['camera_type']
self.update_rate = camera_params['update_rate']
self.image_dims = camera_params['camera_resolution'] + (3, )
self.camera_dict = {'%02i' % (camera_id,): getattr(self, 'last_image_%02i'\
% (camera_id,)) for camera_id in range(camera_params['n_cameras'])}
# ROS Subscribers, Publishers, Services
for camera_id in range(camera_params['n_cameras']):
if self.camera_type in ['rgb', 'both']:
img_callback_rgb = self.__image_callback_wrapper(
camera_id, 'rgb')
topic_name_rgb = '/robot/camera_rgb_%02i' % (camera_id, )
setattr(
self, '__image_rgb_%02i_sub' % (camera_id, ),
rospy.Subscriber(topic_name_rgb,
Image,
img_callback_rgb,
queue_size=1))
if self.camera_type in ['dvs', 'both']:
img_callback_dvs = self.__image_callback_wrapper(
camera_id, 'dvs')
topic_name_dvs = '/robot/camera_dvs_%02i/events' % (
camera_id, )
setattr(
self, '__image_dvs_%02i_sub' % (camera_id, ),
rospy.Subscriber(topic_name_dvs,
EventArray,
img_callback_dvs,
queue_size=1))
self.__set_model_state_pub = rospy.Publisher('/gazebo/set_model_state',
ModelState,
queue_size=1)
rospy.wait_for_service('gazebo/get_model_state', 10.0)
self.__get_model_state_srv = rospy.ServiceProxy(
'gazebo/get_model_state', GetModelState)
# 3D voxel to 2D camera pixel projection parameters (useful for segmentation dataset)
self.cam_transform = []
for camera_id in range(camera_params['n_cameras']):
cam_pose = self.get_object_pose('camera_%02i' % (camera_id))
cam_pos = cam_pose.position
cam_ori = cam_pose.orientation
roll, pitch, yaw = euler_from_quaternion(
[cam_ori.x, cam_ori.y, cam_ori.z, cam_ori.w])
(roll, pitch, yaw) = (angle_in_rad * 180 / np.pi
for angle_in_rad in (roll, pitch, yaw))
cam_projection = ct.RectilinearProjection(
focallength_px=camera_params['focal_length_px'],
image=camera_params['camera_resolution'])
cam_orientation = ct.SpatialOrientation(pos_x_m=cam_pos.x,
pos_y_m=cam_pos.y,
elevation_m=cam_pos.z,
roll_deg=roll,
tilt_deg=90 - pitch,
heading_deg=90 - yaw)
cam_lens = ct.BrownLensDistortion(k1=0.0,
k2=0.0,
k3=0.0,
projection=cam_projection)
self.cam_transform.append(
ct.Camera(projection=cam_projection,
orientation=cam_orientation,
lens=cam_lens))
rospy.init_node('cdp4_data_collection')
rospy.wait_for_service('/gazebo/get_world_properties')
self.__physics_state = rospy.ServiceProxy(
'/gazebo/get_world_properties', GetWorldProperties)
while self.__physics_state().sim_time < 2:
print('Waiting for simulation to be started')
rospy.sleep(2)
self.__path_to_models = os.getenv('HOME') + '/.gazebo/models/'
rospy.wait_for_service('gazebo/set_model_state', 10.0)
self.__set_model_state_srv = rospy.ServiceProxy(
'gazebo/set_model_state', SetModelState)
rospy.wait_for_service('gazebo/spawn_sdf_entity')
self.__spawn_model_srv = rospy.ServiceProxy('/gazebo/spawn_sdf_entity',
SpawnEntity)
rospy.wait_for_service('gazebo/set_visual_properties')
self.__change_color_srv = rospy.ServiceProxy(
'gazebo/set_visual_properties', SetVisualProperties)
def simple_orientation(draw,
elevation=st.floats(0, 1000),
tilt_deg=st.floats(0, 90)):
return ct.SpatialOrientation(draw(elevation), draw(tilt_deg))