Related
With cv2.solvePnP I try to do pose a estimation in pyvista, which is a python wrapper for vtk.
The results of solvePnP seem wrong to me, i.e. the resulting translation and rotation. For simplicity I try to "undo" a translation of the camera. I expect the inverse of the translation to be the result of solvePnP.
The translation is just
Translation = np.array([[ 1., 0., 0., 1000.],
[ 0., 1., 0., 0.],
[ 0., 0., 1., 0.],
[ 0., 0., 0., 1.]])
i.e. a shift along one axis. But the resulting rvec,tvec are
rvec = array([ 0., 0., -3.142]),
tvec = array([ 707.107, 408.248, 8882.736])
The resulting translation and rotation seem nonsensical to me. Since the translation does no rotation, I expect that only tvec has non zero entries to undo the translation in the opposite direction. Concretely, I expect tvec= [-1000,0,0] and rvec=[0,0,0]. If I then apply this (wrong) result to the camera, all points disappear completely.
Here is What I do:
import cv2
import pyvista as pv
from pyvista import examples
import pyvistaqt
from vtk import vtkMatrix4x4, vtkMatrix3x3, vtkTransform
from vtk.util.numpy_support import vtk_to_numpy
import numpy as np
np.set_printoptions(suppress=True,precision=3)
def getCamMatrix():
narray = np.eye(4)
vmatrix = plotter.camera.GetModelViewTransformMatrix()
vmatrix.DeepCopy(narray.ravel(), vmatrix)
return narray
def toVTK(m,n =4):
if n == 4:
newMatrixVTK = vtkMatrix4x4()
else:
newMatrixVTK = vtkMatrix3x3()
for i in range(n):
for j in range(n):
newMatrixVTK.SetElement(i,j, m[i,j])
return newMatrixVTK
def applyMatrixToCam(newMatrix):
global plotter
newMatrixVTK = toVTK(newMatrix)
transform = vtkTransform()
transform.SetMatrix(newMatrixVTK)
transform.Update()
plotter.camera.ApplyTransform(transform)
pass
print("Setting up points in world coordinates")
Points = np.array([[ 2918.972, -887.573, 416.331,1],
[ 2338.002, -702.07 , 1039.864,1],
[ 1458.473, -707.246, 1005.19,1 ],
[ 1219.4 , -890.161, 377.004,1],
[ 1318.727, -1017.829, -156.537,1],
[ 2529.132, -1026.888, -169.222,1]])
pMesh = pv.PolyData(Points[:,:3]) # vtk object to hold the six points
plotter = pyvistaqt.BackgroundPlotter() # setting up the plotting function
plotter.enable_trackball_style()
plotter.add_mesh(pMesh)
print("Transforming from World to Image Coordinates")
# Rotating the points towards a camera at the origin, i.e. applying the default camera transform
projected = (getCamMatrix() # Points.T)[:3,:].T
print("store original image points")
image_points = projected.copy()[:,:2]
print("Applying the perspective transform, i.e. division by the Z-coordinate")
image_points /= projected[:,-1].reshape(-1,1)
print("Setting up a simple translation of the camera position")
Translation = np.array([[ 1., 0., 0., 1000.],
[ 0., 1., 0., 0.],
[ 0., 0., 1., 0.],
[ 0., 0., 0., 1.]])
applyMatrixToCam(Translation)
print("Apply the new Camera Matrix to the six points")
projected_shift = (getCamMatrix() # Points.T)[:3,:].T
retval, rvec, tvec = cv2.solvePnP(projected_shift, np.array(image_points), np.eye(3), None, None, None, False, cv2.SOLVEPNP_EPNP)
R = cv2.Rodrigues(rvec)[0]
extrinsicReal = np.vstack([np.hstack([R.T, -R.T#tvec]), [0,0,0,1]])
applyMatrixToCam(extrinsicReal)
I am working on a tracking application where I use the opencv kalman filter to validate my current measurement of the position. I use the code from this question:
At first I calculate velocity (v) and accelearation (a) of my moving object at (x, y). These 4 values are used as my kalman state. I initiate the kalman filter as follows:
(np.eye(n,m) generates the identity matrix with dimensions nxm):
def initKalman(init_state, fps):
kalman = cv.KalmanFilter(4, 4, 2)
kalman.transitionMatrix = np.array([[1., 0., 1/fps, 0.],
[0., 1., 0., 1/fps],
[0., 0., 1., 0.],
[0, 0., 0., 1.]])
kalman.measurementMatrix = 1. * np.eye(2, 4)
kalman.measurementNoiseCov = 1e-3 * np.eye(2, 2)
kalman.processNoiseCov = 1e-5 * np.eye(4, 4)
kalman.errorCovPost = 1e-1 * np.eye(4, 4)
kalman.statePost = init_state.reshape(4, 1)
return kalman
kinematics = np.array((velocity, acceleration), dtype=np.float32)
kalman_state = np.concatenate((point, kinematics))
kalman_filter = initKalman(kalman_state, fps = 15)
During operation the correction is done as follows:
def correct_kalman(kalman, state):
measurement = (np.dot(kalman.measurementNoiseCov, np.random.randn(2, 1))).reshape(-1)
measurement = np.dot(kalman.measurementMatrix, state) + measurement
return kalman.correct(measurement)
kinematics = np.array((velocity, acceleration), dtype=np.float32)
kalman_state = np.concatenate((point, kinematics))
correct_kalman(kalman_filter, kalman_state)
It seems to work witch is great, but im trying to understand why. In my understanding it shouldn't work because in correct_kalman() the velocity and acceleration are ommited in this code line:
measurement = np.dot(kalman.measurementMatrix, state) + measurement
because the measurementmatrix is just 2 x 4. (In fact, if I set acceleration and speed to 0, the behavior of the filter does not change.)
For Example take the kalman_state = np.array([10., 20., 25., 75.]) and calculate the dot product with the measurementMatrix = 1. * np.eye(2, 4)
then measurement = np.dot(kalman.measurementMatrix, kalman_state) is just
>>> measurement
array([10., 20.])
v and a are gone.
So I changed my measurementMatrix and my measurementNoiseCov to 4 x 4 dimensionality and adjusted my correction acordingly by using np.random.randn(4, 1) but now the kalman filter is way to sluggish and falls behind the measurement.
Why is the first approach working if v and a are not used?
How can I change the measurement matrix in a more targeted way than just iteratively adjusting the values?
Thanks for the help!
I got this working code snippet:
import numpy as np
from matplotlib import pyplot as plt
in_raster = np.random.randn(36, 3, 2151)
matrix = np.reshape(in_raster, [(np.shape(in_raster)[0] * np.shape(in_raster)[1]), np.shape(in_raster)[2]])
# reshaping the matrix to prepare loop
out_raster = np.empty([np.shape(in_raster)[0]/3, np.shape(in_raster)[1]/3, np.shape(in_raster)[2]])
# creating empty output matrix
i = 0
j = 0
while i <= len(in_raster)-9 or j < len(out_raster):
if i % 9 == 0:
avg_in_raster = np.nanmean(matrix[i:i+9, :], axis=0)
out_raster[j] = avg_in_raster
i += 9
j += 1
out_raster = np.reshape(out_raster, [np.shape(out_raster)[0], np.shape(in_raster)[1]/3, np.shape(in_raster)[2]])
# plot example
low = 0
high = 50
for row in range(0, 3):
for col in range(np.shape(in_raster)[1]):
plt.plot(range(low,high), (in_raster[row, col, low:high]))
plt.plot(range(low,high), (out_raster[0,0,low:high]), 'k')
plt.show()
The program averages (aggregates) 3x3 slices of the input matrix (a raster image) and sets up a new one maintainig the dimensionality of the original matrix.
Now I got the feeling that there must be an easier way to achieve this.
Does somebody have an idea how to obtain the same result in a more pythonic way?
Thank you!
To my knowledge, there is no easier or quicker way to perform blockwise averaging. Your code might look big, but most of it is just preparation of arrays and resizing or plotting stuff. Your main function is a well-placed while-loop and the averaging itself you leave to numpy which is already a shortcut and should run quickly.
I don't see any reason to further shorten this, without losing readability.
If you just want to make it look shorter and "more pythonic" but less readable, go for this:
import numpy as np
from matplotlib import pyplot as plt
in_raster = np.random.randn(36, 3, 2151)
size=3
matrix=np.array([in_raster[:,:,i].flatten() for i in np.arange(in_raster.shape[2])]).transpose()
out_raster2 = np.array([np.nanmean(matrix[i:i+size**2, :], axis=0) for i in np.arange(len(matrix)) if not i%size**2]).reshape(np.shape(in_raster)[0]/size, np.shape(in_raster)[1]/size, np.shape(in_raster)[2])
# plot example
low = 0
high = 50
for row in range(0, 3):
for col in range(np.shape(in_raster)[1]):
plt.plot(range(low,high), (in_raster[row, col, low:high]))
plt.plot(range(low,high), (out_raster2[0,0,low:high]), 'k')
plt.show()
#plt.plot((out_raster2-out_raster)[0,0,low:high]) # should be all 0s
#plt.show()
And you could make it a function/method with the attribute size = 3 and quality checks (first and second dimension can be divided by size, etc.).
You should be able to do it by extending the shape in one direction and averaging it in that dimension. Like so:
out_raster1 = np.nanmean(in_raster.reshape(36*3//9, -1, 2151 ), axis=1).reshape(12, 1, -1)
To check for consistency,
>>> out_raster1-out_raster
array([[[ 0., 0., 0., ..., 0., 0., 0.]],
[[ 0., 0., 0., ..., 0., 0., 0.]],
[[ 0., 0., 0., ..., 0., 0., 0.]],
...,
[[ 0., 0., 0., ..., 0., 0., 0.]],
[[ 0., 0., 0., ..., 0., 0., 0.]],
[[ 0., 0., 0., ..., 0., 0., 0.]]])
I have an array of temperature, and an array of 3D points such that temperature[n] is the temperatureat point[n]. How can I make a contour plot of that data (looking at 2D planes)?
I thought on creating a function extract_plane in which a parametric equation of a plane would be passed as an argument, then the points in it and the corresponding temperatures returned (that is what I need help with).
As an example:
import numpy as np
import matplotlib.pyplot as plt
points = np.array([[0., 0., 0.],
[1., 0., 0.],
[1., 1., 0.],
[0., 1., 0.],
[0., 1., 1.],
[0., 0., 1.],
[1., 0., 1.],
[1., 1., 1.]])
temperature = np.array([0, 0, 0, 0, 1, 1, 1, 1.])
I need help creating the following function. As it is, it only extracts the points laying in the plane z=0.
def extract_plane(points, temperature, equation):
"""
Given a set of 3D points, and their corresponding temperatures,
extracts a set of points that are in the plane defined by equation
along their temperatures.
Parameters
----------
points : ndarray (3D)
The set of points.
temperature : ndarray (1D)
The temperatures at the points such that temperature[n] is
the temperature in points[n].
equation : ????
The equation that defines a 2D plane (cross-section) where
the temperature is wanted to be plotted.
Returns
-------
coord : ndarray (1D)
The set of points that are in the plane defined by equation.
temp : ndarray (1D)
The set of temperatures in which temp[n] coresponds to coord[n].
"""
temp = []
coord = []
# plane z=0
for n in range(points.shape[0]):
if (points[n,2] == 0.):
temp += [temperature[n]]
coord += [points[n]]
temp = np.array(temp)
coord = np.array(coord)
return coord, temp
And use griddata found in this cookbook to reshape temp so it can be plotted:
# griddata.py - 2010-07-11 ccampo
def griddata(x, y, z, binsize=0.01, retbin=True, retloc=True):
"""
Place unevenly spaced 2D data on a grid by 2D binning (nearest
neighbor interpolation).
Parameters
----------
x : ndarray (1D)
The idependent data x-axis of the grid.
y : ndarray (1D)
The idependent data y-axis of the grid.
z : ndarray (1D)
The dependent data in the form z = f(x,y).
binsize : scalar, optional
The full width and height of each bin on the grid. If each
bin is a cube, then this is the x and y dimension. This is
the step in both directions, x and y. Defaults to 0.01.
retbin : boolean, optional
Function returns `bins` variable (see below for description)
if set to True. Defaults to True.
retloc : boolean, optional
Function returns `wherebins` variable (see below for description)
if set to True. Defaults to True.
Returns
-------
grid : ndarray (2D)
The evenly gridded data. The value of each cell is the median
value of the contents of the bin.
bins : ndarray (2D)
A grid the same shape as `grid`, except the value of each cell
is the number of points in that bin. Returns only if
`retbin` is set to True.
wherebin : list (2D)
A 2D list the same shape as `grid` and `bins` where each cell
contains the indicies of `z` which contain the values stored
in the particular bin.
Revisions
---------
2010-07-11 ccampo Initial version
"""
# get extrema values.
xmin, xmax = x.min(), x.max()
ymin, ymax = y.min(), y.max()
# make coordinate arrays.
xi = np.arange(xmin, xmax+binsize, binsize)
yi = np.arange(ymin, ymax+binsize, binsize)
xi, yi = np.meshgrid(xi,yi)
# make the grid.
grid = np.zeros(xi.shape, dtype=x.dtype)
nrow, ncol = grid.shape
if retbin: bins = np.copy(grid)
# create list in same shape as grid to store indices
if retloc:
wherebin = np.copy(grid)
wherebin = wherebin.tolist()
# fill in the grid.
for row in range(nrow):
for col in range(ncol):
xc = xi[row, col] # x coordinate.
yc = yi[row, col] # y coordinate.
# find the position that xc and yc correspond to.
posx = np.abs(x - xc)
posy = np.abs(y - yc)
ibin = np.logical_and(posx < binsize/2., posy < binsize/2.)
ind = np.where(ibin == True)[0]
# fill the bin.
bin = z[ibin]
if retloc: wherebin[row][col] = ind
if retbin: bins[row, col] = bin.size
if bin.size != 0:
binval = np.median(bin)
grid[row, col] = binval
else:
grid[row, col] = np.nan # fill empty bins with nans.
# return the grid
if retbin:
if retloc:
return grid, bins, wherebin
else:
return grid, bins
else:
if retloc:
return grid, wherebin
else:
return grid
Then plot:
coord, temp = extract_plane(points, temperature, None)
x = coord[:,0]
y = coord[:,1]
g = griddata(x, y, temp, 1., False, False)
plt.contourf(g)
The functions from the question look a lot more complicated than it needs to be. Using scipy.interpolate.griddata allows to interpolate values on a grid.
import numpy as np
from scipy.interpolate import griddata
import matplotlib.pyplot as plt
points = np.array([[0., 0., 0.],
[1., 0., 0.],
[1., 1., 0.],
[0., 1., 0.],
[0., 1., 1.],
[0., 0., 1.],
[1., 0., 1.],
[1., 1., 1.]])
temperature = np.array([0, 0, 0, 0, 1, 1, 1, 1.])
grid_y, grid_x = np.mgrid[0:1:25j, 0:1:25j]
# equation along which to interpolate
equation = lambda x,y : 0.8*(1-x)
grid_z = equation(grid_x, grid_y)
interp = griddata(points, temperature, (grid_x, grid_y, grid_z), method='linear')
plt.subplot(121)
#plt.contourf(grid_x,grid_y, interp, origin='lower',vmin=0,vmax=1)
plt.imshow(interp, origin='lower',vmin=0,vmax=1)
plt.title('temperature along 0.8*(1-x)')
plt.xlabel("x")
plt.ylabel("y")
from mpl_toolkits.mplot3d import Axes3D
ax = plt.subplot(122, projection=Axes3D.name)
ax.scatter(points[:,0], points[:,1], points[:,2], c=temperature)
ax.set_zlim(-.1,1.1)
ax.plot_surface(grid_x,grid_y,grid_z, facecolors=plt.cm.viridis(interp),
linewidth=0, antialiased=False, shade=False)
ax.set_xlabel("x")
ax.set_ylabel("y")
plt.show()
For equation = lambda x,y : x*(y**.5):
Of course using contourf is equally possible, plt.contourf(grid_x,grid_y, interp, origin='lower',vmin=0,vmax=1):
For my astronomy homework, I need to simulate the elliptical orbit of a planet around a sun. To do this, I need to use a for loop to repeatedly calculate the motion of the planet. However, every time I try to run the program, I get the following error:
RuntimeWarning: invalid value encountered in power
r=(x**2+y**2)**1.5
Traceback (most recent call last):
File "planetenstelsel3-4.py", line 25, in <module>
ax[i] = a(x[i],y[i])*x[i]
ValueError: cannot convert float NaN to integer
I've done some testing, and I think the problem lies in the fact that the values that are calculated are greater than what fits in an integer, and the arrays are defined as int arrays. So if there was a way do define them as float arrays, maybe it would work. Here is my code:
import numpy as np
import matplotlib.pyplot as plt
dt = 3600 #s
N = 5000
x = np.tile(0, N)
y = np.tile(0, N)
x[0] = 1.496e11 #m
y[0] = 0.0
vx = np.tile(0, N)
vy = np.tile(0, N)
vx[0] = 0.0
vy[0] = 28000 #m/s
ax = np.tile(0, N)
ay = np.tile(0, N)
m1 = 1.988e30 #kg
G = 6.67e-11 #Nm^2kg^-2
def a(x,y):
r=(x**2+y**2)**1.5
return (-G*m1)/r
for i in range (0,N):
r = x[i],y[i]
ax[i] = a(x[i],y[i])*x[i]
ay[i] = a(x[i],y[i])*y[i]
vx[i+1] = vx[i] + ax[i]*dt
vy[i+1] = vy[i] + ay[i]*dt
x[i+1] = x[i] + vx[i]*dt
y[i+1] = y[i] + vy[i]*dt
plt.plot(x,y)
plt.show()
The first few lines are just some starting parameters.
Thanks for the help in advance!
When you are doing physics simulations you should definitely use floats for everything. 0 is an integer constant in Python, and thus np.tile creates integer arrays; use 0.0 as the argument to np.tile to do floating point arrays; or preferably use the np.zeros(N) instead:
You can check the datatype of any array variable from its dtype member:
>>> np.tile(0, 10).dtype
dtype('int64')
>>> np.tile(0.0, 10).dtype
dtype('float64')
>>> np.zeros(10)
array([ 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.])
>>> np.zeros(10).dtype
dtype('float64')
To get a zeroed array of float32 you'd need to give a float32 as the argument:
>>> np.tile(np.float32(0), 10)
array([0., 0., 0., 0., 0., 0., 0., 0., 0., 0.], dtype=float32)
or, preferably, use zeros with a defined dtype:
>>> np.zeros(10, dtype='float32')
array([0., 0., 0., 0., 0., 0., 0., 0., 0., 0.], dtype=float32)
You need x = np.zeros(N), etc.: this declares the arrays as float arrays.
This is the standard way of putting zeros in an array (np.tile() is convenient for creating a tiling with a fixed array).