def look_at_matrix(camera_position: np.ndarray, camera_target: np.ndarray,
camera_up: np.ndarray):
z_axis = Utils.normalize(np.array(camera_position - camera_target))
x_axis = Utils.normalize(np.cross(camera_up, z_axis))
y_axis = np.cross(z_axis, x_axis)
translation_x = -np.dot(x_axis, camera_position)
translation_y = -np.dot(y_axis, camera_position)
translation_z = -np.dot(z_axis, camera_position)
translation = np.array([translation_x, translation_y,
translation_z]).reshape((3, 1))
x_axis = x_axis.reshape((3, 1))
y_axis = y_axis.reshape((3, 1))
z_axis = z_axis.reshape((3, 1))
translation = translation.reshape((3, 1))
#TODO: Make this more efficient
mat = np.append(np.append(np.append(x_axis, y_axis, axis=1),
z_axis,
axis=1),
translation,
axis=1)
return np.append(mat, Utils.homogenous_for_SE3(), axis=0)
def lie_ackermann_correction(gradient_step, motion_cov_inv, ackermann_twist,
vo_twist, twist_size):
# ack_prior = np.multiply(Gradient_step_manager.current_alpha,ackermann_pose_prior)
ack_prior = ackermann_twist
#ack_prior = np.matmul(motion_cov_inv, ack_prior) # 1
#ack_prior = np.multiply(gradient_step, ack_prior) # 2
R_w, t_w = exp(vo_twist, twist_size)
R_ack, t_ack = exp(ack_prior, twist_size)
SE_3_w = np.append(np.append(R_w, t_w, axis=1),
Utils.homogenous_for_SE3(),
axis=0)
SE_3_ack = np.append(np.append(R_ack, t_ack, axis=1),
Utils.homogenous_for_SE3(),
axis=0)
SE3_w_ack = SE3.pose_pose_composition_inverse(SE_3_w, SE_3_ack)
w_inc = ln(SE3.extract_rotation(SE3_w_ack),
SE3.extract_translation(SE3_w_ack), twist_size)
w_inc = np.multiply(gradient_step, np.matmul(motion_cov_inv, w_inc)) # 1
#w_inc = np.matmul(motion_cov_inv, w_inc) # 2
#w_inc = np.multiply(gradient_step, w_inc)
return w_inc
def intersections(self,ray : Ray ):
center_to_ray = ray.origin - self.origin
# A is always 1 since vectors are normalized
B = 2.0 * Utils.fast_dot(center_to_ray, ray.direction)
center_to_ray_dot = Utils.fast_dot(center_to_ray, center_to_ray)
C = center_to_ray_dot - self.radius**2.0
discriminant = (B**2.0) - 4.0*C
if discriminant < 0:
return (False,0,0)
elif round(discriminant,3) == 0:
return False, -B / 2.0 , sys.float_info.min
else:
return True, (-B + sqrt(discriminant))/2.0, (-B - sqrt(discriminant))/2.0
def exp(w, twist_size):
w_angle = w[3:twist_size]
w_angle_transpose = np.transpose(w_angle)
w_x = Utils.skew_symmetric(w[3], w[4], w[5])
w_x_squared = np.matmul(w_x, w_x)
# closed form solution for exponential map
theta_sqred = np.matmul(w_angle_transpose, w_angle)[0][0]
theta = math.sqrt(theta_sqred)
A = 0
B = 0
C = 0
# TODO use Taylor Expansion when theta_sqred is small
if not theta == 0:
A = math.sin(theta) / theta
if not theta_sqred == 0:
B = (1 - math.cos(theta)) / theta_sqred
C = (1 - A) / theta_sqred
u = np.array([w[0], w[1], w[2]]).reshape((3, 1))
R_new = I_3 + np.multiply(A, w_x) + np.multiply(B, w_x_squared)
V = I_3 + np.multiply(B, w_x) + np.multiply(C, w_x_squared)
t_new = np.matmul(V, u)
return R_new, t_new
def generate_random_se3(trans_min,trans_max,sigma,yaw_range,pitch_rangle,roll_range):
t = np.reshape(np.random.uniform(trans_min,trans_max,3),(3,1))
yaw = np.random.normal(yaw_range, sigma) #around z
pitch = np.random.normal(pitch_rangle, sigma)# around y
roll = np.random.normal(roll_range, sigma) # around x
R_z = np.array([[cos(yaw), -sin(yaw), 0],
[sin(yaw), cos(yaw), 0],
[0, 0, 1]])
R_y = np.array([[cos(pitch), 0, sin(pitch)],
[0, 1, 0],
[-sin(pitch), 0, cos(pitch)]])
R_x = np.array([[1, 0, 0],
[0, cos(roll), -sin(roll)],
[0, sin(roll), cos(roll)]])
R = np.matmul(np.matmul(R_z,R_y),R_x)
SE3 = np.append(np.append(R,t,axis=1), Utils.homogenous_for_SE3(), axis=0)
return SE3
def ln(R, t, twist_size):
w = np.zeros((twist_size, 1), dtype=matrix_data_type)
trace = np.trace(R)
theta = math.acos((trace - 1.0) / 2.0)
theta_sqred = math.pow(theta, 2.0)
R_transpose = np.transpose(R)
ln_R = I_3
# TODO use Taylor Expansion when theta is small
if not theta == 0:
ln_R = (theta / (2 * math.sin(theta))) * (R - R_transpose)
w[3] = ln_R[2, 1]
w[4] = ln_R[0, 2]
w[5] = ln_R[1, 0]
w_x = Utils.skew_symmetric(w[3], w[4], w[5])
w_x_squared = np.matmul(w_x, w_x)
A = 0
B = 0
coeff = 0
# TODO use Taylor Expansion when theta_sqred is small
if not theta == 0:
A = math.sin(theta) / theta
if not theta_sqred == 0:
B = (1 - math.cos(theta)) / theta_sqred
if not (theta == 0 or theta_sqred == 0):
coeff = (1.0 / (theta_sqred)) * (1.0 - (A / (2.0 * B)))
V_inv = I_3 + np.multiply(0.5, w_x) + np.multiply(coeff, w_x_squared)
u = np.matmul(V_inv, t)
w[0] = u[0]
w[1] = u[1]
w[2] = u[2]
return w
def render(self):
(height, width) = self.resolution
for x in range(0, width):
for y in range(0, height):
ray_direction_camera_space = self.camera.camera_ray_direction_camera_space(
x, y, width, height, self.fov)
camera_to_world = self.camera.se3_inv
rot = SE3.extract_rotation(camera_to_world)
ray_world_space = Utils.normalize(
np.matmul(rot, ray_direction_camera_space))
ray = Geometry.Ray(self.camera.origin_ws[0:3], ray_world_space)
(b, t, sphere) = self.find_closest_intersection(ray)
if sphere.is_intersection_acceptable(b, t):
intersection_point = Geometry.point_for_ray(ray, t)
normal = sphere.normal_for_point(intersection_point)
depth = intersection_point[2]
self.depth_buffer[y, x] = fabs(depth)
self.frame_buffer[y, x] = phong_shading(
self.light_ws, intersection_point, normal)
def phong_shading(light_ws, position_ws, normal):
view = Utils.normalize(light_ws - position_ws)
return Utils.fast_dot(view, normal)
def solve_SE3(X, Y, max_its, eps):
# init
# array for twist values x, y, z, roll, pitch, yaw
t_est = np.array([0, 0, 0], dtype=matrix_data_type).reshape((3, 1))
R_est = np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1]], dtype=matrix_data_type)
I_3 = np.identity(3, dtype=matrix_data_type)
(position_vector_size, N) = X.shape
twist_size = 6
stacked_obs_size = position_vector_size * N
homogeneous_se3_padding = Utils.homogenous_for_SE3()
L_mean = -1
it = -1
# Step Factor
alpha = 0.125
SE_3_est = np.append(np.append(R_est, t_est, axis=1),
Utils.homogenous_for_SE3(),
axis=0)
generator_x = Lie.generator_x()
generator_y = Lie.generator_y()
generator_z = Lie.generator_z()
generator_roll = Lie.generator_roll()
generator_pitch = Lie.generator_pitch()
generator_yaw = Lie.generator_yaw()
for it in range(0, max_its, 1):
# accumulators
J_v = np.zeros((twist_size, 1))
normal_matrix = np.zeros((twist_size, twist_size))
Y_est = np.matmul(SE_3_est, X)
v = Y_est - Y
L = np.sum(np.square(v), axis=0)
L_mean = np.mean(L)
if L_mean < eps:
print('done')
break
Js = JacobianGenerator.get_jacobians_lie(generator_x,
generator_y,
generator_z,
generator_yaw,
generator_pitch,
generator_roll,
Y_est,
N,
stacked_obs_size,
coefficient=2.0)
for i in range(0, N, 1):
J = Js[i]
J_t = np.transpose(J)
error_vector = np.reshape(v[:, i], (position_vector_size, 1))
J_v += np.matmul(-J_t, error_vector)
normal_matrix += np.matmul(J_t, J)
##########################################################
# TODO: Investigate faster inversion with QR
try:
pseudo_inv = linalg.inv(normal_matrix)
# (Q,R) = linalg.qr(normal_matrix_2)
except:
print('Cant invert')
return SE_3_est
w = np.matmul(pseudo_inv, J_v)
# Apply Step Factor
w = alpha * w
w_transpose = np.transpose(w)
w_x = Utils.skew_symmetric(w[3], w[4], w[5])
w_x_squared = np.matmul(w_x, w_x)
# closed form solution for exponential map
theta = math.sqrt(np.matmul(w_transpose, w))
theta_sqred = math.pow(theta, 2)
# TODO use Taylor Expansion when theta_sqred is small
try:
A = math.sin(theta) / theta
B = (1 - math.cos(theta)) / theta_sqred
C = (1 - A) / theta_sqred
except:
print('bad theta')
return SE_3_est
u = np.array([w[0], w[1], w[2]]).reshape((3, 1))
R_new = I_3 + np.multiply(A, w_x) + np.multiply(B, w_x_squared)
V = I_3 + np.multiply(B, w_x) + np.multiply(C, w_x_squared)
t_est += +np.matmul(V, u)
R_est = np.matmul(R_new, R_est)
SE_3_est = np.append(np.append(R_est, t_est, axis=1),
homogeneous_se3_padding,
axis=0)
print('Runtime mean error:', L_mean)
print('mean error:', L_mean, 'iteration: ', it)
return SE_3_est
def solve_photometric(frame_reference,
frame_target,
max_its,
eps,
alpha_step,
use_ndc=False,
debug=False):
# init
# array for twist values x, y, z, roll, pitch, yaw
t_est = np.array([0, 0, 0], dtype=matrix_data_type).reshape((3, 1))
#R_est = np.array([[0.0, -1.0, 0],
# [1.0, 0.0, 0],
# [0, 0, 1]], dtype=matrix_data_type)
R_est = np.identity(3, dtype=matrix_data_type)
I_3 = np.identity(3, dtype=matrix_data_type)
(height, width) = frame_target.pixel_image.shape
N = height * width
position_vector_size = 3
twist_size = 6
stacked_obs_size = position_vector_size * N
homogeneous_se3_padding = Utils.homogenous_for_SE3()
# Step Factor
#alpha = 0.125
Gradient_step_manager = GradientStepManager.GradientStepManager(
alpha_start=alpha_step,
alpha_min=-0.7,
alpha_step=-0.01,
alpha_change_rate=0,
gradient_monitoring_window_start=3,
gradient_monitoring_window_size=0)
v_mean = -10000
v_mean_abs = -10000
it = -1
std = math.sqrt(0.4)
image_range_offset = 10
#depth_factor = 1.0
#depth_factor = 1000 # 0.001 # ZR300
SE_3_est = np.append(np.append(R_est, t_est, axis=1),
Utils.homogenous_for_SE3(),
axis=0)
SE_3_est_orig = np.append(np.append(R_est, t_est, axis=1),
Utils.homogenous_for_SE3(),
axis=0)
SE_3_est_last_valid = np.append(np.append(R_est, t_est, axis=1),
Utils.homogenous_for_SE3(),
axis=0)
generator_x = Lie.generator_x_3_4()
generator_y = Lie.generator_y_3_4()
generator_z = Lie.generator_z_3_4()
generator_roll = Lie.generator_roll_3_4()
generator_pitch = Lie.generator_pitch_3_4()
generator_yaw = Lie.generator_yaw_3_4()
X_back_projection = np.ones((4, N), Utils.matrix_data_type)
X_back_projection_last_valid = np.ones((4, N), Utils.matrix_data_type)
valid_measurements_reference = np.full(N, False)
valid_measurements_last = np.full(N, False)
valid_measurements_target = np.full(N, False)
valid_measurements = valid_measurements_reference
# Precompute back projection of pixels
GaussNewtonRoutines.back_project_image(width, height, image_range_offset,
frame_reference.camera,
frame_reference.pixel_depth,
X_back_projection,
valid_measurements, use_ndc)
if debug:
Plot3D.save_projection_of_back_projected(height, width,
frame_reference,
X_back_projection)
# Precompute the Jacobian of SE3 around the identity
J_lie = JacobianGenerator.get_jacobians_lie(generator_x,
generator_y,
generator_z,
generator_yaw,
generator_pitch,
generator_roll,
X_back_projection,
N,
stacked_obs_size,
coefficient=2.0)
# Precompute the Jacobian of the projection function
J_pi = JacobianGenerator.get_jacobian_camera_model(
frame_reference.camera.intrinsic, X_back_projection)
# count the number of true
#valid_measurements_total = np.logical_and(valid_measurements_reference,valid_measurements_target)
#number_of_valid_reference = np.sum(valid_measurements_reference)
#number_of_valid_total = np.sum(valid_measurements_total)
#number_of_valid_measurements = number_of_valid_reference
for it in range(0, max_its, 1):
start = time.time()
# accumulators
#TODO: investigate preallocate and clear in a for loop
J_v = np.zeros((twist_size, 1))
normal_matrix = np.zeros((twist_size, twist_size))
# Warp with the current SE3 estimate
Y_est = np.matmul(SE_3_est, X_back_projection)
v = np.zeros((N, 1), dtype=matrix_data_type, order='F')
target_index_projections = frame_target.camera.apply_perspective_pipeline(
Y_est)
v_sum = GaussNewtonRoutines.compute_residual(
width, height, target_index_projections, valid_measurements,
frame_target.pixel_image, frame_reference.pixel_image, v,
image_range_offset)
number_of_valid_measurements = np.sum(valid_measurements_reference)
Gradient_step_manager.save_previous_mean_error(v_mean_abs, it)
v_mean = v_sum / number_of_valid_measurements
valid_pixel_ratio = number_of_valid_measurements / N
#v_mean_abs = np.abs(v_mean)
#v_mean_abs = v_mean
# TODO put this in gradient step manager
#if valid_pixel_ratio< 0.8:
# print('Too many pixels are marked invalid')
# Gradient_step_manager.current_alpha+=0.1
# SE_3_est = SE_3_est_last_valid
# valid_measurements = valid_measurements_last
#else:
# SE_3_est_last_valid = SE_3_est
# valid_measurements_last = valid_measurements
Gradient_step_manager.track_gradient(v_mean_abs, it)
if v_mean < eps:
print('done, mean error:', v_mean)
break
Gradient_step_manager.analyze_gradient_history(it)
#Gradient_step_manager.analyze_gradient_history_instantly(v_mean_abs)
# See Kerl et al. ensures error decreases ( For pyramid levels )
#if(v_mean > Gradient_step_manager.last_error_mean_abs):
#continue
GaussNewtonRoutines.gauss_newton_step(width, height,
valid_measurements, J_pi, J_lie,
frame_target.grad_x,
frame_target.grad_y, v, J_v,
normal_matrix,
image_range_offset)
# TODO: Investigate faster inversion with QR
try:
pseudo_inv = linalg.inv(normal_matrix)
#(Q,R) = linalg.qr(normal_matrix)
#Q_t = np.transpose(Q)
#R_inv = linalg.inv(R)
#pseudo_inv = np.multiply(R_inv,Q_t)
except:
print('Cant invert')
return SE_3_est
w = np.matmul(pseudo_inv, J_v)
# Apply Step Factor
w = Gradient_step_manager.current_alpha * w
w_transpose = np.transpose(w)
w_x = Utils.skew_symmetric(w[3], w[4], w[5])
w_x_squared = np.matmul(w_x, w_x)
# closed form solution for exponential map
theta = math.sqrt(np.matmul(w_transpose, w))
theta_sqred = math.pow(theta, 2)
# TODO use Taylor Expansion when theta_sqred is small
try:
A = math.sin(theta) / theta
B = (1 - math.cos(theta)) / theta_sqred
C = (1 - A) / theta_sqred
except:
print('bad theta')
return SE_3_est
u = np.array([w[0], w[1], w[2]]).reshape((3, 1))
R_new = I_3 + np.multiply(A, w_x) + np.multiply(B, w_x_squared)
V = I_3 + np.multiply(B, w_x) + np.multiply(C, w_x_squared)
t_est += +np.matmul(V, u)
R_est = np.matmul(R_new, R_est)
SE_3_est = np.append(np.append(R_est, t_est, axis=1),
homogeneous_se3_padding,
axis=0)
end = time.time()
print('mean error:', v_mean, 'iteration: ', it, 'valid pixel ratio: ',
valid_pixel_ratio, 'runtime: ', end - start)
return SE_3_est
def adjoint_se3(R, t):
t_x = Utils.skew_symmetric(t[0], t[1], t[2])
top = np.append(R, np.matmul(t_x, R), axis=1)
bottom = np.append(np.identity(3), R, axis=1)
return np.append(top, bottom, axis=0)
def __init__(self,
ground_truth_list=None,
plot_steering=False,
title=None,
plot_trajectory=True,
plot_rmse=True):
self.ground_truth_list = []
if ground_truth_list is not None:
self.ground_truth_list = ground_truth_list
self.figure = plt.figure()
self.plot_steering = plot_steering
self.plot_trajectory = plot_trajectory
self.plot_rmse = plot_rmse
if title:
plt.title(title)
if not plot_trajectory and not plot_rmse:
if not self.plot_steering:
self.x_graph = self.figure.add_subplot(131)
self.y_graph = self.figure.add_subplot(132)
self.z_graph = self.figure.add_subplot(133)
else:
self.x_graph = self.figure.add_subplot(231)
self.y_graph = self.figure.add_subplot(232)
self.z_graph = self.figure.add_subplot(233)
self.rev = self.figure.add_subplot(223)
self.steer = self.figure.add_subplot(224)
elif not plot_steering:
if plot_trajectory:
self.se3_graph = self.figure.add_subplot(311, projection='3d')
self.x_graph = self.figure.add_subplot(334)
self.y_graph = self.figure.add_subplot(335)
self.z_graph = self.figure.add_subplot(336)
if self.plot_rmse:
self.rmse_graph = self.figure.add_subplot(313)
else:
if plot_trajectory:
self.se3_graph = self.figure.add_subplot(411, projection='3d')
self.x_graph = self.figure.add_subplot(434)
self.y_graph = self.figure.add_subplot(435)
self.z_graph = self.figure.add_subplot(436)
if self.plot_rmse:
self.rmse_graph = self.figure.add_subplot(413)
self.rev = self.figure.add_subplot(427)
self.steer = self.figure.add_subplot(428)
if plot_steering:
self.rev.set_title("rev cmd")
self.steer.set_title("str cmd")
self.rev.set_xlabel("frame")
self.steer.set_xlabel("frame")
self.rev.set_ylabel("input level")
self.steer.set_ylabel("input level")
#self.se3_graph.set_aspect('equal')
if self.plot_trajectory:
self.se3_graph.set_title("relative pose estimate")
self.x_graph.set_title("X")
self.x_graph.set_title("X")
self.y_graph.set_title("Y")
self.z_graph.set_title("Z")
self.x_graph.set_xlabel("frame")
self.y_graph.set_xlabel("frame")
self.z_graph.set_xlabel("frame")
self.x_graph.set_ylabel("meters")
if self.plot_rmse:
self.rmse_graph.set_title("drift per frame (pose error)")
# These points will be plotted with incomming se3 matricies
X, Y, Z = [0, 0], [0, 0], [0, -1]
#X, Y, Z = [0, 0], [0, 0], [0, 1]
H = np.repeat(1, 2)
self.point_pair = Utils.to_homogeneous_positions(X, Y, Z, H)
if self.plot_steering:
plt.subplots_adjust(left=0.07)
else:
plt.subplots_adjust(left=0.05)
def normal_for_point(self,point_on_shpere):
return Utils.normalize(point_on_shpere - self.origin)
from MotionModels import Ackermann, SteeringCommand, MotionDeltaRobot, Pose import numpy as np from Numerics import Utils, SE3 import matplotlib.pyplot as plt from Visualization import Plot3D X, Y, Z = [0,0], [0,0], [0,-1] H = np.repeat(1,2) origin = Utils.to_homogeneous_positions(X, Y, Z, H) dt = 1.0 pose = Pose.Pose() steering_command_straight = SteeringCommand.SteeringCommands(1.0, 0.0) ackermann_motion = Ackermann.Ackermann() new_motion_delta = ackermann_motion.ackermann_dead_reckoning_delta(steering_command_straight) pose.apply_motion(new_motion_delta,dt) #TODO investigate which theta to use # this might actually be better since we are interested in the uncertainty only in this timestep theta = new_motion_delta.delta_theta # traditional uses accumulated theta #theta = pose.theta motion_cov_small = ackermann_motion.covariance_dead_reckoning(steering_command_straight,theta,dt) se3 = SE3.generate_se3_from_motion_delta(new_motion_delta) origin_transformed = np.matmul(se3, origin)
import matplotlib.pyplot as plt from Visualization import Plot3D from Numerics import Generator, Utils import numpy as np import math #X, Y, Z = [0,0,10,10] , [0,0,10,10] , [0,-1,0,-1] X, Y, Z = [0, 0], [0, 0], [0, -1] H = np.repeat(1, 2) pair = Utils.to_homogeneous_positions(X, Y, Z, H) se3 = Generator.generate_random_se3(2, 2, math.pi, math.pi / 2, 0, 0) se3_2 = Generator.generate_random_se3(2, 2, math.pi, math.pi / 2, 0, 0) pair_transformed = np.matmul(se3, pair) pair_transformed_2 = np.matmul(se3_2, pair) points = np.append(pair, pair_transformed, axis=1) points_xyz = points[0:3, :] fig = plt.figure() ax = fig.add_subplot(111, projection='3d') Plot3D.plot_array_lines(points_xyz, ax) pair_transformed = np.matmul(se3, pair)
import Numerics.Generator as Generator
import Visualization.Plot3D as Plot3D
import numpy as np
import Numerics.Utils as NumUtils
(X, Y, Z) = Generator.generate_3d_plane(1, 1, -30, 20, 4)
H = np.repeat(1, 20)
points = np.transpose(
np.array(list(map(lambda x: list(x), list(zip(X, Y, Z, H))))))
SE3 = Generator.generate_random_se3(-1, 1, 4)
rotated_points = np.matmul(SE3, points)
(X_new, Y_new, Z_new) = list(NumUtils.points_into_components(rotated_points))
#Plot3D.scatter_plot(X_new,Y_new,Z_new)
Plot3D.scatter_plot([(X, Y, Z), (X_new, Y_new, Z_new)],
['original', 'perturbed'])
N = 20
(X, Y, Z) = Generator.generate_3d_plane(1, 1, -30, N, 4)
H = np.repeat(1, N)
points = np.transpose(
np.array(list(map(lambda x: list(x), list(zip(X, Y, Z, H))))))
SE3 = Generator.generate_random_se3(-5, 5, pi / 18, pi / 10, 0, pi / 10)
perturbed_points_gt = np.matmul(SE3, points)
camera = Camera.normalized_camera()
points_persp = camera.apply_perspective_pipeline(points)
#SE3_est = Solver.solve_SE3(points,perturbed_points_gt,20000,0.01)
#perturbed_points_est = np.matmul(SE3_est,points)
(X_orig, Y_orig, Z_orig) = list(Utils.points_into_components(points))
(X_new, Y_new, Z_new) = list(Utils.points_into_components(perturbed_points_gt))
#(X_est,Y_est,Z_est) = list(Utils.points_into_components(perturbed_points_est))
(X_persp, Y_persp, Z_persp) = list(Utils.points_into_components(points_persp))
#Plot3D.scatter_plot([(X_orig,Y_orig,Z_orig),(X_new,Y_new,Z_new),(X_est,Y_est,Z_est)],['original','perturbed','estimate'])
Plot3D.scatter_plot_sub([(X_orig, Y_orig, Z_orig)],
[(X_persp, Y_persp, Z_persp)], ['original'],
['projected'])
import Numerics.Generator as Generator
import Visualization.Plot3D as Plot3D
import numpy as np
import Numerics.Utils as NumUtils
import VisualOdometry.Solver as Solver
from math import pi
N = 100
(X, Y, Z) = Generator.generate_3d_plane(1, 1, -10, N, 4)
H = np.repeat(1, N)
points = np.transpose(
np.array(list(map(lambda x: list(x), list(zip(X, Y, Z, H))))))
SE3 = Generator.generate_random_se3(-10, 10, 4, pi / 2, 0, pi / 2)
rotated_points_gt = np.matmul(SE3, points)
(X_gt, Y_gt, Z_gt) = list(NumUtils.points_into_components(rotated_points_gt))
SE3_est = Solver.solve_SE3(points, rotated_points_gt, 20000, 0.01)
rotated_points_est = np.matmul(SE3_est, points)
(X_est, Y_est,
Z_est) = list(NumUtils.points_into_components(rotated_points_est))
Plot3D.scatter_plot([(X, Y, Z), (X_gt, Y_gt, Z_gt), (X_est, Y_est, Z_est)],
['original', 'ground truth', 'estimate'])
image_height = 320
points = np.transpose(
np.array(list(map(lambda x: list(x), list(zip(X, Y, Z, H))))))
spheres = Geometry.generate_spheres(points)
camera = Camera.normalized_camera(0, 0, image_width / 2, image_height / 2)
camera_translated = Camera.normalized_camera(5, 0, image_width / 2,
image_height / 2)
##############
points_persp = camera.apply_perspective_pipeline(points)
(X_orig, Y_orig, Z_orig) = list(Utils.points_into_components(points))
(X_persp, Y_persp, Z_persp) = list(Utils.points_into_components(points_persp))
##############
scene = Scene.Scene(image_width, image_height, spheres, camera)
scene.render()
frame_buffer_image = ImageProcessing.normalize_to_image_space(
scene.frame_buffer)
depth_buffer_image = scene.depth_buffer
cv2.imwrite("framebuffer.png", frame_buffer_image)
cv2.imwrite("depthbuffer.png", depth_buffer_image)
def camera_ray_direction_camera_space(self, pixel_x, pixel_y, width,
height, fov):
(camera_x, camera_y) = self.pixel_to_camera(pixel_x, pixel_y, width,
height, fov)
ray_cs = np.array([camera_x, camera_y, -1]).reshape(3, 1)
return Utils.normalize(ray_cs)
from MotionModels import Ackermann, SteeringCommand, MotionDeltaRobot, Pose from Numerics import Utils, SE3 import matplotlib.pyplot as plt from Visualization import Plot3D import numpy as np import math X, Y, Z = [0,0], [0,0], [0,-1] H = np.repeat(1,2) origin = Utils.to_homogeneous_positions(X, Y, Z, H) dt = 1.0 pose = Pose.Pose() steering_command_straight = SteeringCommand.SteeringCommands(1.5, 0.0) ackermann_motion = Ackermann.Ackermann() new_motion_delta = ackermann_motion.ackermann_dead_reckoning_delta(steering_command_straight) pose.apply_motion(new_motion_delta,dt) pose.apply_motion(new_motion_delta,dt) pose.apply_motion(new_motion_delta,dt) #TODO investigate which theta to use # this might actually be better since we are interested in the uncertainty only in this timestep #theta = new_motion_delta.delta_theta # traditional uses accumulated theta theta = pose.theta motion_cov_small = ackermann_motion.covariance_dead_reckoning(steering_command_straight,theta,dt)
def twist_to_SE3(twist):
R, t = Lie.exp(twist, 6)
SE3 = np.append(np.append(R, t, axis=1),
Utils.homogenous_for_SE3(),
axis=0)
return SE3
# We only need the gradients of the target frame
frame_reference = Frame.Frame(im_greyscale_reference, depth_reference,
camera_reference, False)
frame_target = Frame.Frame(im_greyscale_target, depth_target, camera_target,
True)
#visualizer = Visualizer.Visualizer(photometric_solver)
SE3_est = Solver.solve_photometric(frame_reference,
frame_target,
threadLock=None,
pose_estimate_list=None,
max_its=20000,
eps=0.001,
alpha_step=1.0,
gradient_monitoring_window_start=5.0,
image_range_offset_start=0,
twist_prior=None,
hessian_prior=None,
use_ndc=use_ndc,
use_robust=True,
track_pose_estimates=False,
debug=False)
euler_angles_XYZ = SE3.rotationMatrixToEulerAngles(
SE3.extract_rotation(SE3_est))
print(SE3_est)
print(Utils.radians_to_degrees(euler_angles_XYZ[0]),
Utils.radians_to_degrees(euler_angles_XYZ[1]),
Utils.radians_to_degrees(euler_angles_XYZ[2]))
def get_standard_deviation_factors_from_covaraince_list(cov_list):
evd_list = Utils.covariance_eigen_decomp_for_list(cov_list)
eigen_value_list = Utils.eigen_values_from_evd_list(evd_list)
return get_standard_deviation_factors_for_projection_for_list(
eigen_value_list)
def solve_photometric(frame_reference,
frame_target,
threadLock,
pose_estimate_list,
max_its,
eps,
alpha_step,
gradient_monitoring_window_start,
image_range_offset_start,
max_depth,
twist_prior=None,
motion_cov_inv_in=None,
use_ndc=False,
use_robust=False,
track_pose_estimates=False,
use_motion_prior=False,
ackermann_pose_prior=None,
use_ackermann=False,
debug=False):
if track_pose_estimates and (threadLock == None
or pose_estimate_list == None):
raise RuntimeError(
'Visualization Flag is set, but no list and lock are supplied')
# init
# array for twist values x, y, z, roll, pitch, yaw
t_est = np.array([0, 0, 0], dtype=matrix_data_type).reshape((3, 1))
#R_est = np.array([[0.0, -1.0, 0],
# [1.0, 0.0, 0],
# [0, 0, 1]], dtype=matrix_data_type)
R_est = np.identity(3, dtype=matrix_data_type)
I_3 = np.identity(3, dtype=matrix_data_type)
I_4 = np.identity(4, dtype=matrix_data_type)
I_6 = np.identity(6, dtype=matrix_data_type)
zero_cov = np.zeros((6, 6), dtype=matrix_data_type)
#SE3_best = np.identity(4,dtype=matrix_data_type)
(height, width) = frame_target.pixel_image.shape
N = height * width
position_vector_size = 3
twist_size = 6
stacked_obs_size = position_vector_size * N
homogeneous_se3_padding = Utils.homogenous_for_SE3()
variance = -1
v_mean = maxsize
image_range_offset = image_range_offset_start
degrees_of_freedom = 5.0 # empirically derived: see paper
normal_matrix_ret = np.identity(6, dtype=Utils.matrix_data_type)
motion_cov_inv = motion_cov_inv_in
#motion_cov_inv = np.linalg.inv(motion_cov_inv_in)
w = np.zeros((twist_size, 1), dtype=Utils.matrix_data_type)
w_empty = np.zeros((twist_size, 1), dtype=Utils.matrix_data_type)
w_prev = np.zeros((twist_size, 1), dtype=Utils.matrix_data_type)
w_acc = np.zeros((twist_size, 1), dtype=Utils.matrix_data_type)
v_id = np.zeros((N, 1), dtype=matrix_data_type, order='F')
pseudo_inv = np.identity(twist_size, dtype=matrix_data_type)
not_better = False
valid_pixel_ratio = 1.0
motion_cov_inv_norm = Utils.norm_covariance_row(motion_cov_inv_in)
fx = frame_reference.camera.intrinsic.extract_fx()
fy = frame_reference.camera.intrinsic.extract_fy()
depth_factor = np.sign(fx)
#depth_factor = -np.sign(fx)
Gradient_step_manager = GradientStepManager.GradientStepManager(
alpha_start=alpha_step,
alpha_min=-0.7,
alpha_step=-0.01,
alpha_change_rate=0,
gradient_monitoring_window_start=gradient_monitoring_window_start,
gradient_monitoring_window_size=0)
SE_3_est = np.append(np.append(R_est, t_est, axis=1),
Utils.homogenous_for_SE3(),
axis=0)
SE_3_prev = np.append(np.append(R_est, t_est, axis=1),
Utils.homogenous_for_SE3(),
axis=0)
#SE_3_est_orig = np.append(np.append(R_est, t_est, axis=1), Utils.homogenous_for_SE3(), axis=0)
#SE_3_est_last_valid = np.append(np.append(R_est, t_est, axis=1), Utils.homogenous_for_SE3(), axis=0)
generator_x = Lie.generator_x_3_4()
#generator_x = Lie.generator_x_3_4_neg()
generator_y = Lie.generator_y_3_4()
#generator_y = Lie.generator_y_3_4_neg()
#generator_z = Lie.generator_z_3_4()
generator_z = Lie.generator_z_3_4_neg()
# Depth factor of -1.0 leads to inverted roll and pitch when displaying
# Why?: Generator defines the direction of increase (My thoughts)
generator_roll = Lie.generator_roll_3_4()
#generator_roll = Lie.generator_roll_3_4_neg()
#generator_pitch = Lie.generator_pitch_3_4()
generator_pitch = Lie.generator_pitch_3_4_neg()
generator_yaw = Lie.generator_yaw_3_4()
#generator_yaw = Lie.generator_yaw_3_4_neg()
X_back_projection = depth_factor * np.ones((4, N), Utils.matrix_data_type)
X_back_projection[3, :] = 1.0
#X_back_projection_last_valid = np.ones((4, N), Utils.matrix_data_type)
valid_measurements_reference = np.full(N, False)
valid_measurements_target = np.full(N, False)
#valid_measurements_last = np.full(N,False)
#valid_measurements_target = np.full(N,False)
valid_measurements = valid_measurements_reference
number_of_valid_measurements = N
#v = np.zeros((N, 1), dtype=matrix_data_type, order='F')
# Precompute back projection of pixels
GaussNewtonRoutines.back_project_image(
width, height, image_range_offset, frame_reference.camera,
frame_reference.pixel_depth, frame_target.pixel_depth,
X_back_projection, valid_measurements, valid_measurements_target,
use_ndc, depth_factor, max_depth)
count = np.sum(valid_measurements)
count_target = np.sum(valid_measurements_target)
z_rot = SE3.makeS03(0, 0, math.pi)
se3_rot = np.identity(4, dtype=matrix_data_type)
se3_rot[0:3, 0:3] = z_rot
#X_back_projection = np.matmul(se3_rot,X_back_projection)
if debug:
Plot3D.save_projection_of_back_projected(height, width,
frame_reference,
X_back_projection)
# Precompute the Jacobian of SE3 around the identity
J_lie = JacobianGenerator.get_jacobians_lie(generator_x,
generator_y,
generator_z,
generator_yaw,
generator_pitch,
generator_roll,
X_back_projection,
N,
stacked_obs_size,
coefficient=1.0)
# Precompute the Jacobian of the projection function
J_pi = JacobianGenerator.get_jacobian_camera_model(
frame_reference.camera.intrinsic, X_back_projection)
# count the number of true
#valid_measurements_total = np.logical_and(valid_measurements_reference,valid_measurements_target)
#number_of_valid_reference = np.sum(valid_measurements_reference)
#number_of_valid_total = np.sum(valid_measurements_total)
#number_of_valid_measurements = number_of_valid_reference
#target_index_projections_id = frame_target.camera.apply_perspective_pipeline(I_4)
target_index_projections = frame_target.camera.apply_perspective_pipeline(
X_back_projection, use_ndc, width, height)
GaussNewtonRoutines.compute_residual(
width, height, target_index_projections, valid_measurements,
valid_measurements_target, frame_target.pixel_image,
frame_reference.pixel_image, frame_target.pixel_depth,
frame_reference.pixel_depth, v_id, image_range_offset)
v = np.copy(v_id)
W = np.ones((1, N), dtype=matrix_data_type, order='F')
for it in range(0, max_its, 1):
start = time.time()
# accumulators
#TODO: investigate preallocate and clear in a for loop
g = np.zeros((twist_size, 1))
normal_matrix = np.identity(twist_size, dtype=matrix_data_type)
# TODO investigate performance impact
if track_pose_estimates:
threadLock.acquire()
pose_estimate_list.append(SE_3_est)
threadLock.release()
#v_diff = math.fabs(Gradient_step_manager.last_error_mean_abs - v_mean)
#v_diff = Gradient_step_manager.last_error_mean_abs - v_mean
#Gradient_step_manager.track_gradient(v_mean,it)
# TODO investigate absolute error threshold aswel?
#if ((v_diff <= eps)) and Gradient_step_manager.check_iteration(it) :
# print('done, mean error:', v_mean, 'diff: ', v_diff, 'pixel ratio:', valid_pixel_ratio)
# break
# no if statement means solver 2
#if v_mean <= Gradient_step_manager.last_error_mean_abs:
not_better = False
prior_empty = False
if twist_prior[0] == 0 and twist_prior[1] == 0 and twist_prior[2] == 0 and twist_prior[3] == 0 and \
twist_prior[4] == 0 and twist_prior[5] == 0:
prior_empty = True
if use_motion_prior:
converged = GaussNewtonRoutines.gauss_newton_step_motion_prior(
width, height, valid_measurements, valid_measurements_target,
W, J_pi, J_lie, frame_target.grad_x, frame_target.grad_y, v, g,
normal_matrix, motion_cov_inv, twist_prior, w,
image_range_offset)
else:
converged = GaussNewtonRoutines.gauss_newton_step(
width, height, valid_measurements, valid_measurements_target,
W, J_pi, J_lie, frame_target.grad_x, frame_target.grad_y, v, g,
normal_matrix, image_range_offset)
normal_matrix_ret = normal_matrix
#try:
# pseudo_inv = linalg.inv(normal_matrix)
#except:
# print('Cant invert')
# return SE_3_est
#w_new = np.matmul(pseudo_inv, g)
try:
w_new = linalg.solve(normal_matrix, g)
except:
print('Cant solve')
return SE_3_est
# initial step with empty motion prior seems to be quite large
#if use_motion_prior and prior_empty:
# w_new = np.multiply(Gradient_step_manager.current_alpha/2.0, w_new)
#else:
w_new = np.multiply(Gradient_step_manager.current_alpha, w_new)
# For using ackermann motion
if use_ackermann:
# V1
# inc = ackermann_pose_prior - w
# w_new += np.matmul(motion_cov_inv,inc)
# w_new += inc
# V2
#factor = 0.1*Gradient_step_manager.current_alpha
factor = 0.1
#factor = math.pow(Gradient_step_manager.current_alpha,it)
# ack_prior = np.multiply(Gradient_step_manager.current_alpha,ackermann_pose_prior)
ack_prior = ackermann_pose_prior
w_new += Lie.lie_ackermann_correction(factor, motion_cov_inv,
ack_prior, w, twist_size)
#else:
# not_better = True
# w_new = w_empty
R_cur, t_cur = Lie.exp(w, twist_size)
R_new, t_new = Lie.exp(w_new, twist_size)
# C_new . C_cur
#t_est = np.add(np.matmul(R_new, t_cur), t_new)
#R_est = np.matmul(R_new, R_cur)
# C_Cur . C_new
t_est = np.add(np.matmul(R_cur, t_new), t_cur)
R_est = np.matmul(R_cur, R_new)
w = Lie.ln(R_est, t_est, twist_size)
#SE_3_current = np.append(np.append(R_cur, t_cur, axis=1), homogeneous_se3_padding, axis=0)
SE_3_est = np.append(np.append(R_est, t_est, axis=1),
homogeneous_se3_padding,
axis=0)
#debug_list = [i for i, x in enumerate(valid_measurements) if x]
Y_est = np.matmul(SE_3_est, X_back_projection)
target_index_projections = frame_target.camera.apply_perspective_pipeline(
Y_est, use_ndc, width, height)
#target_index_projections[2,:] -= depth_factor*1
v = GaussNewtonRoutines.compute_residual(
width, height, target_index_projections, valid_measurements,
valid_measurements_target, frame_target.pixel_image,
frame_reference.pixel_image, frame_target.pixel_depth,
frame_reference.pixel_depth, v, image_range_offset)
number_of_valid_measurements = np.sum(valid_measurements)
valid_pixel_ratio = number_of_valid_measurements / N
if number_of_valid_measurements <= 0 and Gradient_step_manager.check_iteration(
it):
print('pixel ratio break')
print('done, mean error:', v_mean, 'diff: ', v_diff,
'pixel ratio:', valid_pixel_ratio)
#SE_3_est = SE3_best
break
if use_robust:
variance = GaussNewtonRoutines.compute_t_dist_variance(
v,
degrees_of_freedom,
N,
valid_measurements,
valid_measurements_target,
number_of_valid_measurements,
variance_min=1000,
eps=0.001)
if variance > 0.0:
# clear old weights
for i in range(0, N):
W[0, i] = 1
GaussNewtonRoutines.generate_weight_matrix(
W, v, variance, degrees_of_freedom, N)
v_weighted = np.copy(v)
GaussNewtonRoutines.multiply_v_by_diagonal_matrix(
W, v_weighted, N, valid_measurements)
v_sum = np.matmul(np.transpose(v), v_weighted)[0][0]
end = time.time()
#if v_mean < Gradient_step_manager.last_error_mean_abs:
# SE3_best = np.copy(SE_3_est)
#if not not_better: # solver 6
Gradient_step_manager.save_previous_mean_error(v_mean)
if number_of_valid_measurements > 0:
v_mean = v_sum / number_of_valid_measurements
else:
v_mean = maxsize
v_diff = math.fabs(Gradient_step_manager.last_error_mean_abs - v_mean)
print('mean error:', v_mean, 'error diff: ', v_diff, 'iteration: ', it,
'valid pixel ratio: ', valid_pixel_ratio, 'runtime: ',
end - start, 'variance: ', variance)
#v_diff = Gradient_step_manager.last_error_mean_abs - v_mean
#Gradient_step_manager.track_gradient(v_mean,it)
# TODO investigate absolute error threshold aswel?
if ((v_diff <= eps)) and Gradient_step_manager.check_iteration(it):
print('done, mean error:', v_mean, 'diff: ', v_diff,
'pixel ratio:', valid_pixel_ratio)
break
motion_cov_inv = normal_matrix_ret
return SE_3_est, w, motion_cov_inv