import open3d import numpy as np import util import matplotlib.pyplot as plt

def f(a, b, x): return np.exp(-a * x)* np.sin(x) + b

def get_residual(x, a, b, y): return f(a, b, x) - y

def get_jacobian(x, a, b): return np.column_stack((-x*np.exp(-a*x)*np.sin(x), np.ones(x.shape[0])))

num_obs = 50 agt = 2 bgt = 3.5 kgt = np.transpose(np.array([agt, bgt])) lmda = 10 a0 = 10 b0 = -12 k0 = np.transpose(np.array([a0, b0])) num_iter = 200 tol = 1e-15

x_obs = np.linspace(1, 25, num_obs) y_obs = f(agt, bgt, x_obs)

def lm(k0, lmda): k = k0 cost = [] for i in range(0,num_iter): cur_residual = get_residual(x_obs,k[0], k[1], y_obs) cur_jacobian = get_jacobian(x_obs, k[0], k[1]) dk = -np.matmul(np.linalg.inv(np.matmul(np.transpose(cur_jacobian), cur_jacobian) + lmda*np.identity(2)), np.matmul(np.transpose(cur_jacobian), cur_residual)) k = k + dk cost.append(np.sum((cur_residual)**2)) if (i!=0): if(cost[i] > cost[i-1]): lmda = lmda*10 else: lmda = lmda/10 if(abs(dk[0]) < tol and abs(dk[1]) < tol): break return cost, k, i

cost, k, i = lm(k0, lmda) print("The predicted values of k is", k)

plt.figure(figsize=(10, 8)) plt.plot(np.linspace(1, i+1, i+1), cost); plt.title("Loss vs Number of Iterations for Levenberg Marquadt") plt.xlabel("Number of Iterations") plt.ylabel("Loss") plt.show()

y_pred = f(k[0], k[1], x_obs)

plt.figure(figsize=(10, 8)) plt.plot(x_obs, y_obs, label="Ground Truth", marker="o"); plt.plot(x_obs, y_pred, label="Predicted"); plt.legend(); plt.title("Predicted vs Ground Truth(Gauss Newton)") plt.show()

def read_bin_file(file_name): ''' Read the bin file ''' points = np.fromfile(file_name, dtype=np.float32).reshape(-1, 4) points = points[:,:3] # exclude reflectance values, becomes [X Y Z] points = points[1::5,:] # remove every 5th point for display speed (optional) return points

filename = "./data/000013.bin" points = read_bin_file(filename) pointd = np.array(points) # Function used to visualize point clouds, takes a list of 3 x N numpy array as input and plots util.visualize_pointclouds([points.T])

def ransac(pcd, distance_threshold, max_iter): est_plane = np.array([]) est_inliers = np.array([]) max_inlier = 0 for _ in range(max_iter): rnd_idx = np.random.choice(points.shape[0], 3) selected_points = points[rnd_idx] x1, y1, z1 = selected_points[0] x2, y2, z2 = selected_points[1] x3, y3, z3 = selected_points[2] a = (y2 - y1)*(z3 - z1) - (z2 - z1)*(y3 - y1) b = (z2 - z1)*(x3 - x1) - (x2 - x1)*(z3 - z1) c = (x2 - x1)*(y3 - y1) - (y2 - y1)*(x3 - x1) d = -(a*x1 + b*y1 + c*z1) if(a==0 and b ==0 and c==0 and d==0): continue distances = abs(a * points[:,0] + b * points[:,1] + c * points[:,2] + d)/np.sqrt(a**2 + b**2 + c**2+ 0.000001) inliers = points[distances < distance_threshold] if(inliers.shape[0] > max_inlier): max_inlier = inliers.shape[0] est_plane = np.array([a, b, c, d]) est_inliers = inliers return est_plane

distance_threshold, max_iter = (0.1, 10000) a, b, c, d = (ransac(points, distance_threshold, max_iter)) print(a,b,c,d)

x_range = np.linspace(np.min(points[:,0]), np.max(points[:,0]), 60) y_range = np.linspace(np.min(points[:,1]), np.max(points[:,1]), 60) plane_points = [] for px in x_range: for py in y_range: plane_points.append([px, py, -(a * px + b * py + d)/c]) plane_points = np.array(plane_points) util.visualize_pointclouds([points.T, plane_points.T])