def pan(self, dx, dy, dz, relative=False):
"""
Moves the center (look-at) position while holding the camera in place.
If relative=True, then the coordinates are interpreted such that x
if in the global xy plane and points to the right side of the view, y is
in the global xy plane and orthogonal to x, and z points in the global z
direction. Distances are scaled roughly such that a value of 1.0 moves
by one pixel on screen.
"""
if not relative:
self.opts['center'] += QtGui.QVector3D(dx, dy, dz)
else:
azim = self.opts['azimuth']*np.pi/180.0
fov = self.opts['fov']*np.pi/180.0
dist = self.opts['distance']
# How wide in the world's unit of length is the viewport divided by
# the pixel width of the viewport. This will become small when the
# world is tiny, and big when it is big.
scale = 2*dist*np.tan(0.5*fov)/self.width()
e_x = -scale*Vector(-np.sin(azim), np.cos(azim), 0)
e_y = -scale*Vector( np.cos(azim), np.sin(azim), 0)
e_z = scale*Vector( 0, 0, 1)
self.opts['center'] += e_x*dx + e_y*dy + e_z*dz
self.opts['center'] = Vector(self.opts['center'])
self.update()
def pan(self, dx, dy, dz, relative='global'):
"""
Moves the center (look-at) position while holding the camera in place.
============== =======================================================
**Arguments:**
*dx* Distance to pan in x direction
*dy* Distance to pan in y direction
*dx* Distance to pan in z direction
*relative* String that determines the direction of dx,dy,dz.
If "global", then the global coordinate system is used.
If "view", then the z axis is aligned with the view
direction, and x and y axes are inthe plane of the
view: +x points right, +y points up.
If "view-upright", then x is in the global xy plane and
points to the right side of the view, y is in the
global xy plane and orthogonal to x, and z points in
the global z direction.
============== =======================================================
Distances are scaled roughly such that a value of 1.0 moves
by one pixel on screen.
Prior to version 0.11, *relative* was expected to be either True (x-aligned) or
False (global). These values are deprecated but still recognized.
"""
# for backward compatibility:
relative = {True: "view-upright", False: "global"}.get(relative, relative)
if relative == 'global':
self.opts['center'] += QtGui.QVector3D(dx, dy, dz)
elif relative == 'view-upright':
cPos = self.cameraPosition()
cVec = self.opts['center'] - cPos
dist = cVec.length() ## distance from camera to center
xDist = dist * 2. * np.tan(0.5 * self.opts['fov'] * np.pi / 180.) ## approx. width of view at distance of center point
xScale = xDist / self.width()
zVec = QtGui.QVector3D(0,0,1)
xVec = QtGui.QVector3D.crossProduct(zVec, cVec).normalized()
yVec = QtGui.QVector3D.crossProduct(xVec, zVec).normalized()
self.opts['center'] = self.opts['center'] + xVec * xScale * dx + yVec * xScale * dy + zVec * xScale * dz
elif relative == 'view':
# pan in plane of camera
# obtain basis vectors
qc = self.opts['rotation'].conjugated()
xv = qc.rotatedVector( Vector(1,0,0) )
yv = qc.rotatedVector( Vector(0,1,0) )
zv = qc.rotatedVector( Vector(0,0,1) )
scale_factor = self.pixelSize( self.opts['center'] )
# apply translation
self.opts['center'] += scale_factor * (xv*-dx + yv*dy + zv*dz)
else:
raise ValueError("relative argument must be global, view, or view-upright")
self.update()
def update_quad(self, state):
quadrotor_position = np.array([[state.pn], [state.pe], [state.pd]]) # NED coordinates
# attitude of quadrotor as a rotation matrix R from body to inertial
R = self._Euler2Rotation(state.phi, state.theta, state.psi)
# rotate and translate points defining quadrotor
rotated_points = self._rotate_points(self.points, R)
translated_points = self._translate_points(rotated_points, quadrotor_position)
# convert North-East Down to East-North-Up for rendering
R = np.array([[0, 1, 0], [1, 0, 0], [0, 0, -1]])
translated_points = R @ translated_points
# convert points to triangular mesh defined as array of three 3D points (Nx3x3)
mesh = self._quadrotor_points_to_mesh(translated_points)
# initialize the drawing the first time update() is called
if not self.plot_initialized:
# initialize drawing of triangular mesh.
self.body = gl.GLMeshItem(vertexes=mesh, # defines the triangular mesh (Nx3x3)
vertexColors=self.meshColors, # defines mesh colors (Nx1)
drawEdges=True, # draw edges between mesh elements
smooth=False, # speeds up rendering
computeNormals=False) # speeds up rendering
self.window.addItem(self.body) # add body to plot
self.plot_initialized = True
# else update drawing on all other calls to update()
else:
# reset mesh using rotated and translated points
self.body.setMeshData(vertexes=mesh, vertexColors=self.meshColors)
# update the center of the camera view to the quadrotor location
view_location = Vector(state.pe, state.pn, -state.pd) # defined in ENU coordinates
self.window.opts['center'] = view_location
# redraw
self.app.processEvents()
def __init__(self):
self.scale = 4000
# initialize Qt gui application and window
self.app = pg.QtGui.QApplication([]) # initialize QT
self.window = gl.GLViewWidget() # initialize the view object
self.window.setWindowTitle('Path Viewer')
self.window.setGeometry(
0, 0, 1500,
1500) # args: upper_left_x, upper_right_y, width, height
grid = gl.GLGridItem() # make a grid to represent the ground
grid.scale(self.scale / 20, self.scale / 20, self.scale /
20) # set the size of the grid (distance between each line)
self.window.addItem(grid) # add grid to viewer
self.window.setCameraPosition(distance=self.scale,
elevation=90,
azimuth=-90)
view_location = Vector(2000, 2000, 3000) # defined in ENU coordinates
# self.window.setCameraPosition(distance=self.scale, elevation=50, azimuth=-90)
# view_location = Vector(2000, 2000, 0) # defined in ENU coordinates
self.window.opts['center'] = view_location
self.window.setBackgroundColor('k') # set background color to black
self.window.show() # display configured window
self.window.raise_() # bring window to the front
self.plot_initialized = False # has the mav been plotted yet?
# get points that define the non-rotated, non-translated mav and the mesh colors
self.mav_points, self.mav_meshColors = self.get_mav_points()
# dubins path parameters
self.dubins_path = dubins_parameters()
self.mav_body = []
def updateview_camera(sr):
global fire
if fire:
x = sr.ui.slider_3d_camera_x.value()
y = sr.ui.slider_3d_camera_y.value()
z = sr.ui.slider_3d_camera_z.value()
distance = sr.ui.slider_3d_distance.value()
azimute = sr.ui.slider_3d_azimute.value()
elevation = sr.ui.slider_3d_elevation.value()
sr.scratchReader.options.put("view",
"3d",
"camera",
"position",
value=[x, y, z])
sr.scratchReader.options.put("view",
"3d",
"camera",
"azimute",
value=azimute)
sr.scratchReader.options.put("view",
"3d",
"camera",
"distance",
value=distance)
sr.scratchReader.options.put("view",
"3d",
"camera",
"elevation",
value=elevation)
sr.ui.plot3d_pot_widget.opts["center"] = Vector(x, y, z)
sr.ui.plot3d_pot_widget.setCameraPosition(distance=distance,
elevation=elevation,
azimuth=azimute)
def track3D(state):
app = QtGui.QApplication([])
w = gl.GLViewWidget()
w.setWindowTitle('3d trajectory')
w.resize(600, 500)
# instance of Custom3DAxis
axis = Custom3DAxis(w, color=(0.6, 0.6, 0.2, .6))
w.addItem(axis)
w.opts['distance'] = 75
w.opts['center'] = Vector(0, 0, 15)
# add xy grid
gx = gl.GLGridItem()
gx.setSize(x=40, y=40, z=10)
gx.setSpacing(x=5, y=5)
w.addItem(gx)
# trajectory line
pos0 = np.array([[0, 0, 0]])
pos, q = np.array(state[:3]), state[3:7]
uAxis, angle = q2ua(*q)
track0 = np.concatenate((pos0, pos.reshape(1, 3)))
plt = gl.GLLinePlotItem(pos=track0, width=2, color=(1, 0, 0, .6))
w.addItem(plt)
# orientation arrow
sphereData = gl.MeshData.sphere(rows=20, cols=20, radius=0.3)
sphereMesh = gl.GLMeshItem(meshdata=sphereData, smooth=True, shader='shaded', glOptions='opaque')
w.addItem(sphereMesh)
ArrowData = gl.MeshData.cylinder(rows=20, cols=20, radius=[0.2, 0.], length=0.7)
ArrowMesh = gl.GLMeshItem(meshdata=ArrowData, smooth=True, color=(1, 0, 0, 0.6), shader='balloon',
glOptions='opaque')
ArrowMesh.rotate(90, 0, 1, 0)
w.addItem(ArrowMesh)
w.show()
i = 1
pts = pos.reshape(1, 3)
def update():
'''update position and orientation'''
nonlocal i, pts, state
pos, q = np.array(state[:3]) * 100, state[3:7]
uAxis, angle = q2ua(*q)
pt = (pos).reshape(1, 3)
if pts.size < 150:
pts = np.concatenate((pts, pt))
else:
pts = np.concatenate((pts[-50:, :], pt))
plt.setData(pos=pts)
ArrowMesh.resetTransform()
sphereMesh.resetTransform()
ArrowMesh.rotate(angle, uAxis[0], uAxis[1], uAxis[2])
ArrowMesh.translate(*pos)
sphereMesh.translate(*pos)
i += 1
timer = QtCore.QTimer()
timer.timeout.connect(update)
timer.start(50)
if (sys.flags.interactive != 1) or not hasattr(QtCore, 'PYQT_VERSION'):
QtGui.QApplication.instance().exec_()
def update(
self, t, R
): #Remember that viewer operates in a ENU frame. May need to adjust this a little bit if it doesn't do what I want
#translate and rotate points here
trans_pts = R @ self.points
trans_pts = trans_pts + t[:, None]
R2 = np.array(
[[0, 1, 0], [1, 0, 0], [0, 0, -1]]
) #This converts to ENU. The result is the same as using the one to SEU
# R2 = np.array([[-1.0, 0.0, 0], [0, 1, 0], [0, 0, -1]])
trans_pts = R2 @ trans_pts
mesh = self.pointsToMesh(trans_pts)
if not self.plot_initialize:
self.body = gl.GLMeshItem(vertexes=mesh,
vertexColors=self.mesh_colors,
drawEdges=True,
smooth=False,
computeNormals=False)
self.window.addItem(self.body)
self.plot_initialize = True
else:
self.body.setMeshData(vertexes=mesh, vertexColors=self.mesh_colors)
view_location = Vector(t[1], t[0], -t[2]) #in ENU frame
# self.window.opts['center'] = view_location
self.application.processEvents()
def __init__(self, parent=None):
if GLViewWidget.ShareWidget is None:
## create a dummy widget to allow sharing objects (textures, shaders, etc) between views
GLViewWidget.ShareWidget = QtOpenGL.QGLWidget()
QtOpenGL.QGLWidget.__init__(self, parent, GLViewWidget.ShareWidget)
self.setFocusPolicy(QtCore.Qt.ClickFocus)
self.opts = {
'center':
Vector(0, 0, 0), ## will always appear at the center of the widget
'distance': 10.0, ## distance of camera from center
'fov': 60, ## horizontal field of view in degrees
'elevation': 30, ## camera's angle of elevation in degrees
'azimuth': 45, ## camera's azimuthal angle in degrees
## (rotation around z-axis 0 points along x-axis)
'viewport': None, ## glViewport params; None == whole widget
}
self.items = []
self.noRepeatKeys = [
QtCore.Qt.Key_Right, QtCore.Qt.Key_Left, QtCore.Qt.Key_Up,
QtCore.Qt.Key_Down, QtCore.Qt.Key_PageUp, QtCore.Qt.Key_PageDown
]
self.keysPressed = {}
self.keyTimer = QtCore.QTimer()
self.keyTimer.timeout.connect(self.evalKeyState)
self.makeCurrent()
def __init__(self, parent=None, devicePixelRatio=None):
global ShareWidget
if ShareWidget is None:
## create a dummy widget to allow sharing objects (textures, shaders, etc) between views
ShareWidget = QtOpenGL.QGLWidget()
QtOpenGL.QGLWidget.__init__(self, parent, ShareWidget)
self.setFocusPolicy(QtCore.Qt.ClickFocus)
self.opts = {
'center': Vector(0,0,0), ## will always appear at the center of the widget
'rotation' : QtGui.QQuaternion(1,0,0,0), ## camera rotation (quaternion:wxyz)
'distance': 10.0, ## distance of camera from center
'fov': 60, ## horizontal field of view in degrees
'viewport': None, ## glViewport params; None == whole widget
'devicePixelRatio': devicePixelRatio,
}
self.setBackgroundColor('k')
self.items = []
self.noRepeatKeys = [QtCore.Qt.Key_Right, QtCore.Qt.Key_Left, QtCore.Qt.Key_Up, QtCore.Qt.Key_Down, QtCore.Qt.Key_PageUp, QtCore.Qt.Key_PageDown]
self.keysPressed = {}
self.keyTimer = QtCore.QTimer()
self.keyTimer.timeout.connect(self.evalKeyState)
self.makeCurrent()
def update(self, state):
#This will update the animation
mav_position = np.array([[state.pn], [state.pe], [-state.h]])
R = self.EulerToRotation(state.phi, state.theta, state.psi)
rotated_pts = self.rotatePoints(self.points, R)
trans_pts = self.translatePoints(rotated_pts, mav_position)
R2 = np.array([[0, 1, 0], [1, 0, 0],
[0, 0, -1]]) # Convert to ENU coordinates for rendering
trans_pts = R2 @ trans_pts
mesh = self.pointsToMesh(trans_pts)
if not self.plot_initialize:
self.body = gl.GLMeshItem(
vertexes=mesh, #defines mesh (Nx3x3)
vertexColors=self.mesh_colors,
drawEdges=True,
smooth=False, #speeds up rendering
computeNormals=False) # speeds up rendering
self.window.addItem(self.body)
self.plot_initialize = True
else:
self.body.setMeshData(vertexes=mesh, vertexColors=self.mesh_colors)
view_location = Vector(state.pe, state.pn, state.h) # in ENU frame
self.window.opts['center'] = view_location
self.application.processEvents() #redraw
def update(self, path, state):
"""
Update the drawing of the mav.
The input to this function is a (message) class with properties that define the state.
The following properties are assumed:
state.pn # north position
state.pe # east position
state.h # altitude
state.phi # roll angle
state.theta # pitch angle
state.psi # yaw angle
"""
mav_position = np.array([[state.pn], [state.pe],
[-state.h]]) # NED coordinates
# attitude of mav as a rotation matrix R from body to inertial
R = Euler2Rotation(state.phi, state.theta, state.psi)
# rotate and translate points defining mav
rotated_points = self._rotate_points(self.points, R)
translated_points = self._translate_points(rotated_points,
mav_position)
# convert North-East Down to East-North-Up for rendering
R = np.array([[0, 1, 0], [1, 0, 0], [0, 0, -1]])
translated_points = R @ translated_points
# convert points to triangular mesh defined as array of three 3D points (Nx3x3)
mesh = self._points_to_mesh(translated_points)
# initialize the drawing the first time update() is called
if not self.plot_initialized:
if path.flag == 'line':
straight_line_object = self.straight_line_plot(path)
self.window.addItem(
straight_line_object) # add straight line to plot
else: # path.flag=='orbit
orbit_object = self.orbit_plot(path)
self.window.addItem(orbit_object)
# initialize drawing of triangular mesh.
self.body = gl.GLMeshItem(
vertexes=mesh, # defines the triangular mesh (Nx3x3)
vertexColors=self.meshColors, # defines mesh colors (Nx1)
drawEdges=True, # draw edges between mesh elements
smooth=False, # speeds up rendering
computeNormals=False) # speeds up rendering
self.window.addItem(self.body) # add body to plot
self.plot_initialized = True
# else update drawing on all other calls to update()
else:
# reset mesh using rotated and translated points
self.body.setMeshData(vertexes=mesh, vertexColors=self.meshColors)
# update the center of the camera view to the mav location
view_location = Vector(state.pe, state.pn,
state.h) # defined in ENU coordinates
self.window.opts['center'] = view_location
# redraw
self.app.processEvents()
def update(self, state):
if not self.plot_initialised:
self.uav_plot = DrawUAV(state, self.window)
self.plot_initialised = True
else:
self.uav_plot.update(state)
view_location = Vector(state.py, state.px, state.h)
self.window.opts['center'] = view_location
self.app.processEvents()
def cameraPosition(self):
"""Return current position of camera based on center, dist, elevation, and azimuth"""
center = self.opts['center']
dist = self.opts['distance']
elev = self.opts['elevation'] * np.pi / 180.
azim = self.opts['azimuth'] * np.pi / 180.
pos = Vector(center.x() + dist * np.cos(elev) * np.cos(azim),
center.y() + dist * np.cos(elev) * np.sin(azim),
center.z() + dist * np.sin(elev))
return pos
def update(self, state):
# initialize the drawing the first time update() is called
if not self.plot_initialized:
self.mav_plot = DrawMav(state, self.window)
self.plot_initialized = True
# else update drawing on all other calls to update()
else:
self.mav_plot.update(state)
# update the center of the camera view to the mav location
view_location = Vector(state.east, state.north, state.altitude) # defined in ENU coordinates
self.window.opts['center'] = view_location
# redraw
self.app.processEvents()
def update(self, state):
# initialize the drawing the first time update() is called
if not self.plot_initialized:
self.quad_plot = drawQuad(state, self.window, self.arm_length)
self.plot_initialized = True
# else update drawing on all other calls to update()
else:
self.quad_plot.update(state)
# update the center of the camera view to the mav location
view_location = Vector(state.item(1), state.item(0),
-state.item(2)) # defined in ENU coordinates
self.window.opts['center'] = view_location
self.tick()
def update(self, state):
# initialize the drawing the first time update() is called
if not self.plot_initialized:
self.mav_plot = drawMav(state, self.window)
self.plot_initialized = True
# else update drawing on all other calls to update()
else:
self.mav_plot.update(state)
# update the center of the camera view to the mav location
view_location = Vector(state.pe, state.pn,
state.h) # defined in ENU coordinates
self.window.opts['center'] = view_location
self.window.setCameraPosition(
azimuth=-np.degrees(state.psi + state.beta) - 90,
elevation=-np.degrees(state.theta) + 10)
# redraw
self.app.processEvents()
def update(self, waypoints, path, state):
# initialize the drawing the first time update() is called
if not self.plot_initialized:
self.drawMAV(state)
self.drawWaypoints(waypoints, path.orbit_radius)
self.drawPath(path)
self.plot_initialized = True
# else update drawing on all other calls to update()
else:
self.drawMAV(state)
if waypoints.flag_waypoints_changed == True:
self.drawWaypoints(waypoints, path.orbit_radius)
if path.flag_path_changed == True:
self.drawPath(path)
# update the center of the camera view to the mav location
view_location = Vector(state.pe, state.pn,
state.h) # defined in ENU coordinates
self.window.opts['center'] = view_location
# redraw
self.app.processEvents()
def follow(pdf):
global prev, i
x, y = np.where(pdf == np.amax(pdf))
x = x[0]
y = y[0]
#print('>>> FOLLOWING', x,y)
x = x/pdf.shape[0]
y = y/pdf.shape[1]
x *= rescale
y *= rescale
x -= rescale/2
y -= rescale/2
xy = np.array([x,y])
cx = np.array([w.opts['center'].x(), w.opts['center'].y()])
dx = xy - cx
#print('>>> FOLLOWING', xy)
#ki = .05 # fast follow
ki = .01 # medium follow
#ki = .005 # slow follow
i += ki*(dx)
new_xy = i
#d = .005*((dx) - prev)
#new_xy += d
prev = (new_xy).copy()
w.opts['center'] = Vector(new_xy[0], new_xy[1], 0)
def __init__(self):
super().__init__()
self.grid_size = 50
self.pos_reduction = 1e-06
self.default_vol = 0.01
self.vol_sun = 20
self.absolute_vol_sun = 1.412e18
self.setCameraPosition(distance=self.grid_size, pos=Vector(0, 0, -10))
"""grid_vector = QVector3D(self.grid_size, self.grid_size, self.grid_size)
x_grid = gl.GLGridItem(grid_vector, color=(255, 255, 255, 40))
y_grid = gl.GLGridItem(grid_vector)
z_grid = gl.GLGridItem(grid_vector)
self.addItem(x_grid)
self.addItem(y_grid)
self.addItem(z_grid)"""
sphere = gl.MeshData.sphere(15, 15, self.vol_sun)
sun = gl.GLMeshItem(meshdata=sphere, color=mkColor(250, 250, 0))
sun.translate(*self.get_position(1))
self.addItem(sun)
def run_obstacle_detection():
global qapp
if show_point_cloud:
# QT app
gl_widget = gl.GLViewWidget()
gl_widget.show()
gl_grid = gl.GLGridItem()
gl_widget.addItem(gl_grid)
# initialize some points data
pos = np.zeros((1, 3))
sp2 = gl.GLScatterPlotItem(pos=pos)
sp2.setGLOptions(
'opaque'
) # Ensures not to allow vertexes located behind other vertexes to be seen.
gl_widget.addItem(sp2)
if save_frames:
try:
os.mkdir(saved_dir)
except FileExistsError as e:
pass
for f in os.listdir(saved_dir):
os.remove(saved_dir + f)
thetas = np.array([])
phis = np.array([])
obstacle_list = []
obstacle_id = 0
for i in range(0, len(os.listdir(frames_dir))):
#print(i)
depth_frame = np.load(frames_dir + '/%d.npy' % i)
h, w = depth_frame.shape[:2]
depth_frame[depth_frame > depth_cutoff] = 0
depth_frame[:y_cutoff, ...] = 0
obstacles = np.zeros(
depth_frame.shape
) # empty image that will store the locations of detected obstacles
img = np.uint8(depth_frame / 4500. * 255.)
ground_plane_roi = depth_frame.copy()
ground_plane_roi[:y_cutoff + 100, ...] = 0
mask = create_circular_mask(h, w, center=[roi_x, roi_y], radius=100)
ground_plane_roi[mask] = 0
xyz_arr = depth_matrix_to_point_cloud(
depth_frame) # convert depth data to XYZ coordinates
roi_point_cloud = depth_matrix_to_point_cloud(ground_plane_roi)
num_points = np.count_nonzero(roi_point_cloud[..., 0]) // 100
center, plane, phi, theta = get_orientation(roi_point_cloud,
num_points, 1)
thetas = np.append(thetas, theta)
phis = np.append(phis, phi)
#print(center, plane, np.median(theta), np.median(phis))
CameraPosition['elevation'] = np.median(thetas)
CameraPosition['azimuth'] = np.median(phis)
if thetas.size > memory:
thetas = thetas[1:]
if phis.size > memory:
phis = phis[1:]
center = apply_camera_orientation(center, CameraPosition)
plane = apply_camera_orientation(plane, CameraPosition)
xyz_arr = apply_camera_matrix_orientation(xyz_arr, CameraPosition)
plane_img = np.zeros(img.size)
plane_img[xyz_arr[:, 2] > dist_thresh + center[2]] = 1
plane_img = np.uint8(
np.reshape(plane_img, (424, 512)) *
255) # reshape to match depth data and convert to uint8
plane_img = np.uint8(
(np.ones((424, 512)) * 255) - plane_img
) # invert img so pixel value corresponds to NOT ground plane
ret, plane_img = cv2.threshold(
plane_img, 0, 255, cv2.THRESH_BINARY
) # filter points that are probaly not ground plane
plane_img = cv2.subtract(img, plane_img)
# noise removal
kernel = np.ones((3, 3), np.uint8)
opening = cv2.morphologyEx(
plane_img, cv2.MORPH_OPEN, kernel,
iterations=3) # erosion followed by dilation
# erosiong
opening = cv2.erode(opening, kernel=kernel, iterations=1)
color = cv2.cvtColor(img,
cv2.COLOR_GRAY2BGR) # BGR image to draw labels on
# begin contour detection
contours, hierarchy = cv2.findContours(opening, cv2.RETR_TREE,
cv2.CHAIN_APPROX_SIMPLE)
color = cv2.drawContours(color, contours, -1, (0, 255, 0), 1)
for cntr in contours:
try:
# calculate diameter of equivalent circle
# this measurement is only used for checking if countours fit our bounds
area = cv2.contourArea(cntr)
equi_diameter = np.sqrt(4 * area / np.pi)
# Hardcoded Diameter Range in pixels
LOW_DIAMETER_BOUND = 20
HIGH_DIAMETER_BOUND = 150
HIGH_DISTANCE_BOUND = 4500
# Original tolerances were 20 and 150
if (equi_diameter > LOW_DIAMETER_BOUND
and equi_diameter < HIGH_DIAMETER_BOUND):
mask = np.zeros_like(
img
) # mask will contain the fitted and adjusted ellipse of a single obstacle
ellipse = cv2.fitEllipse(cntr)
x, y, obj_length, obj_height = cv2.boundingRect(cntr)
rect = cv2.minAreaRect(cntr)
equi_diameter = obj_length # bounding rectangle gives a better approximation of diameter
box = cv2.boxPoints(rect)
box = np.int0(box)
mask = cv2.ellipse(mask, ellipse, (255, 255, 255),
-1) # draw the fitted ellipse
rows = mask.shape[0]
cols = mask.shape[1]
M = np.float32([[1, 0, 0], [
0, 1, equi_diameter / 4
]]) # shift mask down to match obstacle, not edge
mask = cv2.warpAffine(mask, M, (cols, rows))
mask = cv2.erode(
mask, kernel, iterations=3
) # erode the mask to remove background points
img_fg = cv2.bitwise_and(
depth_frame, depth_frame, mask=mask
) # use the mask to isolate original depth values
img_fg = cv2.medianBlur(
img_fg, 5) # median blur to further remove noise
obstacles = cv2.add(np.float32(img_fg),
np.float32(obstacles))
mean_val = np.median(
img_fg[img_fg.nonzero()]
) # compute the non-zero average of obstacle depth values
moment = cv2.moments(
cntr
) # get the centroid of the obstacle using its moment
cx = int(moment['m10'] / moment['m00'])
cy = int(moment['m01'] / moment['m00'])
if mean_val < HIGH_DISTANCE_BOUND: # kinect loses accuracy beyond 4.5m
coords = depth_to_point_cloud_pos(
cx, cy, mean_val
) # convert obstacle depth to XYZ coordinate
mm_diameter = equi_diameter * (
1.0 / CameraParams['fx']
) * mean_val # convert pixel diameter to mm
if 100 < mm_diameter < 400:
new_obstacle = True
current_obstacle = None
for obstacle in obstacle_list:
x_match = abs(obstacle.x - coords[0]) < 0.3
y_match = abs(obstacle.y - coords[1]) < 0.3
z_match = abs(obstacle.z - mean_val) < 0.5
diameter_match = abs(obstacle.diameter -
mm_diameter) / 1000. < 0.5
if x_match and y_match:
obstacle.x = coords[0]
obstacle.y = coords[1]
obstacle.z = coords[2]
obstacle.diameter = mm_diameter / 1000
new_obstacle = False
obstacle.lifetime = obstacle_lifetime
current_obstacle = Obstacle(
obstacle.id, obstacle.x, obstacle.y,
obstacle.z, obstacle.diameter,
obstacle_lifetime)
if obstacle.lifetime == 0:
obstacle_list.remove(obstacle)
break
if new_obstacle:
current_obstacle = Obstacle(
obstacle_id, coords[0], coords[1],
coords[2], mm_diameter, obstacle_lifetime)
obstacle_id += 1
obstacle_list.append(current_obstacle)
if visualize:
# begin visualization
cv2.drawContours(color, [box], 0, (0, 0, 255),
1)
cv2.rectangle(color, (x, y),
(x + obj_length, y + obj_height),
(0, 255, 0), 2)
font = cv2.FONT_HERSHEY_SIMPLEX
cv2.putText(color,
'id = %d' % current_obstacle.id,
(cx, cy + 15), font, 0.4,
(255, 0, 255), 1, cv2.LINE_AA)
cv2.putText(color, "x = %.2f" % coords[0],
(cx, cy + 30), font, 0.4,
(0, 0, 255), 1, cv2.LINE_AA)
cv2.putText(color, "y = %.2f" % coords[1],
(cx, cy + 45), font, 0.4,
(0, 255, 0), 1, cv2.LINE_AA)
cv2.putText(color,
"z = %.2f" % (mean_val / 1000),
(cx, cy + 60), font, 0.4,
(255, 0, 127), 1, cv2.LINE_AA)
cv2.putText(
color,
"diameter = %.2f" % (mm_diameter / 1000),
(cx, cy + 75), font, 0.4, (255, 127, 0), 1,
cv2.LINE_AA)
except cv2.error as e:
print(e)
pass
if save_frames:
plt.imsave(saved_dir + '%d.png' % i, color)
if show_point_cloud:
# Calculate a dynamic vertex size based on window dimensions and camera's position - To become the "size" input for the scatterplot's setData() function.
v_rate = 5.0 # Rate that vertex sizes will increase as zoom level increases (adjust this to any desired value).
v_scale = np.float32(v_rate) / gl_widget.opts[
'distance'] # Vertex size increases as the camera is "zoomed" towards center of view.
v_offset = (
gl_widget.geometry().width() / 2000
)**2 # Vertex size is offset based on actual width of the viewport.
v_size = v_scale + v_offset
set_point_cloud_position(gl_widget,
pos=Vector(center[1] + 2, center[2],
center[0] - 1))
point_cloud_color = color.copy()
_, plane_thresh = cv2.threshold(plane_img, 1, 2, cv2.THRESH_BINARY)
point_cloud_color[:, :, 1] *= plane_thresh
colors = np.divide(point_cloud_color,
255) # values must be between 0.0 - 1.0
colors = colors.reshape(colors.shape[0] * colors.shape[1], 3)
#colors = colors[..., ::-1] # BGR to RGB
sp2.setData(pos=xyz_arr, size=v_size, color=colors)
if visualize:
cv2.imshow('detected_obstacles', color)
cv2.imshow('plane', plane_img)
cv2.imshow('roi', np.uint8(ground_plane_roi / 4500. * 255.))
for obstacle in obstacle_list:
obstacle.lifetime -= 1
if obstacle.lifetime == 0:
obstacle_list.remove(obstacle)
print(obstacle)
print('\n')
key = cv2.waitKey(delay=1)
if key == ord('q'):
break
def pan(self, x, y, z):
from pyqtgraph import Vector
self.w.opts['center'] = Vector(x, y, z)
def draw_mav(self, state, y_manager):
"""
Draws the location of the mav at the specified location with the given roll pitch and yaw
:param state: The state that the mav should be drawn. [X, Y, Z, Roll, Pitch, Yaw]
:return: null
"""
#pull out the relevant information from the state vector
loc = np.array([state[0], -state[1], -state[2]])
phi = state[3]
theta = -state[4]
psi = -state[5]
if not self.init: #create body if not yet initialized
self.body = gl.GLMeshItem(vertexes=self.verts, smooth=False, drawEdges=False, vertexColors=self.colors) #create meshes
self.w.addItem(self.body) #add meshes to plot
path_pos = np.array([[0, 0, 0], [0, 0, 0]])
path_color = [0, 255, 0, 1]
self.path = gl.GLLinePlotItem(pos=path_pos, color=path_color, width=1, antialias=True, mode='lines')
self.w.addItem(self.path)
if self.init:
# rotate bodies to specified state
#precalculate sines and cosines
cPhi = np.cos(phi)
sPhi = np.sin(phi)
cTheta = np.cos(theta)
sTheta = np.sin(theta)
cPsi = np.cos(psi)
sPsi = np.sin(psi)
# rotMat = np.array([[cTheta * cPsi, -cTheta*sPsi, sTheta], #Make the rotation matrix
# [sPhi * sTheta * cPsi + cPhi * sPsi, -sPhi * sTheta * sPsi + cPhi * cPsi, -sPhi * cTheta],
# [-cPhi * sTheta * cPsi + sPhi * sPsi, cPhi * sTheta * sPsi + sPhi * cPsi, cPhi * cTheta]])
# rotMat = np.array([[cPhi * cTheta, cPhi * sTheta * sPsi - sPhi * cPsi, cPhi * sTheta * cPsi + sPhi * sPsi],
# [sPhi * cTheta, sPhi * sTheta * sPsi + cPhi * cPsi, sPhi * sTheta * cPsi - cPhi * sPsi],
# [-sTheta, cTheta*sPsi, cTheta * cPsi]])
r_roll = np.array([[1, 0, 0],
[0, cPhi, -sPhi],
[0, sPhi, cPhi]])
r_pitch = np.array([[cTheta, 0, sTheta],
[0, 1, 0],
[-sTheta, 0, cTheta]])
r_yaw = np.array([[cPsi, -sPsi, 0],
[sPsi, cPsi, 0],
[0, 0, 1]])
# rotMat = r_yaw * r_pitch * r_roll
# rotMat = r_roll * r_pitch * r_yaw
# newPoints = np.array([rotMat.dot(P.points[0]) + loc, #rotate and translate original points
# rotMat.dot(P.points[1]) + loc,
# rotMat.dot(P.points[2]) + loc,
# rotMat.dot(P.points[3]) + loc,
# rotMat.dot(P.points[4]) + loc,
# rotMat.dot(P.points[5]) + loc,
# rotMat.dot(P.points[6]) + loc,
# rotMat.dot(P.points[7]) + loc,
# rotMat.dot(P.points[8]) + loc,
# rotMat.dot(P.points[9]) + loc,
# rotMat.dot(P.points[10]) + loc,
# rotMat.dot(P.points[11]) + loc,
# rotMat.dot(P.points[12]) + loc,
# rotMat.dot(P.points[13]) + loc,
# rotMat.dot(P.points[14]) + loc,
# rotMat.dot(P.points[15]) + loc])
newPoints = np.array([np.dot(r_yaw,np.dot(r_pitch, np.dot(r_roll, P.points[0]))) + loc,
np.dot(r_yaw,np.dot(r_pitch, np.dot(r_roll, P.points[1]))) + loc,
np.dot(r_yaw,np.dot(r_pitch, np.dot(r_roll, P.points[2]))) + loc,
np.dot(r_yaw,np.dot(r_pitch, np.dot(r_roll, P.points[3]))) + loc,
np.dot(r_yaw,np.dot(r_pitch, np.dot(r_roll, P.points[4]))) + loc,
np.dot(r_yaw,np.dot(r_pitch, np.dot(r_roll, P.points[5]))) + loc,
np.dot(r_yaw,np.dot(r_pitch, np.dot(r_roll, P.points[6]))) + loc,
np.dot(r_yaw,np.dot(r_pitch, np.dot(r_roll, P.points[7]))) + loc,
np.dot(r_yaw,np.dot(r_pitch, np.dot(r_roll, P.points[8]))) + loc,
np.dot(r_yaw,np.dot(r_pitch, np.dot(r_roll, P.points[9]))) + loc,
np.dot(r_yaw,np.dot(r_pitch, np.dot(r_roll, P.points[10]))) + loc,
np.dot(r_yaw,np.dot(r_pitch, np.dot(r_roll, P.points[11]))) + loc,
np.dot(r_yaw,np.dot(r_pitch, np.dot(r_roll, P.points[12]))) + loc,
np.dot(r_yaw,np.dot(r_pitch, np.dot(r_roll, P.points[13]))) + loc,
np.dot(r_yaw,np.dot(r_pitch, np.dot(r_roll, P.points[14]))) + loc,
np.dot(r_yaw,np.dot(r_pitch, np.dot(r_roll, P.points[15]))) + loc])
#convert points to xyz coordinates
newVerts = np.array([[newPoints[0], newPoints[1], newPoints[2]], #Make the new verticies of the meshes
[newPoints[0], newPoints[1], newPoints[4]],
[newPoints[0], newPoints[3], newPoints[4]],
[newPoints[0], newPoints[3], newPoints[2]],
[newPoints[5], newPoints[2], newPoints[3]],
[newPoints[5], newPoints[1], newPoints[2]],
[newPoints[5], newPoints[1], newPoints[4]],
[newPoints[5], newPoints[3], newPoints[4]],
[newPoints[6], newPoints[7], newPoints[9]],
[newPoints[7], newPoints[8], newPoints[9]],
[newPoints[10], newPoints[11], newPoints[12]],
[newPoints[10], newPoints[12], newPoints[13]],
[newPoints[5], newPoints[14], newPoints[15]]])
self.body.setMeshData(vertexes=newVerts, vertexColors=self.colors)
if y_manager is not None:
if y_manager[0] is True:
a = y_manager[2] - 500000 * y_manager[3]
b = y_manager[2] + 500000 * y_manager[3]
path_pos = np.array([[a.item(0), -a.item(1), -a.item(2)],
[b.item(0), -b.item(1), -b.item(2)]])
self.path.setData(pos=path_pos)
else:
c = y_manager[4]
rho = y_manager[5]
cx = c.item(0)
cy = -c.item(1)
cz = -c.item(2)
theta_circ = np.linspace(0, 2*np.pi, 100)
theta_circ = theta_circ.reshape((100,1))
x = cx + rho * np.cos(theta_circ)
y = cy + rho * np.sin(theta_circ)
z = np.linspace(cz, cz, 100)
z = z.reshape((100,1))
path_pos = np.hstack((x, y, z))
self.path.setData(pos=path_pos, mode='line_strip')
viewLoc = Vector(loc[0], loc[1], loc[2]) #vector to define the new camera view location
self.w.opts['center'] = viewLoc #update the camera view location to maintain UAV in the center of the view
if self.init is False: #Flip the init flag if it's false
self.init = True
def center_camera(self):
self.window.opts["center"] = Vector(self.aircraft_state.pn,
self.aircraft_state.pe,
-self.aircraft_state.pd)
def update(self):
if self.frame_idx < self.frame_count - 1:
self.frame_idx += 1
pts = (self.global_positions[self.frame_idx])[:, [0, 2, 1]]
self.points.setData(pos=pts, color=pg.glColor(1.0), size=10)
i = 0
for bone_dependency in self.bone_dependencies:
if bone_dependency[1] == -1:
continue
curr_bone_pos = pts[bone_dependency[0]]
parent_bone_pos = pts[bone_dependency[1]]
line = np.vstack((curr_bone_pos, parent_bone_pos))
self.bone_lines[i].setData(pos=line,
color=self.line_color,
width=self.line_width,
antialias=True)
i += 1
xs = self.global_positions[self.frame_idx, :, 0]
zs = self.global_positions[self.frame_idx, :, 2]
ys = self.global_positions[self.frame_idx, :, 1]
max_range = np.array([
xs.max() - xs.min(),
ys.max() - ys.min(),
zs.max() - zs.min()
]).max() / 2.0
mid_x = (xs.max() + xs.min()) * 0.5
mid_y = (ys.max() + ys.min()) * 0.5
mid_z = (zs.max() + zs.min()) * 0.5
curr_cam_pos = self.window.opts['center']
curr_dist = self.window.opts['distance']
new_dist = curr_dist + 0.05 * (max_range * 7 - curr_dist)
new_cam_pos = curr_cam_pos + 0.05 * (Vector(mid_x, mid_z, mid_y) -
curr_cam_pos)
self.window.setCameraPosition(pos=new_cam_pos, distance=new_dist)
if not self.heading_dirs is None:
origin = np.mean(pts, axis=0)
line, line2, line3 = self.__get_arrow(
self.heading_dirs[self.frame_idx], origin, new_dist / 5)
self.bone_lines[i].setData(pos=line,
color=self.arrow_color,
width=self.line_width,
antialias=True)
self.bone_lines[i + 1].setData(pos=line2,
color=self.arrow_color,
width=self.line_width,
antialias=True)
self.bone_lines[i + 2].setData(pos=line3,
color=self.arrow_color,
width=self.line_width,
antialias=True)
if self.write_to_file:
currQImage = self.window.grabFrameBuffer()
cvMat = QImageToCvMat(currQImage)
self.out.write(cvMat)
else:
if self.write_to_file:
self.out.release()
self.timer.stop()
self.window.clear()
self.window.reset()
self.app.closeAllWindows()
print("finished animation")
def update(self, state):
"""
Update the drawing of the spacecraft.
The input to this function is a (message) class with properties that define the state.
The following properties are assumed to be:
state.pn # north position
state.pe # east position
state.h # altitude
state.phi # roll angle
state.theta # pitch angle
state.psi # yaw angle
"""
spacecraft_position = np.array([[state.pn], [state.pe],
[-state.h]]) # NED coordinates
# attitude of spacecraft as a rotation matrix R from body to inertial
R = self._Euler2Rotation(state.phi, state.theta, state.psi)
# rotate and translate points defining spacecraft
rotated_points = self._rotate_points(self.points, R)
translated_points = self._translate_points(rotated_points,
spacecraft_position)
# convert North-East Down to East-North-Up for rendering
R_disp = np.array([[0, 1, 0], [1, 0, 0], [0, 0, -1]])
translated_points = np.matmul(R_disp, translated_points)
# convert points to triangular mesh defined as array of three 3D points (Nx3x3)
mesh = self._points_to_mesh(translated_points)
# initialize the drawing the first time update() is called
if not self.plot_initialized:
# initialize drawing of triangular mesh.
self.body = gl.GLMeshItem(
vertexes=mesh, # defines the triangular mesh (Nx3x3)
vertexColors=self.meshColors, # defines mesh colors (Nx1)
drawEdges=False, # draw edges between mesh elements
smooth=False, # speeds up rendering
computeNormals=False) # speeds up rendering
self.window.addItem(self.body) # add body to plot
# add all three axis
axis_length = 220.0
naxis_pts = np.array([[0.0, 0.0, 0.0], [0.0, axis_length, 0.0]])
naxis = gl.GLLinePlotItem(pos=naxis_pts,
color=pg.glColor('r'),
width=3.0)
self.window.addItem(naxis)
eaxis_pts = np.array([[0.0, 0.0, 0.0], [axis_length, 0.0, 0.0]])
eaxis = gl.GLLinePlotItem(pos=eaxis_pts,
color=pg.glColor('g'),
width=3.0)
self.window.addItem(eaxis)
daxis_pts = np.array([[0.0, 0.0, 0.0], [0.0, 0.0, -axis_length]])
daxis = gl.GLLinePlotItem(pos=daxis_pts,
color=pg.glColor('b'),
width=3.0)
self.window.addItem(daxis)
self.plot_initialized = True
# else update drawing on all other calls to update()
else:
# reset mesh using rotated and translated points
self.body.setMeshData(vertexes=mesh, vertexColors=self.meshColors)
# update the center of the camera view to the spacecraft location
view_location = Vector(state.pe, state.pn,
state.h) # defined in ENU coordinates
self.window.opts['center'] = view_location
# redraw
self.app.processEvents()
def cameraPosition(self):
"""Return current position of camera based on center, dist, elevation, and azimuth"""
center = self.opts['center']
dist = self.opts['distance']
pos = center - self.opts['rotation'].rotatedVector( Vector(0,0,dist) )
return pos
def draw_mav(self, state):
"""
Draws the location of the mav at the specified location with the given roll pitch and yaw
:param state: The state that the mav should be drawn. [X, Y, Z, Roll, Pitch, Yaw]
:return: null
"""
#pull out the relevant information from the state vector
loc = np.array([state[0], -state[1], -state[2]])
phi = state[3]
theta = -state[4]
psi = -state[5]
if not self.init: #create body if not yet initialized
self.body = gl.GLMeshItem(vertexes=self.verts,
smooth=False,
drawEdges=False,
vertexColors=self.colors) #create meshes
self.w.addItem(self.body) #add meshes to plot
if self.init:
# rotate bods to specified state
#precalculate sines and cosines
cPhi = np.cos(phi)
sPhi = np.sin(phi)
cTheta = np.cos(theta)
sTheta = np.sin(theta)
cPsi = np.cos(psi)
sPsi = np.sin(psi)
rotMat = np.array([
[cTheta * cPsi, -cTheta * sPsi,
sTheta], #Make the rotation matrix
[
sPhi * sTheta * cPsi + cPhi * sPsi,
-sPhi * sTheta * sPsi + cPhi * cPsi, -sPhi * cTheta
],
[
-cPhi * sTheta * cPsi + sPhi * sPsi,
cPhi * sTheta * sPsi + sPhi * cPsi, cPhi * cTheta
]
])
# rotMat = np.array([[cPhi * cTheta, cPhi * sTheta * sPsi - sPhi * cPsi, cPhi * sTheta * cPsi + sPhi * sPsi],
# [sPhi * cTheta, sPhi * sTheta * sPsi + cPhi * cPsi, sPhi * sTheta * cPsi - cPhi * sPsi],
# [-sTheta, cTheta*sPsi, cTheta * cPsi]])
r_roll = np.array([[1, 0, 0], [0, cPhi, -sPhi], [0, sPhi, cPhi]])
r_pitch = np.array([[cTheta, 0, sTheta], [0, 1, 0],
[-sTheta, 0, cTheta]])
r_yaw = np.array([[cPsi, -sPsi, 0], [sPsi, cPsi, 0], [0, 0, 1]])
# rotMat = r_yaw * r_pitch * r_roll
# rotMat = r_roll * r_pitch * r_yaw
# newPoints = np.array([rotMat.dot(P.points[0]) + loc, #rotate and translate original points
# rotMat.dot(P.points[1]) + loc,
# rotMat.dot(P.points[2]) + loc,
# rotMat.dot(P.points[3]) + loc,
# rotMat.dot(P.points[4]) + loc,
# rotMat.dot(P.points[5]) + loc,
# rotMat.dot(P.points[6]) + loc,
# rotMat.dot(P.points[7]) + loc,
# rotMat.dot(P.points[8]) + loc,
# rotMat.dot(P.points[9]) + loc,
# rotMat.dot(P.points[10]) + loc,
# rotMat.dot(P.points[11]) + loc,
# rotMat.dot(P.points[12]) + loc,
# rotMat.dot(P.points[13]) + loc,
# rotMat.dot(P.points[14]) + loc,
# rotMat.dot(P.points[15]) + loc])
newPoints = np.array([
np.dot(r_yaw, np.dot(r_pitch, np.dot(r_roll, P.points[0]))) +
loc,
np.dot(r_yaw, np.dot(r_pitch, np.dot(r_roll, P.points[1]))) +
loc,
np.dot(r_yaw, np.dot(r_pitch, np.dot(r_roll, P.points[2]))) +
loc,
np.dot(r_yaw, np.dot(r_pitch, np.dot(r_roll, P.points[3]))) +
loc,
np.dot(r_yaw, np.dot(r_pitch, np.dot(r_roll, P.points[4]))) +
loc,
np.dot(r_yaw, np.dot(r_pitch, np.dot(r_roll, P.points[5]))) +
loc,
np.dot(r_yaw, np.dot(r_pitch, np.dot(r_roll, P.points[6]))) +
loc,
np.dot(r_yaw, np.dot(r_pitch, np.dot(r_roll, P.points[7]))) +
loc,
np.dot(r_yaw, np.dot(r_pitch, np.dot(r_roll, P.points[8]))) +
loc,
np.dot(r_yaw, np.dot(r_pitch, np.dot(r_roll, P.points[9]))) +
loc,
np.dot(r_yaw, np.dot(r_pitch, np.dot(r_roll, P.points[10]))) +
loc,
np.dot(r_yaw, np.dot(r_pitch, np.dot(r_roll, P.points[11]))) +
loc,
np.dot(r_yaw, np.dot(r_pitch, np.dot(r_roll, P.points[12]))) +
loc,
np.dot(r_yaw, np.dot(r_pitch, np.dot(r_roll, P.points[13]))) +
loc,
np.dot(r_yaw, np.dot(r_pitch, np.dot(r_roll, P.points[14]))) +
loc,
np.dot(r_yaw, np.dot(r_pitch, np.dot(r_roll, P.points[15]))) +
loc
])
#convert points to xyz coordinates
newVerts = np.array([
[newPoints[0], newPoints[1],
newPoints[2]], #Make the new verticies of the meshes
[newPoints[0], newPoints[1], newPoints[4]],
[newPoints[0], newPoints[3], newPoints[4]],
[newPoints[0], newPoints[3], newPoints[2]],
[newPoints[5], newPoints[2], newPoints[3]],
[newPoints[5], newPoints[1], newPoints[2]],
[newPoints[5], newPoints[1], newPoints[4]],
[newPoints[5], newPoints[3], newPoints[4]],
[newPoints[6], newPoints[7], newPoints[9]],
[newPoints[7], newPoints[8], newPoints[9]],
[newPoints[10], newPoints[11], newPoints[12]],
[newPoints[10], newPoints[12], newPoints[13]],
[newPoints[5], newPoints[14], newPoints[15]]
])
self.body.setMeshData(vertexes=newVerts, vertexColors=self.colors)
viewLoc = Vector(
loc[0], loc[1],
loc[2]) #vector to define the new camera view location
self.w.opts[
'center'] = viewLoc #update the camera view location to maintain UAV in the center of the view
if self.init is False: #Flip the init flag if it's false
self.init = True