import logging

import numpy as np

def sigmoid(z):
    return 1 / (1 + np.exp(-z))

def propagate(w, b, X, Y):
    m = X.shape[1]

    # forward propagation
    A = sigmoid(np.dot(w.T, X) + b)
    cost = -(1/m) * np.sum(Y*np.log(A) + (1-Y)*np.log(1-A))

    # backward propagation
    dw = (1/m) * np.dot(X, (A - Y).T)
    db = (1/m) * np.sum(A - Y)

    cost = np.squeeze(cost)

    grads = { "dw": dw, "db": db }

    return grads, cost

def optimize(w, b, X, Y, iterations, learning_rate):
    costs = []

    for i in range(iterations):
        grads, cost = propagate(w, b, X, Y)

        w = w - (learning_rate * grads['dw'])
        b = b - (learning_rate * grads['db'])

        if i % 100 == 0:
            logging.info(f"iteration: {i}, cost: {cost}")
            costs.append(cost)

        params = { "w": w, "b": b }

        grads = { "dw": grads["dw"], "db": grads["db"] }

    return params, grads, costs

def predict(w, b, X):
    m = X.shape[1]
    w = w.reshape(X.shape[0], 1)

    Y_prediction = np.zeros((1,m))

    A = sigmoid(np.dot(w.T, X) + b)

    for i in range(A.shape[1]):
        if A[0,i] > 0.5:
            Y_prediction[0, i] = 1

    return Y_prediction


import numpy
from matplotlib import pyplot

def euler_pc(f, x_end, y0, N):
    x, dx = numpy.linspace(0, x_end, N+1, retstep=True)
    y = numpy.zeros((len(y0),N+1))
    y[:,0] = y0
    for n in range(N):
        fn = f(x[n], y[:,n])
        yp = y[:,n] + dx * fn
        y[:,n+1] = y[:,n] + dx / 2 * (fn + f(x[n+1], yp))
    return x, dx, y

def ab2(f, x_end, y0, N):
    x, dx = numpy.linspace(0, x_end, N+1, retstep=True)
    y = numpy.zeros((len(y0),N+1))
    fn = numpy.zeros((len(y0),N+1))
    y[:,0] = y0
    fn[:,0] = f(x[0], y[:,0])
    x_epc, dx_epc, y_epc = euler_pc(f, dx, y0, 1)
    y[:,1] = y_epc[:,1]
    for n in range(1,N):
        fn[:,n] = f(x[n], y[:,n])
        y[:,n+1] = y[:,n] + dx * (3 * fn[:,n] - fn[:,n-1]) / 2
    return x, dx, y

if __name__=="__main__":

    def f_sin(x, y):
        return -numpy.sin(x)
    print("Euler Predictor-Corrector")
    x, dx, y = euler_pc(f_sin, 0.5, [1], 5)
    print("dx=", dx, "y(0.5)=", y[0,-1])
    x, dx, y = euler_pc(f_sin, 0.5, [1], 50)
    print("dx=", dx, "y(0.5)=", y[0,-1])
    Npoints = 5*2**numpy.arange(1,10)
    dx_all = 0.5/Npoints
    errors = numpy.zeros_like(dx_all)
    for i, N in enumerate(Npoints):
        x, dx, y = euler_pc(f_sin, 0.5, [1], N)
        errors[i] = abs(y[0,-1] - numpy.cos(0.5))
        dx_all[i] = dx
    pyplot.figure(figsize=(12,6))
    pyplot.loglog(dx_all, errors, 'kx')
    pyplot.loglog(dx_all, errors[0]*(dx_all/dx_all[0])**2, 'b-',
                  label=r"$\propto \Delta x^2$")
    pyplot.legend(loc='upper left')
    pyplot.xlabel(r"$\Delta x$")
    pyplot.ylabel("Error")
    pyplot.show()

    print("Adams-Bashforth 2")
    x, dx, y = ab2(f_sin, 0.5, [1], 5)
    print("dx=", dx, "y(0.5)=", y[0,-1])
    x, dx, y = ab2(f_sin, 0.5, [1], 50)
    print("dx=", dx, "y(0.5)=", y[0,-1])
    Npoints = 5*2**numpy.arange(1,10)
    dx_all = 0.5/Npoints
    errors = numpy.zeros_like(dx_all)
    for i, N in enumerate(Npoints):
        x, dx, y = ab2(f_sin, 0.5, [1], N)
        errors[i] = abs(y[0,-1] - numpy.cos(0.5))
        dx_all[i] = dx
    pyplot.figure(figsize=(12,6))
    pyplot.loglog(dx_all, errors, 'kx')
    pyplot.loglog(dx_all, errors[0]*(dx_all/dx_all[0])**2, 'b-',
                  label=r"$\propto \Delta x^2$")
    pyplot.legend(loc='upper left')
    pyplot.xlabel(r"$\Delta x$")
    pyplot.ylabel("Error")
    pyplot.show()


# Question 1, Lab 6
# AB Satyaprakash, 180123062

# imports
from sympy.abc import t,y
import numpy as np
import sympy as sp
import pandas as pd
import matplotlib.pyplot as plt

# functions
def getEulerApproximation(f,X,Y,h):
    for i in range(1,Y.shape[0]):
        Y[i]=Y[i-1] + (f.subs({t:X[i-1], y:Y[i-1]})*h)

def getActualValues(g,X):
    for i in range(X.shape[0]):
        Z[i]=g.subs(t,X[i])

def phi(n, k, x, z):

    prod = 1
    for i in range(n):
        if i != k:
            prod = prod * (z - x[i])

    return prod

def lagrangeInterpolation(z, x, fx):

    n = len(x)
    l = np.empty(n)
    for i in range(n):
        l[i] = phi(n,i,x,z)/phi(n,i,x,x[i])

    ans = np.dot(l, fx)

    return ans

# program body
# t belongs to [1,2] and y(1)=-1, with h = 0.05
a, b, h = 1, 2, 0.05
X = np.arange(a,b+h/2,h)
Y = np.zeros(X.shape[0])
Z = np.zeros(X.shape[0])
Y[0] = -1

f = 1/(t**2) - y/t - (y**2) # from question
g = -1/t

getEulerApproximation(f,X,Y,h)
getActualValues(g,X)

print('(a)')
table={'Evaluate':[], 'Approx':[], 'Actual':[], 'Error':[]}

for i in range(X.shape[0]):
    table['Evaluate'].append('y({})'.format(round(X[i],2)))
    table['Approx'].append(Y[i])
    table['Actual'].append(Z[i])
    table['Error'].append(abs(Y[i]-Z[i]))

df = pd.DataFrame(table,columns=['Evaluate', 'Approx', 'Actual','Error'])
print(df)

print('\n(b)')
print('(I)')
x = 1.052
intepolatedVal = lagrangeInterpolation(x,X,Y)
print('Estimated value of y({}) from interpolation ={}'.format(x,intepolatedVal))
print('Actual value of y = {}'.format(g.subs(t,x)))
print('The error between them = {}'.format(abs(intepolatedVal-g.subs(t,x))))

print('(II)')
x = 1.555
intepolatedVal = lagrangeInterpolation(x,X,Y)
print('Estimated value of y({}) from interpolation ={}'.format(x,intepolatedVal))
print('Actual value of y = {}'.format(g.subs(t,x)))
print('The error between them = {}'.format(abs(intepolatedVal-g.subs(t,x))))

print('(III)')
x = 1.978
intepolatedVal = lagrangeInterpolation(x,X,Y)
print('Estimated value of y({}) from interpolation ={}'.format(x,intepolatedVal))
print('Actual value of y = {}'.format(g.subs(t,x)))
print('The error between them = {}'.format(abs(intepolatedVal-g.subs(t,x))))

# plotting the values in the table.
plt.plot(X,Y, label='Euler Approximation')
plt.plot(X,Z, label='Actual value')
plt.xlabel('Value of t')
plt.ylabel('Value of y')
plt.title('Comparision between Euler Approx and Actual values of y')
plt.legend()
plt.show()

Semester 6/MA 322 (Scientific Computing Theory)/Labs/Lab 6/Code/q3.py


#!/usr/bin/python
#   X = GAUSS(N) returns N Legendre points X in (-1,1).
#
#   X, C = GAUSS(N) returns also a vector C of weights for
#   Gauss-Legendre quadrature.
#
#   X, C = GAUSS(N, [A, B]) returns the nodes and weeights
#   for the interval [A, B].

import numpy as np

def gauss(N, dom=[-1,1]):
    x = np.zeros(shape=(N,1))
    for k in range(N):
        x[k] = -np.cos((0.5+k)*np.pi/N)
    tol = 1e-10
    dx = 1 + 0 *x
    # Loop until convergence:
    while np.linalg.norm(dx, np.inf) > tol:
        # Recurrence relation for Legendre polynomials:
        Pm2 = 1
        Pm1 = x
        for n in range(1,N):
            P = ( (2*n+1)*Pm1*x - n*Pm2 ) / (n+1)
            Pm2 = Pm1
            Pm1 = P

        # Derivative.  See NIST (18.9.17)
        dPdx = -N * (x*P - Pm2) / (1 - x**2)

        # Newton step:
        dx = -P / dPdx;
        # Newton update:
        x = x + dx;

    # Weights
    c = 2 / ( (1-x**2) * dPdx**2 );

    # Scale
    a = dom[0];
    b = dom[1];
    x = .5*(b-a)*x + .5*(b+a);
    c = .5*(b-a)*c;

    return (x, c)

if __name__ == "__main__":
    x, c = gauss(5, [-1, 1])
    print 'x = ', x
    print 'c = ', c


import numpy as np

def dot_product(vector1, vector2):
    """ Implement dot product of the two vectors.
    Args:
        vector1: numpy array of shape (x, n)
        vector2: numpy array of shape (n, x)

    Returns:
        out: numpy array of shape (x,x) (scalar if x = 1)
    """
    out = None
    ### YOUR CODE HERE

    if(len(np.shape(vector1))==1):
        vector1.shape=(1,vector1.shape[0]);
    if(len(np.shape(vector2))==1):
        vector2.shape=(1,vector2.shape[0]);

    input_size1=np.shape(vector1);

    row=input_size1[0];

    out = np.ones((row,row));

    for i in range(row):
        for j in range(row):
            x1=vector1[i,];
            x2=vector2.T[j];
            out[i,j]=x1.dot(x2);

    if(np.shape(out)==(1,1)):
        out=out[0,0];

    ### END YOUR CODE
    return out

def matrix_mult(M, vector1, vector2):
    """ Implement (vector1.T * vector2) * (M * vector1)
    Args:
        M: numpy matrix of shape (x, n)
        vector1: numpy array of shape (1, n)
        vector2: numpy array of shape (n, 1)

    Returns:
        out: numpy matrix of shape (1, x)
    """
    out = None
    ### YOUR CODE HERE
    x=vector1.T*vector2;
    y=M*vector1;
    out=x.T.dot(y.T);
    ### END YOUR CODE

    return out

def svd(matrix):
    """ Implement Singular Value Decomposition
    Args:
        matrix: numpy matrix of shape (m, n)

    Returns:
        u: numpy array of shape (m, m)
        s: numpy array of shape (k)
        v: numpy array of shape (n, n)
    """
    u = None
    s = None
    v = None
    ### YOUR CODE HERE
    u,s,v=np.linalg.svd(matrix,1,1);
    ### END YOUR CODE

    return u, s, v

def get_singular_values(matrix, n):
    """ Return top n singular values of matrix
    Args:
        matrix: numpy matrix of shape (m, w)
        n: number of singular values to output

    Returns:
        singular_values: array of shape (n)
    """
    singular_values = None
    u, s, v = svd(matrix)
    ### YOUR CODE HERE
    singular_values=s[0:n];
    ### END YOUR CODE
    return singular_values

def eigen_decomp(matrix):
    """ Implement Eigen Value Decomposition
    Args:
        matrix: numpy matrix of shape (m, )

    Returns:
        w: numpy array of shape (m, m) such that the column v[:,i] is the eigenvector corresponding to the eigenvalue w[i].
    """
    w = None
    v = None
    ### YOUR CODE HERE
    w,v=np.linalg.eig(matrix);
    ### END YOUR CODE
    return w, v

def get_eigen_values_and_vectors(matrix, num_values):
    """ Return top n eigen values and corresponding vectors of matrix
    Args:
        matrix: numpy matrix of shape (m, m)
        num_values: number of eigen values and respective vectors to return

    Returns:
        eigen_values: array of shape (n)
        eigen_vectors: array of shape (m, n)
    """
    w, v = eigen_decomp(matrix)
    eigen_values = []
    eigen_vectors = []
    ### YOUR CODE HERE
    m=np.shape(matrix)[0];
    value_sorted=np.argsort(w);
    eigen_values=w[sorted(value_sorted[-num_values:])];
    eigen_vectors=np.zeros((m,num_values));
    for i in range(m):
        for j in range(num_values):
            eigen_vectors[i,j]=v[i,value_sorted[-1-j]];
    ### END YOUR CODE
    return eigen_values, eigen_vectors


import numpy as np
import matplotlib.image as mpimg
import matplotlib.pyplot as plt

from sklearn.decomposition import PCA

img = mpimg.imread('wild.png')

#Now, let's look at the size of this numpy array object img as well as plot it using imshow.

print(img.shape)
plt.axis('off')
plt.imshow(img)

# In[65]:

#Okay, so the array has 711 rows each of pixel 996x3. Let's reshape it into a format that PCA can understand.
# 2988 = 996 * 3
img_r = np.reshape(img, (711, 2988))
print(img_r.shape)

# In[66]:

# Great, now lets run RandomizedPCA with 64 components (8x8 pixels) and transform the image.

ipca = PCA(64, svd_solver='randomized').fit(img_r)
img_c = ipca.transform(img_r)
print(img_c.shape)
print(np.sum(ipca.explained_variance_ratio_))

#Great, looks like with 64 components we can explain about 96% of the variance.

# In[69]:

#OK, now to visualize how PCA has performed this compression, let's inverse transform the PCA output and
#reshape for visualization using imshow.
temp = ipca.inverse_transform(img_c)
print(temp.shape)

#reshaping 2988 back to the original 996 * 3
temp = np.reshape(temp, (711,996,3))

print(temp.shape)

# In[83]:

#Great, now lets visualize like before with imshow
plt.axis('off')
plt.imshow(temp.astype(np.uint8))


#!/usr/bin/env python
# -*- coding: utf-8 -*-
# @Time     : 2018/11/25 14:27
# @Author   : Iydon
# @File     : 7.3.py

import numpy as np

def jacobi_method(A, b, x0=None):
    """
    A = L(ow) + D(iag) + U(pper)
    |ρ(inv(D)*(L+U))| < 1
    => convergent.
    """
    A_,b_ = A.copy(),b.copy()
    diag  = np.diag(A_)
    shap  = A.shape
    A_ = np.diag(diag) - A_
    if x0 is not None:
        x_ = x0.copy()
    else:
        x_ = np.zeros(b.shape)
    for i in range(shap[0]):
        A_[i,:] /= diag[i]
        b_[i,:] /= diag[i]
    while True:
        yield x_
        x_ = A_*x_ + b_

def gauss_seidel(A, b, x0=None):
    """
    A = L(ow) + D(iag) + U(pper)
    |ρ(inv(D-L)*U)| < 1
    => convergent.
    """
    A_,b_ = A.copy(),b.copy()
    DL_   = np.tril(A_)
    U_    = np.triu(A_, 1)
    if x0 is not None:
        x_ = x0.copy()
    else:
        x_ = np.zeros(b.shape)
    invDL_ = np.linalg.inv(DL_)
    A_ = np.matmul( invDL_, U_ )
    b_ = np.matmul( invDL_, b_ )
    while True:
        yield x_
        x_ = np.matmul(-A_, x_) + b_

A  = np.matrix([[3.,-1,1],[3,6,2],[3,3,7]])
b  = np.matrix([[1.],[0],[4]])
x0 = np.matrix([[0.],[0],[0]])
result = jacobi_method(A, b, x0)
t = 0
for r in result:
    print(t, r.T)
    t += 1
    if t > 10: break

result = gauss_seidel(A, b, x0)
t = 0
for r in result:
    print(t, r.T)
    t += 1
    if t > 10: break


#!/usr/bin/python

'''
mjtsai1974@20180606, v1.0, MLE for https://mjtsai1974.github.io/DevBlog/2018/06/06/bayesian-ml-beyes-to-practice/

http://www.astroml.org/book_figures/chapter3/fig_gaussian_distribution.html

dist = scipy.stats.norm(...)

Where ... should be filled in with the desired distribution parameters Once we have defined the distribution parameters in this way, these distribution objects have many useful methods; for example:

    dist.pmf(x) computes the Probability Mass Function at values x in the case of discrete distributions
    dist.pdf(x) computes the Probability Density Function at values x in the case of continuous distributions
    dist.rvs(N) computes N random variables distributed according to the given distribution
'''

import numpy as np
from scipy.stats import norm
from matplotlib import pyplot as plt

'''
#----------------------------------------------------------------------
# This function adjusts matplotlib settings for a uniform feel in the textbook.
# Note that with usetex=True, fonts are rendered with LaTeX.  This may
# result in an error if LaTeX is not installed on your system.  In that case,
# you can set usetex to False.
from astroML.plotting import setup_text_plots
setup_text_plots(fontsize=8, usetex=False)
'''

# Define the given measured weights and the prior(the most current value)
prior = 14.2
weights = [13.9, 14.1, 17.5]
n = len(weights)

# Define the distributions to be plotted
mu = prior
sample_variance = np.var(weights, ddof=1)
sample_std_deviation = np.sqrt(sample_variance)

population_variance = np.var(weights, ddof=0)
population_std_deviation = np.sqrt(population_variance)

sigma_values = [sample_std_deviation, population_std_deviation]
linestyles = ['-', '--', ':']
colours = ['red', 'blue']
colours_sample = ['black', 'purple', 'green']
x_axis = np.linspace(mu - 2 * np.max(sigma_values), mu + 2 * np.max(sigma_values), 100) #use the

# plot the distributions
fig, ax = plt.subplots(figsize=(5, 3.75))

for sigma, ls, color in zip(sigma_values, linestyles, colours):
    # create a gaussian / normal distribution
    dist = norm(mu, sigma)

    plt.plot(x_axis, dist.pdf(x_axis), ls=ls, c=color,
             label=r'$\mu=%.3f,\ \sigma=%.3f$' % (mu, sigma))

plt.xlim(mu - 2 * np.max(sigma_values), mu + 2 * np.max(sigma_values))
plt.ylim(0, 0.45)

plt.xlabel('$x$')
plt.ylabel(r'$p(x|\mu,\sigma)$')
plt.title('Gaussian Distribution')

plt.legend()
plt.show()

# plot the new distribution v.s. mother N(prior, sample_std_deviation)
fig, ax = plt.subplots(figsize=(5, 3.75))

mother_dist = norm(mu, sample_std_deviation)
weight_sample_sigmas = []

for w in weights:
    weight_sigma = 1/(mother_dist.pdf(w) * np.sqrt(2 * np.pi))
    weight_sample_sigmas.append(weight_sigma)

    print('P({0:5.3f}) of N({1:5.3f},{2:5.3f}) = {3:5.3f} for N({0:5.3f}), the std error {4:5.3f}'.format(w, prior, sample_std_deviation, mother_dist.pdf(w), weight_sigma))

# plot the mother N(prior, sample_std_deviation)
plt.plot(x_axis, mother_dist.pdf(x_axis), ls='-', c='red',
             label=r'$\mu=%.3f,\ \sigma=%.3f$' % (mu, sample_std_deviation))

# plot the new N(w_sample, weight_sigma)
for w, sigma, ls, color in zip(weights, weight_sample_sigmas, linestyles, colours_sample):
    dist = norm(w, sigma)  # create a gaussian / normal distribution

    plt.plot(x_axis, dist.pdf(x_axis), ls=ls, c=color,
             label=r'$\mu=%.3f,\ \sigma=%.3f$' % (w, sigma))

plt.xlim(mu - 2 * np.max(sigma_values), mu + 2 * np.max(sigma_values))
plt.ylim(0, 0.45)

plt.xlabel('$x$')
plt.ylabel(r'$p(x|\mu,\sigma)$')
plt.title('Gaussian Distribution')

plt.legend()
plt.show()

# plot the new distribution v.s. mother N(prior, population_std_deviation)
fig, ax = plt.subplots(figsize=(5, 3.75))

mother_dist = norm(mu, population_std_deviation)
weight_population_sigmas = []

for w in weights:
    weight_sigma = 1/(mother_dist.pdf(w) * np.sqrt(2 * np.pi))
    weight_population_sigmas.append(weight_sigma)

    print('P({0:5.3f}) of N({1:5.3f},{2:5.3f}) = {3:5.3f} for N({0:5.3f}), the std error {4:5.3f}'.format(w, prior, population_std_deviation, mother_dist.pdf(w), weight_sigma))

# plot the mother N(prior, sample_std_deviation)
plt.plot(x_axis, mother_dist.pdf(x_axis), ls='-', c='blue',
             label=r'$\mu=%.3f,\ \sigma=%.3f$' % (mu, population_std_deviation))

# plot the new N(w_sample, weight_sigma)
for w, sigma, ls, color in zip(weights, weight_population_sigmas, linestyles, colours_sample):
    dist = norm(w, sigma)  # create a gaussian / normal distribution

    plt.plot(x_axis, dist.pdf(x_axis), ls=ls, c=color,
             label=r'$\mu=%.3f,\ \sigma=%.3f$' % (w, sigma))

plt.xlim(mu - 2 * np.max(sigma_values), mu + 2 * np.max(sigma_values))
plt.ylim(0, 0.45)

plt.xlabel('$x$')
plt.ylabel(r'$p(x|\mu,\sigma)$')
plt.title('Gaussian Distribution')

plt.legend()
plt.show()

# MLE to ask for possible real weight with N(prior, sample std deviation)
MLE_max = 0
mu_max = 0

mother_dist = norm(mu, sample_std_deviation)

for w, sigma in zip(weights, weight_sample_sigmas):
    dist = norm(w, sigma)

    MLE_now = 1.0

    for j in range(len(weights)):
        MLE_now = dist.pdf(weights[j]) * mother_dist.pdf(w) * MLE_now

    if MLE_now > MLE_max:
        MLE_max = MLE_now
        mu_max = w

print('using N(prior, sample std deviation), the MLE for weight {0: 5.3f}'.format(mu_max))

# MLE to ask for possible real weight with N(prior, population std deviation)
MLE_max = 0
mu_max = 0

mother_dist = norm(mu, population_std_deviation)

for w, sigma in zip(weights, weight_sample_sigmas):
    dist = norm(w, sigma)

    MLE_now = 1.0

    for j in range(len(weights)):
        MLE_now = dist.pdf(weights[j]) * mother_dist.pdf(w) * MLE_now

    if MLE_now > MLE_max:
        MLE_max = MLE_now
        mu_max = w

print('using N(prior, population std deviation), the MLE for weight {0: 5.3f}'.format(mu_max))


import math

import numpy as np

# Calculates Rotation Matrix given euler angles.
def euler_angle_to_rotation_matrix(theta):
    """
    Converts from theta [yaw, pitch, roll] to a rotation matrix
    """

    R_x = np.array(
        [
            [1, 0, 0],
            [0, math.cos(theta[0]), -math.sin(theta[0])],
            [0, math.sin(theta[0]), math.cos(theta[0])],
        ]
    )

    R_y = np.array(
        [
            [math.cos(theta[1]), 0, math.sin(theta[1])],
            [0, 1, 0],
            [-math.sin(theta[1]), 0, math.cos(theta[1])],
        ]
    )

    R_z = np.array(
        [
            [math.cos(theta[2]), -math.sin(theta[2]), 0],
            [math.sin(theta[2]), math.cos(theta[2]), 0],
            [0, 0, 1],
        ]
    )

    R = np.dot(R_z, np.dot(R_y, R_x))

    return R

def rotation_matrix_to_euler_angle(R):
    """
    Converts a rotation matrix to [yaw, pitch, roll]
    """
    yaw = math.atan2(R[1, 0], R[0, 0])
    pitch = math.atan2(-R[2, 0], math.sqrt(R[2, 1] ** 2 + R[2, 2] ** 2))
    roll = math.atan2(R[2, 1], R[2, 2])

    return yaw, pitch, roll

def static_transform_to_extrinsic(transform):
    """
    Calculates the extrinsic matrix for a ROS static_transform_publisher

    Parameters:
    - transform: a list with [x, y, z yaw, pitch, roll]
    """
    assert len(transform) == 6

    x, y, z, yaw, pitch, roll = transform
    rotation = euler_angle_to_rotation_matrix([yaw, pitch, roll])
    translation = np.array([x, y, z]).reshape(-1, 1)
    homogeneous = get_homogeneous_transformation(rotation, translation)
    return homogeneous

def extrinsic_to_static_transform(mat):
    """Converts an extrinsic matrix into rotational and translational components.

    Args:
        mat (np.array): a 4x4 extrinsic matrix

    Returns:
        np.array: a 1x6 np.array of form x, y, z, yaw, pitch, roll
    """
    x, y, z = mat[:3, 3]
    yaw, pitch, roll = rotation_matrix_to_euler_angle(mat)

    return np.array([x, y, z, yaw, pitch, roll])

def get_homogeneous_transformation(rotation, translation):
    """
    Parameters:
    - rotation: a 3x3 rotation np array
    - translation: a 3x1 translation np array
    """
    homogeneous = np.zeros((4, 4))
    homogeneous[-1][-1] = 1
    homogeneous[:3, :3] = rotation
    homogeneous[:3, -1:] = translation
    return homogeneous

def get_relative_transformation(a_to_base, b_to_base):
    """
    Finds the relative transformation from a to b given transformations for both
    relative to base_link.

    See: https://stackoverflow.com/a/55169091/6942666 for more details

    Parameters:
    - a_to_base: 4x4 np.array representing homoegenous transformation of a to base_link
    - b_to_base: 4x4 np.array representing homoegenous transformation of b to base_link
    """

    base_to_a = np.linalg.inv(a_to_base)
    return base_to_a @ b_to_base

if __name__ == "__main__":
    """
    <node pkg="tf" type="static_transform_publisher"
        respawn="true"
        name="camera_static_transform_publisher"
        args="0.5080 0.0 0.1778 0 0.05 0 base_link camera 100"
    />

    <node pkg="tf" type="static_transform_publisher"
        respawn="true"
        name="velodyne_static_transform_publisher"
        args="0.4445 0.0 0.09525 0.0 0.06981 0.0 base_link velodyne 100"
    />

    Camera static transform:
    [[ 0.99875026  0.          0.04997917  0.508     ]
    [ 0.          1.          0.          0.        ]
    [-0.04997917  0.          0.99875026  0.1778    ]
    [ 0.          0.          0.          1.        ]]

    Velodyne static transform:
    [[ 0.99756427  0.          0.06975331  0.4445    ]
    [ 0.          1.          0.          0.        ]
    [-0.06975331  0.          0.99756427  0.09525   ]
    [ 0.          0.          0.          1.        ]]

    Camera relative to Velodyne
    [[ 0.99980379  0.          0.0198087  -0.05929486]
    [ 0.          1.          0.          0.        ]
    [-0.0198087   0.          0.99980379 -0.08562051]
    [ 0.          0.          0.          1.        ]]

    Overall camera to velodyne transform
    Translation (x, y, z): -0.059294861111785835 0.0 -0.08562051124429254
    Rotation (y, p, r): 0.0 0.019810000000000008 0.0
    """

    print("Camera static transform: ")
    # Add values for the camera to base here
    camera_to_base = static_transform_to_extrinsic([0.5080, 0.0, 0.1778, 0, 0.05, 0])
    print(camera_to_base)

    print("Velodyne static transform: ")
    # Add values for the velodyne to base here
    velodyne_to_base = static_transform_to_extrinsic(
        [0.4445, 0.0, 0.09525, 0.0, 0.06981, 0.0]
    )
    print(velodyne_to_base)

    print("Camera relative to Velodyne")
    # Add values for the camera to velodyne here
    camera_to_velodyne = get_relative_transformation(camera_to_base, velodyne_to_base)
    print(camera_to_velodyne)

    print("Overall camera to velodyne transform")
    x, y, z, yaw, pitch, roll = extrinsic_to_static_transform(camera_to_velodyne)
    print("Translation (x, y, z):", x, y, z)
    print("Rotation (y, p, r):", yaw, pitch, roll)