如何在嵌入3D的表面上绘制测地曲线?

2020-02-07 javascript python geometry computational-geometry geogebra

我想到了这个视频或这个模拟 ,并且我想从某个起点在3D某种表面上重现测地线,由函数f(x,y)给出。

中点方法似乎在计算上和代码上都很密集,因此我想问一问是否有一种方法可以基于在不同点处的法线向量生成近似测地曲线。每个点都有一个与之相关的切向量空间,因此,似乎知道法线并不能确定向前移动曲线的特定方向。

我曾尝试过与Geogebra一起工作,但我意识到可能有必要转向其他软件平台,例如Python(或Poser?),Matlab或其他平台。

这个想法可行吗,我能获得一些有关如何实施的想法吗?


如果它提供了有关如何回答问题的一些想法,以前有一个答案(现在已消除了不幸),建议使用函数形式为z = F(x,y)的地形的中点方法,从之间的直线开始端点,在表面上分成几小段[假定XY平面(?)上的直线],然后提起[确定XY平面(?)上线段之间的节点]。接下来,它建议找到“一个中点” [我猜连接曲面上每对连续投影点对的线段的中点(?)],然后投影“它” [我想这些中点中的每一个都闭合,但不完全在[?(?)]]在表面上正交(在法线方向上),使用等式Z + t = F(X + t Fx,Y + t Fy)[我想这是一个意味着为零的点积...

在此处输入图片说明

(?)],其中(X,Y,Z)是中点的坐标,Fx,Fy是F的偏导数,而t是未知数[这是理解这一点的主要问题...我应该怎么做?我找到这个t了吗?像(X + t,Y + t,Z + t)一样将其添加到(X,Y,Z)的每个坐标上吗?接着?]。这是t中的一个非线性方程,通过牛顿迭代法求解。


作为更新/书签,Alvise Vianello 在GitHub的 页面友善发布了Python计算机模拟的测地线。非常感谢你!

Answers

我有一种方法应该适用于任意3D曲面,即使该曲面中有孔或嘈杂。目前它的运行速度很慢,但似乎可以正常运行,并且可能会为您提供一些有关如何执行此操作的想法。

基本前提是微分几何的前提是:

1.)生成代表您的表面的点集

2.)从该点集生成ak最近邻点接近图(我在这里也将尺寸间的距离归一化,因为我觉得它更准确地捕获了“邻居”的概念)

3.)通过使用该点及其邻居作为矩阵的列,计算与该邻近图中每个节点关联的切线空间,然后执行SVD。 SVD之后,左奇异向量为我的切线空间提供了新的基础(前两个列向量是我的平面向量,第三个垂直于该平面)

4.)使用dijkstra的算法在此邻近图上从起始节点移动到结束节点,但不是使用欧几里得距离作为边缘权重,而是使用通过切线空间平行传输的向量之间的距离。

它受到本文的启发(减去所有展开的内容): https : //arxiv.org/pdf/1806.09039.pdf

请注意,我留下了一些我正在使用的辅助函数,这些函数可能与您没有直接关系(大部分是平面绘图)。

您要查看的函数是get_knn,build_proxy_graph,generate_tangent_spaces和geodesic_single_path_dijkstra。

实现也可能会得到改善。

这是代码:

 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 

 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 

 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 

 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 

 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 
 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 

 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 
 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 
 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 
 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 
 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 

 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 
 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 

 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 

 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 

 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 
 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 

 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(min_range, max_range, steps) xx, yy = np.meshgrid(steps_for_plane, steps_for_plane) d = -initial_point.dot(normal_vec) eps = 0.000000001 if abs(normal_vec[2]) < eps and abs(normal_vec[1]) > eps: zz = (-xx*normal_vec[2] - yy*normal_vec[0] - d)/normal_vec[1] else: zz = (-xx*normal_vec[0] - yy*normal_vec[1] - d)/normal_vec[2] return xx, yy, zz # def plot_tangent_plane_at_point(pointset, flattened_points, node, normal_vec): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # node_loc = flattened_points[node] # print("Node loc: {}".format(node_loc)) # xx, yy, zz = plane(10, 10, 500, 500, normal_vec, node_loc) # # xx, yy, zz = get_plane_points(normal_vec, node_loc) # print("Normal Vec: {}".format(normal_vec)) # ax.plot_surface(X=xx, Y=yy, Z=zz) # ax.plot([node_loc[0]], [node_loc[1]], [node_loc[2]], markerfacecolor='k', markeredgecolor='k', marker='o', markersize=10) # plt.show() def generate_tangent_spaces(proxy_graph, flattened_points): # This depth should gaurantee at least 16 neighbors tangent_spaces = {} for node in proxy_graph.nodes(): neighbors = list(nx.neighbors(proxy_graph, node)) node_point = flattened_points[node] zero_mean_mat = np.zeros((len(neighbors)+1, len(node_point))) for i, neighbor in enumerate(neighbors): zero_mean_mat[i] = flattened_points[neighbor] zero_mean_mat[-1] = node_point zero_mean_mat = zero_mean_mat - np.mean(zero_mean_mat, axis=0) u, s, v = svd(zero_mean_mat.T) # smat = np.zeros(u.shape[0], v.shape[0]) # smat[:s.shape[0], :s.shape[0]] = np.diag(s) tangent_spaces[node] = u return tangent_spaces def geodesic_single_path_dijkstra(flattened_points, proximity_graph, tangent_frames, start, end): # short circuit if start == end: return [] # Create min priority queue minheap = [] pred = {} dist = defaultdict(lambda: 1.0e+100) # for i, point in enumerate(flattened_points): R = {} t_dist = {} geo_dist = {} R[start] = np.eye(3) t_dist[start] = np.ones((3,)) dist[start] = 0 start_vector = flattened_points[start] for neighbor in nx.neighbors(proxy_graph, start): pred[neighbor] = start dist[neighbor] = np.linalg.norm(start_vector - flattened_points[neighbor]) heapq.heappush(minheap, (dist[neighbor], neighbor)) while minheap: r_dist, r_ind = heapq.heappop(minheap) if r_ind == end: break q_ind = pred[r_ind] u, s, v = svd(tangent_frames[q_ind].T*tangent_frames[r_ind]) R[r_ind] = np.dot(R[q_ind], u * vT) t_dist[r_ind] = t_dist[q_ind]+np.dot(R[q_ind], tangent_frames[q_ind].T * (r_dist - dist[q_ind])) geo_dist[r_ind] = np.linalg.norm(t_dist[r_ind]) for neighbor in nx.neighbors(proxy_graph, r_ind): temp_dist = dist[r_ind] + np.linalg.norm(flattened_points[neighbor] - flattened_points[r_ind]) if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist pred[neighbor] = r_ind heapq.heappush(minheap, (dist[neighbor], neighbor)) # found ending index, now loop through preds for path current_ind = end node_path = [end] while current_ind != start: node_path.append(pred[current_ind]) current_ind = pred[current_ind] return node_path def plot_path_on_surface(pointset, flattened_points, path): # ax = get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) # ax.plot(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2], linewidth=10.0) # plt.show() get_meshgrid_ax(x=pointset[:, :, 0], y=pointset[:, :, 1], z=pointset[:, :, 2]) points_in_path = flattened_points[path] mlab.plot3d(points_in_path[:, 0], points_in_path[:, 1], points_in_path[:, 2] *.1) mlab.show() """ True geodesic of graph. Build proximity graph Find tangent space using geodisic neighborhood at each point in graph Parallel transport vectors between tangent space points Use this as your distance metric Dijkstra's Algorithm """ if __name__ == "__main__": x, y, z = surface_squares(-5, 5, -5, 5, 500) # plot_meshgrid(x, y, z) pointset = np.stack([x, y, z], axis=2) proxy_graph_num_neighbors = 16 flattened_points = pointset.reshape(pointset.shape[0]*pointset.shape[1], pointset.shape[2]) flattened_points = flattened_points proxy_n_dist, proxy_n_indices = get_knn(flattened_points, proxy_graph_num_neighbors) # Generate a proximity graph using proxy_graph_num_neighbors # Nodes = number of points, max # of edges = number of points * num_neighbors proxy_graph = build_proxy_graph(proxy_n_dist, proxy_n_indices) # Now, using the geodesic_num_neighbors, get geodesic neighborshood for tangent space construction tangent_spaces = generate_tangent_spaces(proxy_graph, flattened_points) node_to_use = 2968 # 3rd vector of tangent space is normal to plane # plot_tangent_plane_at_point(pointset, flattened_points, node_to_use, tangent_spaces[node_to_use][:, 2]) path = geodesic_single_path_dijkstra(flattened_points, proxy_graph, tangent_spaces, 250, 249750) plot_path_on_surface(pointset, flattened_points, path) 
 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mayavi import mlab from sklearn.neighbors import NearestNeighbors from scipy.linalg import svd import networkx as nx import heapq from collections import defaultdict def surface_squares(x_min, x_max, y_min, y_max, steps): x = np.linspace(x_min, x_max, steps) y = np.linspace(y_min, y_max, steps) xx, yy = np.meshgrid(x, y) zz = xx**2 + yy**2 return xx, yy, zz def get_meshgrid_ax(x, y, z): # fig = plt.figure() # ax = fig.gca(projection='3d') # ax.plot_surface(X=x, Y=y, Z=z) # return ax fig = mlab.figure() su = mlab.surf(xT, yT, zT, warp_scale=0.1) def get_knn(flattened_points, num_neighbors): # need the +1 because each point is its own nearest neighbor knn = NearestNeighbors(num_neighbors+1) # normalize flattened points when finding neighbors neighbor_flattened = (flattened_points - np.min(flattened_points, axis=0)) / (np.max(flattened_points, axis=0) - np.min(flattened_points, axis=0)) knn.fit(neighbor_flattened) dist, indices = knn.kneighbors(neighbor_flattened) return dist, indices def rotmatrix(axis, costheta): """ Calculate rotation matrix Arguments: - `axis` : Rotation axis - `costheta` : Rotation angle """ x, y, z = axis c = costheta s = np.sqrt(1-c*c) C = 1-c return np.matrix([[x*x*C+c, x*y*Cz*s, x*z*C+y*s], [y*x*C+z*s, y*y*C+c, y*z*Cx*s], [z*x*Cy*s, z*y*C+x*s, z*z*C+c]]) def plane(Lx, Ly, Nx, Ny, n, d): """ Calculate points of a generic plane Arguments: - `Lx` : Plane Length first direction - `Ly` : Plane Length second direction - `Nx` : Number of points, first direction - `Ny` : Number of points, second direction - `n` : Plane orientation, normal vector - `d` : distance from the origin """ x = np.linspace(-Lx/2, Lx/2, Nx) y = np.linspace(-Ly/2, Ly/2, Ny) # Create the mesh grid, of a XY plane sitting on the orgin X, Y = np.meshgrid(x, y) Z = np.zeros([Nx, Ny]) n0 = np.array([0, 0, 1]) # Rotate plane to the given normal vector if any(n0 != n): costheta = np.dot(n0, n)/(np.linalg.norm(n0)*np.linalg.norm(n)) axis = np.cross(n0, n)/np.linalg.norm(np.cross(n0, n)) rotMatrix = rotmatrix(axis, costheta) XYZ = np.vstack([X.flatten(), Y.flatten(), Z.flatten()]) X, Y, Z = np.array(rotMatrix*XYZ).reshape(3, Nx, Ny) eps = 0.000000001 dVec = d #abs((n/np.linalg.norm(n)))*d#np.array([abs(n[i])/np.linalg.norm(n)*val if abs(n[i]) > eps else val for i, val in enumerate(d)]) # X, Y, Z = X+dVec[0], Y+dVec[1], Z+dVec[2] return X, Y, Z def build_proxy_graph(proxy_n_dist, proxy_n_indices): G = nx.Graph() for distance_list, neighbor_list in zip(proxy_n_dist, proxy_n_indices): # first element is always point current_node = neighbor_list[0] neighbor_list = neighbor_list[1:] distance_list = distance_list[1:] for neighbor, dist in zip(neighbor_list, distance_list): G.add_edge(current_node, neighbor, weight=dist) return G def get_plane_points(normal_vec, initial_point, min_range=-10, max_range=10, steps=1000): steps_for_plane = np.linspace(m