Speed up nd-array computations in Numpy - python

I want to implement a custom undistortion function like in OpenCV using numpy module on Python.
From documentation is known that undistort function is just a combination of of initUndistortRectifyMap() and remap().
Since remap() is quite simple operation, the main issue is to implement maps for remap().
I wrote a code to construct maps, but it seems to me that it works quite slowly.
The code consists of three main parts:
Reshape original image points to a well-shaped array to multiply it on the inverse of the camera matrix and perform a multiplication.
Distort points in the z = 1 plane.
Reshape points again to perform another multiplication to get back to image points.
I took an image with size of (4032 x 3024).
One matrix multiplication works on my pc for about 1 sec. And the distortion function works for about 2.4 sec.
I tried to multiply same shaped matrices with OpenCV Mats on C++, and I took 0.0002 sec.
The question is how to speed up the computations, because it seems to me that I am doing something wrong, because of such a big difference.
I found here an advice to make all arrays contiguous, but this did not help
The code:
import numpy
import time
def _distort_z_1(x, y, k1, k2, k3, k4, k5, k6, p1, p2):
x2 = x * x
y2 = y * y
xy = x * y
r2 = x2 + y2
r4 = r2 * r2
r6 = r4 * r2
radial = \
(1 + k1 * r2 + k2 * r4 + k3 * r6) / \
(1 + k4 * r2 + k5 * r4 + k6 * r6)
tangential_x = 2 * p1 * xy + p2 * (r2 + 2 * x2)
tangential_y = p1 * (r2 + 2 * y2) + 2 * p2 * xy
x_distorted = x * radial + tangential_x
y_distorted = y * radial + tangential_y
return x_distorted, y_distorted
# Change dimension from [2 x H x W] to [H x W x 3 x 1] to correctly multiply with [3 x 3] matrix
def _homogeneous_reshape(points_x, points_y):
points_homogeneous_reshaped = (
# Add extra axis to change from [H x W x 3] to [H x W x 3 x 1]
numpy.expand_dims(
# Change from [3 x H x W] to [H x W x 3]
numpy.transpose(
# Change from [2 x H x W] to [3 x H x W] (homogeneous coordinates)
numpy.stack(
numpy.broadcast_arrays(points_x, points_y, 1)),
(1, 2, 0)),
-1))
return points_homogeneous_reshaped
def _homogeneous_reshape_back(points_homogeneous_reshaped):
points_homogeneous = (
# Get back from [H x W x 3] to [3 x H x W]
numpy.transpose(
# Remove extra axis: [H x W x 3 x 1] to [H x W x 3]
numpy.squeeze(
points_homogeneous_reshaped),
(2, 0, 1)))
# Get back from homogeneous coordinates
points_x, points_y, _ = points_homogeneous
return points_x, points_y
def _get_undistort_rectify_maps(distortion_coefficients, camera_matrix, image_width, image_height):
image_points = numpy.meshgrid(range(image_width), range(image_height))
# print("BEGIN: _homogeneous_reshape")
start = time.time()
image_points_homogeneous_reshaped = _homogeneous_reshape(*image_points)
end = time.time()
print("END: _homogeneous_reshape", end - start)
camera_matrix_inv = numpy.linalg.inv(camera_matrix)
# print("BEGIN: camera_matrix_inv # image_points_homogeneous_reshaped")
start = time.time()
image_points_homogeneous_z_1_reshaped = camera_matrix_inv # image_points_homogeneous_reshaped
end = time.time()
print("END: camera_matrix_inv # image_points_homogeneous_reshaped", end - start)
# print("BEGIN: _homogeneous_reshape_back")
start = time.time()
image_points_z_1 = _homogeneous_reshape_back(image_points_homogeneous_z_1_reshaped)
end = time.time()
print("END: _homogeneous_reshape_back", end - start)
# print("BEGIN: _distort_z_1")
start = time.time()
x_distorted, y_distorted = _distort_z_1(
*image_points_z_1,
**distortion_coefficients)
end = time.time()
print("END: _distort_z_1", end - start)
# print("BEGIN: _homogeneous_reshape")
start = time.time()
points_homogeneous_z_1_distorted_reshaped = _homogeneous_reshape(x_distorted, y_distorted)
end = time.time()
print("END: _homogeneous_reshape", end - start)
# print("BEGIN: _homogeneous_reshape")
start = time.time()
points_homogeneous_distorted_reshaped = camera_matrix # points_homogeneous_z_1_distorted_reshaped
end = time.time()
print("END: camera_matrix # points_homogeneous_z_1_distorted_reshaped", end - start)
# print("BEGIN: _homogeneous_reshape_back")
start = time.time()
points_homogeneous_distorted = _homogeneous_reshape_back(points_homogeneous_distorted_reshaped)
end = time.time()
print("END: _homogeneous_reshape_back", end - start)
return (map.astype(numpy.float32) for map in points_homogeneous_distorted)
if __name__ == "__main__":
image_width = 4032
image_height = 3024
distortion_coefficients = {
"k1": 0, "k2": 0, "k3": 0, "k4": 0, "k5": 0, "k6": 0,
"p1": 0, "p2": 0}
camera_matrix = numpy.array([
[1000, 0, 2016],
[0, 1000, 1512],
[0, 0, 1]])
map_x, map_y = _get_undistort_rectify_maps(
distortion_coefficients,
camera_matrix,
image_width,
image_height)

Related

Python implementation of Francis double step QR iteration algorithm does not converge

I am implementing the Francis double step QR Iteration algorithm using the notes and psuedocode from lecture https://people.inf.ethz.ch/arbenz/ewp/Lnotes/chapter4.pdf - Algorithm 4.5
The psuedocode is provided in Matlab I believe.
Below is the implementation of my code.
# compute upper hessenberg form of matrix
def hessenberg(A):
m,n = A.shape
H = A.astype(np.float64)
for k in range(n-2):
x = H[k+1:, k]
v = np.concatenate([np.array([np.sign(x[0]) * np.linalg.norm(x)]), x[1:]])
v = v / np.linalg.norm(v)
H[k+1:, k:] -= 2 * np.outer(v, np.dot(v, H[k+1:, k:]))
H[:, k+1:] -= 2 * np.outer(np.dot(H[:, k+1:], v), v)
return(H)
# compute first three elements of M
def first_three_M(T,s,t):
x = T[0, 0]**2 + T[0, 1] * T[1, 0] - s * T[0, 0] + t
y = T[1, 0] * (T[0, 0] + T[1, 1] - s)
z = T[1, 0] * T[2, 1]
return(x,y,z)
# householder reflection
def householder_reflection_step(x_1):
v = x_1[0] + np.sign(x_1[0]) * np.linalg.norm(x_1)
v = v / np.linalg.norm(v)
P = np.eye(3) - 2 * np.outer(v, v)
return(P)
# update elements of M
def update_M(T,k,p):
x = T[k+1, k]
y = T[k+2, k]
if k < p - 3:
z = T[k+3, k]
else:
z = 0
return(x,y,z)
# givens rotation
def givens_step(T,x_2,x,y,p,q,n):
# calculate c and s
c = x / np.sqrt(x**2 + y**2)
s = -y / np.sqrt(x**2 + y**2)
P = np.array([[c, s], [-s, c]])
T[q-1:p, p-3:n] = P.T # T[q-1:p, p-3:n]
T[0:p, p-2:p] = T[0:p, p-2:p] # P
return(T)
# deflation step
def deflation_step(T,p,q,epsilon):
if abs(T[p-1, p-2]) < epsilon * (abs(T[p-2, p-2]) + abs(T[p-1, p-1])):
T[p-1, p-2] = 0
p = p - 1
q = p - 1
elif abs(T[p-2, p-3]) < epsilon * (abs(T[p-3, p-3]) + abs(T[p-2, p-2])):
T[p-2, p-3] = 0
p = p - 2
q = p - 1
return(T,p,q)
# francis qr step
def francis_step(H, epsilon=0.90):
n = H.shape[0]
T = H.copy().astype(np.float64)
p = n - 1
while p > 2:
q = p - 1
s = T[q, q] + T[p, p]
t = T[q, q] * T[p, p] - T[q, p] * T[p, q]
# Compute M
x,y,z = first_three_M(T,s,t)
x_1 = np.transpose([[x], [y], [z]])
# Bulge chasing
for k in range(p - 3):
# Compute Householder reflector
P = householder_reflection_step(x_1)
r = max(1, k-1)
T[k:k+3, r:] = P.T # T[k:k+3, r:]
r = min(k + 3, p)
T[0:r, k:k+3] = T[0:r, k:k+3] # P
# Update M
x,y,z = update_M(T,k,p)
x_2 = np.transpose([[x], [y]])
# Compute Givens rotation
T = givens_step(T,x_2,x,y,p,q,n)
# Check for convergence
T,p,q = deflation_step(T,p,q,epsilon)
return(T)
# francis qr iteration
def francis_qr_iteration(A):
m,n = A.shape
H = hessenberg(A)
eigvals = []
iters = 0
max_iters = 100
while iters<max_iters:
# Perform Francis step
T = francis_step(H)
eigvals.append(np.diag(T))
iters+=1
return(eigvals)
# for quick testing
A = np.array([[2, 2, 3, 4, 2],
[1, 2, 4, 2, 3],
[4, 1, 2, 1, 5],
[5, 2, 5, 2, 1],
[3, 6, 3, 1, 4]])
eigenvals = francis_qr_iteration(A)
#comparing our method to scipy - final eigvals obtained
print(len(eigenvals))
print(sorted(eigenvals[-1]))
print(sorted(scipy.linalg.eig(A)[0].real))
And this is the output I am getting.
100
[-4.421235127393854, -0.909209110641351, -0.8342390091346807, 3.7552499102751575, 8.215454029003958]
[-3.0411228516834217, -1.143605409373778, -1.143605409373778, 3.325396565009845, 14.002937105421134]
The matrix T is not changing and hence it does not converge to the Schur form through which I can obtain the eigenvalues by using np.diag(T). I believe the error is coming either from the Givens rotation step or the Householder reflection step. It could be an indexing issue since I tried to work in python using matlab psuedocode. Please let me know where I am going wrong so I can improve the code and make it converge.

The output value of cv2.projectPoints differs from my implementation from scratch

I am trying to understand how the cv2.projectPoints. For this I have created the following test:
import numpy as np
import cv2
##################################################################
# Camera parameters
I = np.array([[0.11867264, 0, 0.5399652], [0, 0.37119691, 0.76215127], [0, 0, 1]])
E = np.array([[0.85939021, 0.78837968, 0.04341585, 0.99755739],[0.84512984, 0.19973536, 0.09509114, 0.47567923], [0.00813835, 0.00662538, 0.16825557, 0.00391849]])
D = np.array([0.0809947, 0.1508598, 0.69108758, 0.2972208, 0.96983757])
point_3d = np.array([0.2249427, 0.13465326, 0.02870871])
##################################################################
# METHOD 1
# project onto image place using opencv, we assume 0 distortion
[u, v] = cv2.projectPoints(point_3d, E[:, :3], E[:, 3], I, D)[0].squeeze()
print('Image coordinates are {}, {} using opencv'.format(u, v))
# METHOD 2
# project onto image place using extrinsics and intrinsics independently
# Convert the 3D point to homogeneous coordinates
point_h = np.array([point_3d[0], point_3d[1], point_3d[2], 1]).reshape(4, 1)
# Transform the 3D point to camera coordinates
cam_coords= np.dot(E, point_h)
# Normalize the camera coordinates
x, y, z = cam_coords/cam_coords[2]
# radial dist
r2 = x**2 + y**2
r4 = r2**2
r6 = r2**3
rdist = 1 + D[0] * r2 + D[1] * r4 + D[4]*r6
x_dist = x * rdist
y_dist = y * rdist
# tan dist
tanx = 2*D[2] * x * y + D[3]*(r2 + 2 * x**2)
tany = D[2]*(r2 + 2 * y**2) + 2*D[3] * x * y
x_dist = x_dist + tanx
y_dist = y_dist + tany
# # Back to absolute coordinates
x_dist = I[0][0] * x_dist + I[0][2]
y_dist = I[1][1] * y_dist + I[1][2]
print('Image coordinates are {}, {} using E and I independently'.format(x_dist, y_dist))
diff = np.abs((u - x_dist) + (v - y_dist))
print('Comparison against opencv implementation {}'.format(diff))
# METHOD 3
# project onto image place using the P matrix
#
point_h = np.array([point_3d[0], point_3d[1], point_3d[2], 1]).reshape(4, 1)
P = I # E
coord = P # point_h
x = coord[0] / coord[2]
y = coord[1] / coord[2]
# # To relative coordinates
x = (x - I[0, 2])/I[0, 0]
y = (y - I[1, 2])/I[1, 1]
# radial dist
r2 = x**2 + y**2
r4 = r2**2
r6 = r2**3
rdist = 1 + D[0] * r2 + D[1] * r4 + D[4]*r6
x_dist = x * rdist
y_dist = y * rdist
# tan dist
tanx = 2*D[2] * x * y + D[3]*(r2 + 2 * x**2)
tany = D[2]*(r2 + 2 * y**2) + 2*D[3] * x * y
x_dist = x_dist + tanx
y_dist = y_dist + tany
# # Back to absolute coordinates
x_dist = x_dist * I[0, 0] + I[0, 2]
y_dist = y_dist * I[1, 1] + I[1, 2]
print('Image coordinates are {}, {} using P'.format(x_dist,y_dist))
diff = np.abs((u - x_dist) + (v - y_dist))
print('Comparison against opencv implementation {}'.format(diff))
The output of this piece of code is the following one:
Image coordinates are 58328092212356.85, 97724995854418.78 using opencv
Image coordinates are [5.83280922e+13], [9.77249959e+13] using E and I independently
Comparison against opencv implementation [0.015625]
Image coordinates are [5.83280922e+11], [9.77249959e+11] using P
Comparison against opencv implementation [0.265625]
It seems like when I compute the Projection matrix using the intrinsic and the extrisinc matrices, I do not get the same solution as OpenCV. The distortion model applied is the one described in the documentation.

How to fix error when inversing image maps in OpenCV

I am trying to inverse a set of maps using this answer here. I used two of his methods so there is more detail as to how they work in his answer. I also left some comments out to shorten the code.
I have my own camera matrix and distortion coeff's that I use to create an x and y map with cv2.initUndistortRectifyMap(), but when I pass them to invert_maps() I get an out-of-bounds error shown below.
None of this (except the bottom part) is my code and its pretty advanced stuff so I have no clue how to debug it. And I dont have enough credit to comment on the orignal answer. Anyone got a solution?
import numpy as np
import cv2 as cv2
from scipy import ndimage as ndi
from matplotlib import pyplot as plt
import glob
def bilinear_inverse(p, vertices, numiter=4):
p = np.asarray(p)
v = np.asarray(vertices)
sh = p.shape[1:]
if v.ndim == 2:
v = np.expand_dims(v, axis=tuple(range(2, 2 + len(sh))))
# Start in the center
s = .5 * np.ones((2,) + sh)
s0, s1 = s
for k in range(numiter):
# Residual
r = v[0] * (1 - s0) * (1 - s1) + v[1] * s0 * (1 - s1) + v[2] * s0 * s1 + v[3] * (1 - s0) * s1 - p
# Jacobian
J11 = -v[0, 0] * (1 - s1) + v[1, 0] * (1 - s1) + v[2, 0] * s1 - v[3, 0] * s1
J21 = -v[0, 1] * (1 - s1) + v[1, 1] * (1 - s1) + v[2, 1] * s1 - v[3, 1] * s1
J12 = -v[0, 0] * (1 - s0) - v[1, 0] * s0 + v[2, 0] * s0 + v[3, 0] * (1 - s0)
J22 = -v[0, 1] * (1 - s0) - v[1, 1] * s0 + v[2, 1] * s0 + v[3, 1] * (1 - s0)
inv_detJ = 1. / (J11 * J22 - J12 * J21)
s0 -= inv_detJ * (J22 * r[0] - J12 * r[1])
s1 -= inv_detJ * (-J21 * r[0] + J11 * r[1])
return s
def invert_map(xmap, ymap, diagnostics=False):
"""
Generate the inverse of deformation map defined by (xmap, ymap) using inverse bilinear interpolation.
"""
# Generate quadrilaterals from mapped grid points.
quads = np.array([[ymap[:-1, :-1], xmap[:-1, :-1]],
[ymap[1:, :-1], xmap[1:, :-1]],
[ymap[1:, 1:], xmap[1:, 1:]],
[ymap[:-1, 1:], xmap[:-1, 1:]]])
# Range of indices possibly within each quadrilateral
x0 = np.floor(quads[:, 1, ...].min(axis=0)).astype(int)
x1 = np.ceil(quads[:, 1, ...].max(axis=0)).astype(int)
y0 = np.floor(quads[:, 0, ...].min(axis=0)).astype(int)
y1 = np.ceil(quads[:, 0, ...].max(axis=0)).astype(int)
# Quad indices
i0, j0 = np.indices(x0.shape)
# Offset of destination map
x0_offset = x0.min()
y0_offset = y0.min()
# Index range in x and y (per quad)
xN = x1 - x0 + 1
yN = y1 - y0 + 1
# Shape of destination array
sh_dest = (1 + x1.max() - x0_offset, 1 + y1.max() - y0_offset)
# Coordinates of destination array
yy_dest, xx_dest = np.indices(sh_dest)
xmap1 = np.zeros(sh_dest)
ymap1 = np.zeros(sh_dest)
TN = np.zeros(sh_dest, dtype=int)
# Smallish number to avoid missing point lying on edges
epsilon = .01
# Loop through indices possibly within quads
for ix in range(xN.max()):
for iy in range(yN.max()):
# Work only with quads whose bounding box contain indices
valid = (xN > ix) * (yN > iy)
# Local points to check
p = np.array([y0[valid] + ix, x0[valid] + iy])
# Map the position of the point in the quad
s = bilinear_inverse(p, quads[:, :, valid])
# s out of unit square means p out of quad
# Keep some epsilon around to avoid missing edges
in_quad = np.all((s > -epsilon) * (s < (1 + epsilon)), axis=0)
# Add found indices
ii = p[0, in_quad] - y0_offset
jj = p[1, in_quad] - x0_offset
ymap1[ii, jj] += i0[valid][in_quad] + s[0][in_quad]
xmap1[ii, jj] += j0[valid][in_quad] + s[1][in_quad]
# Increment count
TN[ii, jj] += 1
ymap1 /= TN + (TN == 0)
xmap1 /= TN + (TN == 0)
if diagnostics:
diag = {'x_offset': x0_offset,
'y_offset': y0_offset,
'mask': TN > 0}
return xmap1, ymap1, diag
else:
return xmap1, ymap1
# cam matrix and dist coeff's that I brought
cam_matrix = np.array([ [1223.07784, 0, 926.80065],
[ 0, 1231.71291, 546.10496],
[ 0, 0, 1]], dtype='float32')
distortion_profile = np.array([-0.32077, 0.15041, 0.001004, 0.00028, -0.04252], dtype='float32')
# get current maps
mapx, mapy = cv2.initUndistortRectifyMap(cam_matrix, distortion, None, cam_matrix, (1920, 1080), 5)
# invert the maps
mapx_invert, mapy_invert = invert_map(mapx, mapy)
# apply mapping to image
inversed = cv2.remap(img, mapx_invert, mapy_invert ,cv2.INTER_LINEAR)
cv2.imwrite('inversed.png', inversed)
Error:
File "c:\Users\...\redist_image2.py", line 121, in invert_map
ymap1[ii, jj] += i0[valid][in_quad] + s[0][in_quad]
IndexError: index 1382 is out of bounds for axis 1 with size 1020

Implementing the Adams Bashforth Moulton Method in Python

I am currently working on implementing the Adams Bashforth Moulton Method for solving a pendulum problem.
My current code is as follows:
import numpy as np
#####################################################################################
# DEF func
#####################################################################################
def func(y,t):
n=y.size
f=np.zeros(6)
xp=np.array([y[3],y[4],y[5]])[np.newaxis]
mass=1.
F1=0.
F2=0.
F3=-mass*9.8
F=np.array([F1,F2,F3])[np.newaxis]
phix=2.*y[0]
phiy=2.*y[1]
phiz=2.*y[2]
G=np.array([phix,phiy,phiz])[np.newaxis]
H=2.*np.eye(3)
lambd=(mass*np.dot(xp,np.dot(H,xp.T))+np.dot(F,G.T))/np.dot(G,G.T)
f[0]=y[3]
f[1]=y[4]
f[2]=y[5]
for k in range(0,3):
f[k+3]=(F[0,k]-lambd*G[0,k])/mass
return f
def dF_matrix(y):
n=y.size
dF=np.zeros((6,6))
xp=np.array([y[3],y[4],y[5]])[np.newaxis]
mass=1.
F1=0.
F2=0.
F3=-mass*9.8
F=np.array([F1,F2,F3])[np.newaxis]
phix=2.*y[0]
phiy=2.*y[1]
phiz=2.*y[2]
G=np.array([phix,phiy,phiz])[np.newaxis]
H=2.*np.eye(3)
lambd=(mass*np.dot(xp,np.dot(H,xp.T))+np.dot(F,G.T))/np.dot(G,G.T)
dF[0,3]=1
dF[1,4]=1
dF[2,5]=1
dF[3,0]=(y[0]*F1+2*lambd)/mass
dF[3,1]=(y[0]*F2)/mass
dF[3,2]=(y[0]*F3)/mass
dF[3,3]=phix*y[3]
dF[3,4]=phix*y[4]
dF[3,5]=phix*y[5]
dF[4,0]=(y[1]*F1)/mass
dF[4,1]=(y[1]*F2+2*lambd)/mass
dF[4,2]=(y[1]*F3)/mass
dF[4,3]=phiy*y[3]
dF[4,4]=phiy*y[4]
dF[4,5]=phiy*y[5]
dF[5,0]=(y[2]*F1)/mass
dF[5,1]=(y[2]*F2)/mass
dF[5,2]=(y[2]*F3+2*lambd)/mass
dF[5,3]=phiz*y[3]
dF[5,4]=phiz*y[4]
dF[5,5]=phiz*y[5]
return dF
#####################################################################################
# PYTHON script:
# Solve ODE for pendulum problem
#####################################################################################
from scipy.integrate import odeint
from mpl_toolkits import mplot3d
import matplotlib.pyplot as plt
import time
### Forward Euler Method
def forward_Euler(function, y_matrix, time):
y = np.zeros((np.size(time), np.size(y_matrix)))
y[0, :] = y_matrix
for i in range(len(time) - 1):
dt = time[i + 1] - time[i]
y[i + 1, :] = y[i, :] + np.asarray(function(y[i, :], time[i])) * dt
return y
### Modified Euler Method
def modified_Euler(function, y_matrix, time):
y = np.zeros((np.size(time), np.size(y_matrix)))
y[0, :] = y_matrix
for i in range(len(time) - 1):
dt = time[i + 1] - time[i]
k1 = np.asarray(function(y[i, :], time[i]))
k2 = np.asarray(function(y[i, : + k1], time[i]))
y[i + 1, :] = y[i, :] + (k1 + k2) / 2
return y
### Adams-Bashforth 2nd order
def Adams_Bash_2nd(function, y_matrix, time):
y = np.zeros((np.size(time), np.size(y_matrix)))
y[0, :] = y_matrix
for i in range(len(time) - 1):
dt = time[i + 1] - time[i]
f_1 = function(y[i, :], time[i])
f_2 = function(f_1, time[i])
y[i + 1, :] = y[i, :] + dt * f_1 + ((dt**2)/2) * f_2
return y
### Adams Bashforth Moulton Method
def Adams_Moulton(function, y_matrix, time):
y = np.zeros((np.size(time), np.size(y_matrix)))
y[0, :] = y_matrix
### predictor formula
for i in range(len(time) - 1):
dt = time[i + 1] - time[i]
f_1 = function(y[i, :], time[i])
f_2 = function(f_1, time[i])
y[i + 1, :] = y[i, :] + dt * f_1 + ((dt**2)/2) * f_2
### Corrector formula
for i in range(len(time-1)):
dt = time[i + 1] - time[i]
k_1 = 9 * (function(y[i, :], time[i+1]))
k_2 = 19 * (function(y[i, :], time[i]))
k_3 = 5 * (function(y[i, :], time[i-1]))
k_4 = (function(y[i, :], time[i-2]))
# === ERROR HAPPENS HERE ===
y[i + 1, :] = y + (dt/24) * (k_1 + k_2 - k_3 + k_4)
return y
# initial condition
y0=np.array([0.0,1.0,0.0,0.8,0.0,1.2])
print('ODE Python ..')
time_start = time.clock()
nt = 500
# time points
t = np.linspace(0,25,nt)
# solve ODE
y1 = odeint(func,y0,t)
time_elapsed = (time.clock() - time_start)
print('elapsed time',time_elapsed)
# compute residual:
r=y1[:,0]**2+y1[:,1]**2+y1[:,2]**2-1
rmax1=np.max(np.abs(r))
print('error',rmax1)
fig = plt.figure()
ax = plt.axes(projection='3d')
ax.plot3D(y1[:,0],y1[:,1],y1[:,2], 'gray')
plt.show()
# print ODE Euler Method
time_start = time.clock()
nt = 500
# time points
t = np.linspace(0, 25, nt)
# solve ODE
y1 = forward_Euler(func, y0, t)
print(y1)
time_elapsed = (time.clock() - time_start)
print('elapsed time', time_elapsed)
# compute residual:
r = y1[:, 0] ** 2 + y1[:, 1] ** 2 + y1[:, 2] ** 2 - 1
rmax1 = np.max(np.abs(r))
print('error', rmax1)
fig = plt.figure()
ax = plt.axes(projection='3d')
ax.plot3D(y1[:, 0], y1[:, 1], y1[:, 2], 'gray')
plt.show()
### Modified Euler method
time_start = time.clock()
nt = 500
# time points
t = np.linspace(0, 25, nt)
# solve ODE
y1 = forward_Euler(func, y0, t)
print(y1)
time_elapsed = (time.clock() - time_start)
print('elapsed time', time_elapsed)
# compute residual:
r = y1[:, 0] ** 2 + y1[:, 1] ** 2 + y1[:, 2] ** 2 - 1
rmax1 = np.max(np.abs(r))
print('error', rmax1)
fig = plt.figure()
ax = plt.axes(projection='3d')
ax.plot3D(y1[:, 0], y1[:, 1], y1[:, 2], 'gray')
plt.show()
### Adams-Bashforth 2nd order
time_start = time.clock()
nt = 500
# time points
t = np.linspace(0, 25, nt)
# solve ODE
y1 = Adams_Bash_2nd(func, y0, t)
print(y1)
time_elapsed = (time.clock() - time_start)
print('elapsed time', time_elapsed)
# compute residual:
r = y1[:, 0] ** 2 + y1[:, 1] ** 2 + y1[:, 2] ** 2 - 1
rmax1 = np.max(np.abs(r))
print('error', rmax1)
fig = plt.figure()
ax = plt.axes(projection='3d')
ax.plot3D(y1[:, 0], y1[:, 1], y1[:, 2], 'gray')
plt.show()
### Adams-Moulton 1st order
# time_start = time.clock()
nt = 500
# time points
t = np.linspace(0, 25, nt)
# solve ODE
y1 = Adams_Moulton(func, y0, t)
print(y1)
# time_elapsed = (time.clock() - time_start)
# print('elapsed time', time_elapsed)
# compute residual:
r = y1[:, 0] ** 2 + y1[:, 1] ** 2 + y1[:, 2] ** 2 - 1
rmax1 = np.max(np.abs(r))
print('error', rmax1)
fig = plt.figure()
ax = plt.axes(projection='3d')
ax.plot3D(y1[:, 0], y1[:, 1], y1[:, 2], 'gray')
plt.show()
exit()
My fucntion adam_moulton() keeps returning the following error:
Traceback (most recent call last):
File "C:/Users/Andrew/Documents/solve_pendulum.py", line 249, in <module>
y1 = Adams_Moulton(func, y0, t)
File "C:/Users/Andrew/Documents/solve_pendulum.py", line 148, in Adams_Moulton
y[i + 1, :] = y + (dt/24) * (k_1 + k_2 - k_3 + k_4)
ValueError: could not broadcast input array from shape (500,6) into shape (6)
What is wrong in my thought process for implementing this multistep method?
Thank you all for the help!

The function definition for the pendulum in Cartesian coordinates, a particle moving under gravity and the constraint |x|^2=L^2=const., can be written much shorter as
def func(y,t):
x,v = y[:3],y[3:6]
grav = np.array([0., 0., -9.8 ])
lambd = (grav.dot(x)+v.dot(v))/x.dot(x)
return np.concatenate([v, grav - lambd*x] )
# initial condition
y0=np.array([0.0,1.0,0.0,0.8,0.0,1.2])
As long as no friction term is included, the mass cancels out and need not be considered in the equations.
An inefficient implementation (no use of Jacobian and Newton-like steps) of the 4th order Adams-Moulton PECE method is
def Adams_Moulton_4th(function, y_matrix, time):
y = np.zeros((np.size(time), np.size(y_matrix)))
y[0] = y_matrix
### bootstrap steps with 4th order one-step method
dt = time[1] - time[0]
y[1] = RK4_step(function,y[0], time[0], dt, N=4)
y[2] = RK4_step(function,y[1], time[1], dt, N=4)
y[3] = RK4_step(function,y[2], time[2], dt, N=4)
f_m2 = function(y[0], time[0])
f_m1 = function(y[1], time[1])
f_0 = function(y[2], time[2])
f_1 = function(y[3], time[3])
for i in range(3,len(time) - 1):
### first shift the "virtual" function value array so that
### [f_m3, f_m2, f_m1, f_0] corresponds to [ f[i-3], f[i-2], f[i-1], f[i] ]
f_m3, f_m2, f_m1, f_0 = f_m2, f_m1, f_0, f_1
### predictor formula 4th order [ 55/24, -59/24, 37/24, -3/8 ]
y[i+1] = y[i] + (dt/24) * (55*f_0 - 59*f_m1 + 37*f_m2 - 9*f_m3)
f_1 = function(y[i+1], time[i+1])
### Corrector formula 4th order [ 3/8, 19/24, -5/24, 1/24 ]
y[i+1] = y[i] + (dt/24) * (9*f_1 + 19*f_0 - 5*f_m1 + f_m2)
f_1 = function(y[i+1], time[i+1])
return y
and results in with nt=2500 steps in a radius error of 2.39e-05 with a plot that is close to the reference solution by odeint.
The classical RK4 method used in the bootstrapping process was implemented as
def RK4_step(f,y,t,dt, N=1):
dt /= N;
for k in range(N):
k1=f(y,t)*dt; k2=f(y+k1/2,t+dt/2)*dt; k3=f(y+k2/2,t+dt/2)*dt; k4=f(y+k3,t+dt)*dt;
y, t = y+(k1+2*(k2+k3)+k4)/6, t+dt
return y

Difficulties in understanding Python code that computes transformed mesh

There's some code that I have basically translated from its original version in MATLAB to Python but there is a part that I don't understand. I have one triangle mesh that I want to transform. For this, the transformations for each triangle face is computed. After that, the edges of the transformed triangle are specified. However, what happens then is unclear to me. If you have any mathematical or geometric background to share concerning the following steps, I would appreciate it very much!
weight = np.ones(1)
fixvertex = 1
fixto = np.zeros((3))
M = scipy.sparse.lil_matrix((len(template) + fixvertex, len(template)))
dx = scipy.sparse.lil_matrix((len(template) + fixvertex, 3))
for i in range(len(faces)):
v = faces[i, :]
### ... what happens inbetween is irrelevant
# Transform triangle
x = T # np.array([template[v[0], :], template[v[1], :], template[v[2], :]]).reshape(3, 3).T
p12 = x[:, 0] - x[:, 1]
p13 = x[:, 0] - x[:, 2]
p23 = x[:, 1] - x[:, 2]
p12 = p12 / np.linalg.norm(p12)
p13 = p13 / np.linalg.norm(p13)
p23 = p23 / np.linalg.norm(p23)
# I do not get the point of the following lines
wts = [(p13.T).dot(p23), -(p12.T).dot(p23), (p12.T).dot(p13)]
wij3 = [[0, wts[0] / np.sqrt(1 - wts[0] ** 2), wts[1] / np.sqrt(1 - wts[1] ** 2)],
[0, 0, wts[2] / np.sqrt(1 - wts[2] ** 2)],
[0, 0, 0]]
wij3 = np.asarray(wij3)
wij3 = wij3 + wij3.T
WIJ = wij3 - np.diag([sum(x) for x in zip(*wij3)])
M[np.ix_(v, v)] = M[np.ix_(v, v)] + WIJ
dx[v, :] = dx[v, :] + (WIJ # x.T)
weight = np.ones((fixvertex)) * weight
for i in range(fixvertex):
M[len(template) + i, fixvertex - 1] = weight[i]
dx[len(template):len(template) + fixvertex, :] = np.multiply(fixto, np.tile(weight, (3)))
M = np.real(M)
dx = np.real(dx)
Mt = M.T
model = scipy.sparse.linalg.spsolve(Mt # M, Mt # dx)

Categories