def gauss_newton_step(width, height, valid_measurements, valid_measurements_target, W, J_pi, J_lie, target_image_grad_x, target_image_grad_y, v, g, normal_matrix_return, image_range_offset): convergence = False start = time.time() for y in range(image_range_offset, height - image_range_offset, 1): for x in range(image_range_offset, width - image_range_offset, 1): flat_index = matrix_to_flat_index_rows(y, x, height) if not valid_measurements[ flat_index] or not valid_measurements_target[flat_index]: continue J_image = get_jacobian_image(target_image_grad_x, target_image_grad_y, x, y) J_pi_element = J_pi[flat_index] J_lie_element = J_lie[flat_index] J_pi_lie = np.matmul(J_pi_element, J_lie_element) J_full = np.matmul(J_image, J_pi_lie) J_t = np.transpose(J_full) w_i = W[0, flat_index] error_sample = v[flat_index][0] g += np.multiply(w_i, np.multiply(-J_t, error_sample)) normal_matrix_return += np.multiply(w_i, np.matmul(J_t, J_full)) # different stopping criterion using max norm #if math.fabs(np.amax(g)< 0.001): #convergence = True end = time.time() #print('Runtime Gauss Newton Step:', end-start) return convergence
def compute_residual(width, height, target_index_projections, valid_measurements, target_image, reference_image, v, image_range_offset): v_sum = 0 start = time.time() for y in range(image_range_offset, height - image_range_offset, 1): for x in range(image_range_offset, width - image_range_offset, 1): flat_index = matrix_to_flat_index_rows(y, x, height) x_index = target_index_projections[0, flat_index] y_index = target_index_projections[1, flat_index] v[flat_index][0] = 0 if not 0 < y_index < height or not 0 < x_index < width: valid_measurements[flat_index] = False continue # A newer SE3 estimate might re-validate a sample / pixel valid_measurements[flat_index] = True x_target = math.floor(x_index) y_target = math.floor(y_index) error = target_image[y_target, x_target] - reference_image[y, x] error_sq = error * error v[flat_index][0] = error_sq v_sum += error_sq #v_sum = v_sum*v_sum # If the estimate is so bad that all measurements are invalid if v_sum == 0: v_sum = -1000 end = time.time() #print('Runtime for Compute Residual:', end-start) return v_sum
def compute_residual(width, height, target_index_projections, valid_measurements, valid_measurements_target, target_image, reference_image, target_depth, reference_depth, v, image_range_offset): v_sum = 0 start = time.time() for y in range(image_range_offset, height - image_range_offset, 1): for x in range(image_range_offset, width - image_range_offset, 1): flat_index = matrix_to_flat_index_rows(y, x, height) v[flat_index][0] = 0 #if true then invalid depth measurements are being considered if not valid_measurements[ flat_index] or not valid_measurements_target[flat_index]: continue x_index = target_index_projections[0, flat_index] y_index = target_index_projections[1, flat_index] if not (image_range_offset < y_index < height - image_range_offset) or not (image_range_offset < x_index < width - image_range_offset): # res no flag - these have to be disabled for R300 since the depth image is very sparse! valid_measurements[flat_index] = False continue # A newer SE3 estimate might re-validate a sample / pixel # TODO: investigate this flag in thesis # Might set invalid depth measurements to True such that they can contribute to the residual # res no flag - these have to be disabled for R300 since the depth image is very sparse! valid_measurements[flat_index] = True x_target = math.floor(x_index) y_target = math.floor(y_index) flat_index_target = matrix_to_flat_index_rows( y_target, x_target, height) #if not valid_measurements_target[flat_index_target]: # continue #error = target_image[y_target, x_target] - reference_image[y, x] #1 error = reference_image[y, x] - target_image[y_target, x_target] #2 v[flat_index][0] = error end = time.time() # print('Runtime for Compute Residual:', end-start) # return v_sum return v
def back_project_image(width, height, image_range_offset, reference_camera, reference_depth_image, target_depth_image, X_back_projection, valid_measurements, valid_measurements_target, use_ndc, depth_direction, max_depth): start = time.time() for y in range(0, height, 1): for x in range(0, width, 1): flat_index = matrix_to_flat_index_rows(y, x, height) depth = reference_depth_image[y, x] depth_target = target_depth_image[y, x] valid_measurements[flat_index] = True valid_measurements_target[flat_index] = True # For opencl maybe do this in a simple kernel before # Sets invalid depth measurements to False such that they do not impact the gauss newton step focal_m = reference_camera.intrinsic.extract_fx() / (1000.0 * width) if depth == 0: # this value directly influences the pose estimation! # TODO write about this #depth = depth_direction*(focal_m+max_depth) depth = depth_direction * (max_depth) #depth = depth_direction*1 valid_measurements[flat_index] = False #continue if depth_target == 0: valid_measurements_target[flat_index] = False depth_ref = depth #depending on the direction of the focal length, the depth sign has to be adjusted # Since our virtual image plane is on the same side as our depth values # we push all depth values out to guarantee that they are always infront of the image plane # Better depth results without pushing it out (?) #if valid_measurements[flat_index]: # depth_ref = depth_direction*(focal_m + depth) #depth_ref = depth_direction*(depth) # back projection from ndc seems to give better convergence x_back = x y_back = y if use_ndc: x_back /= width y_back /= height #X_back_projection[0:3, flat_index] = reference_camera.back_project_pixel(x, y, depth_ref)[:, 0] X_back_projection[ 0:3, flat_index] = reference_camera.back_project_pixel( x_back, y_back, depth_ref)[:, 0] end = time.time()
def gauss_newton_step_motion_prior(width, height, valid_measurements, valid_measurements_target, W, J_pi, J_lie, target_image_grad_x, target_image_grad_y, v, g, normal_matrix_return, motion_cov_inv, twist_prior, twist_prev, image_range_offset): convergence = False start = time.time() twist_delta = np.subtract(twist_prior, twist_prev) motion_prior = np.matmul(motion_cov_inv, twist_delta) for y in range(image_range_offset, height - image_range_offset, 1): for x in range(image_range_offset, width - image_range_offset, 1): flat_index = matrix_to_flat_index_rows(y, x, height) if not valid_measurements[ flat_index] or not valid_measurements_target[flat_index]: continue J_image = get_jacobian_image(target_image_grad_x, target_image_grad_y, x, y) J_pi_element = J_pi[flat_index] J_lie_element = J_lie[flat_index] J_pi_lie = np.matmul(J_pi_element, J_lie_element) J_full = np.matmul(J_image, J_pi_lie) J_t = np.transpose(J_full) w_i = W[0, flat_index] error_sample = v[flat_index][0] g += np.multiply(w_i, np.multiply(-J_t, error_sample)) normal_matrix_return += np.multiply(w_i, np.matmul(J_t, J_full)) # TODO: Can optimize this into one mult and 1 add per line #g += motion_prior #normal_matrix_return += motion_cov_inv # different stopping criterion using max norm #if math.fabs(np.amax(g)< 0.001): #convergence = True g += motion_prior normal_matrix_return += motion_cov_inv end = time.time() #print('Runtime Gauss Newton Step:', end-start) return convergence
def gauss_newton_step(width, height, valid_measurements, J_pi, J_lie, target_image_grad_x, target_image_grad_y, v, J_v_return, normal_matrix_return, image_range_offset): start = time.time() for y in range(image_range_offset, height - image_range_offset, 1): for x in range(image_range_offset, width - image_range_offset, 1): flat_index = matrix_to_flat_index_rows(y, x, height) if not valid_measurements[flat_index]: continue J_image = get_jacobian_image(target_image_grad_x, target_image_grad_y, x, y) J_pi_element = J_pi[flat_index] J_lie_element = J_lie[flat_index] J_pi_lie = np.matmul(J_pi_element, J_lie_element) J_full = np.matmul(J_image, J_pi_lie) J_t = np.transpose(J_full) error_vector = v[flat_index][0] J_v_return += np.multiply(error_vector, -J_t) normal_matrix_return += np.matmul(J_t, J_full) end = time.time()
def back_project_image(width, height, image_range_offset, reference_camera, reference_depth_image, X_back_projection, valid_measurements, use_ndc): start = time.time() for y in range(image_range_offset, height - image_range_offset, 1): for x in range(image_range_offset, width - image_range_offset, 1): flat_index = matrix_to_flat_index_rows(y, x, height) depth_ref = reference_depth_image[y, x] # For opencl maybe do this in a simple kernel before if depth_ref == 0: depth_ref = 1000 valid_measurements[flat_index] = False # back projection from ndc seems to give better convergence x_back = x y_back = y if use_ndc: x_back /= width y_back /= height #X_back_projection[0:3, flat_index] = reference_camera.back_project_pixel(x, y, depth_ref)[:, 0] X_back_projection[ 0:3, flat_index] = reference_camera.back_project_pixel( x_back, y_back, depth_ref)[:, 0] end = time.time()