00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029 import numpy as np
00030 from hrl_lib.util import getTime
00031
00032
00033
00034
00035
00036
00037
00038 def gaussian_curvature(points, p=np.matrix([0,0,1]).T):
00039 assert(points.shape[0] == 3)
00040 if points.shape[1] < 2:
00041 return np.matrix([0, 0, 0.0]).T, np.array([0, 0, 0.0])
00042 c = np.cov(points)
00043 try:
00044 u, s, vh = np.linalg.svd(c)
00045 u = np.matrix(u)
00046
00047
00048 if u[:,2].T * p < 0:
00049 return -u[:,2], s
00050 else:
00051 return u[:,2], s
00052 except np.linalg.linalg.LinAlgError, e:
00053 print e
00054 print 'oh no!!!!!! why is this happening?'
00055 print points.shape
00056 print c
00057 print points
00058
00059 def spread(points):
00060 assert(points.shape[0] == 3)
00061 if points.shape[1] < 2:
00062 return np.matrix([0, 0, 0.0]).T, np.array([0, 0, 0.0])
00063
00064 c = np.cov(points)
00065 u, s, vh = np.linalg.svd(c)
00066 u = np.matrix(u)
00067 return u[:,0:2], s[0:2]
00068
00069
00070
00071
00072
00073
00074
00075
00076
00077 def gaussian_curvature2(points, x=np.matrix([1,0,0]).T, z=np.matrix([0,0,1]).T):
00078 assert(points.shape[0] == 3)
00079
00080 c = np.cov(points)
00081 u, s, vh = np.linalg.svd(c)
00082 u = np.matrix(u)
00083
00084 choices = range(3)
00085
00086 z_distances = [np.degrees(np.arccos(np.abs(z.T * u[:,i])))[0,0] for i in choices]
00087 z_choice = choices[np.argmin(z_distances)]
00088
00089
00090 choices.remove(z_choice)
00091
00092 x_distances = [np.degrees(np.arccos(np.abs(x.T * u[:,i])))[0,0] for i in choices]
00093 x_choice = choices[np.argmin(x_distances)]
00094
00095
00096 choices.remove(x_choice)
00097
00098
00099 xv = u[:,x_choice]
00100 xs = s[x_choice]
00101
00102 ys = s[choices[0]]
00103
00104 zv = u[:,z_choice]
00105 zs = s[z_choice]
00106
00107
00108 if np.degrees(np.arccos(xv.T * x)) > 90.0:
00109 xv = -xv
00110 if np.degrees(np.arccos(zv.T * z)) > 90.0:
00111 zv = -zv
00112
00113
00114 yv = np.matrix(np.cross(xv.T, zv.T)).T
00115 directions = np.concatenate([xv, yv, zv], axis=1)
00116 sd = [xs, ys, zs]
00117 if np.linalg.det(directions) < 0:
00118 directions[:,1] = -directions[:,1]
00119 assert np.linalg.det(directions) > 0
00120 return directions, sd
00121
00122
00123 def plot_axis(x,y, z, directions):
00124 from enthought.mayavi import mlab
00125 mlab.quiver3d(x, y, z, [directions[0,0]], [directions[1,0]], [directions[2,0]], scale_factor=1, color=(1,0,0))
00126 if directions.shape[1] > 1:
00127 mlab.quiver3d(x, y, z, [directions[0,1]], [directions[1,1]], [directions[2,1]], scale_factor=1, color=(0,1,0))
00128 mlab.quiver3d(x, y, z, [directions[0,2]], [directions[1,2]], [directions[2,2]], scale_factor=1, color=(0,0,1))
00129
00130 def generate_pts():
00131
00132 radius = np.random.random((1, 2000))
00133 angle = np.random.random((1,2000)) * 2*np.pi
00134 x = radius * np.cos(angle)
00135 y = radius * np.sin(angle)
00136 r = np.concatenate((x,y))
00137 r[0,:] = r[0,:] * 2
00138 xy_plane = np.matrix(np.eye(3))[:,(0, 2)]
00139 pts = xy_plane * r
00140 return pts
00141
00142
00143 def demo1():
00144 from enthought.mayavi import mlab
00145 pts = generate_pts()
00146 directions, magnitudes = gaussian_curvature(pts)
00147 print directions.T.A[0].tolist()
00148
00149 print magnitudes.tolist()
00150 mlab.points3d(pts[0,:].A1, pts[1,:].A1, pts[2,:].A1, mode='sphere', scale_factor=.05)
00151 plot_axis(0,0,0, directions)
00152 plot_axis(2,0,0, np.eye(3))
00153 mlab.show()
00154
00155 def demo2():
00156 from enthought.mayavi import mlab
00157 pts = generate_pts()
00158 direction, magnitudes = gaussian_curvature(pts)
00159 print 'eigen values', magnitudes.T
00160 mlab.points3d(pts[0,:].A1, pts[1,:].A1, pts[2,:].A1, mode='sphere', scale_factor=.05)
00161 plot_axis(0,0,0, direction)
00162 plot_axis(2,0,0, np.eye(3))
00163 mlab.show()
00164
00165 if __name__ == '__main__':
00166 demo1()
00167
00168
00169
00170
00171
00172
00173
00174
00175
00176
00177
00178
00179
00180
00181
00182
00183
00184
00185
00186
00187
00188
00189
00190
00191
00192
00193
00194
00195
00196
00197
00198
00199
00200
00201
00202
00203
00204
00205
00206
00207
00208
00209
00210
00211
00212
00213
00214
00215
00216
00217
00218
00219
00220
00221
00222
00223
00224
00225
00226
00227
00228
00229
00230
00231
00232
00233
00234
00235
00236
00237
00238
00239
00240
00241
00242
00243
00244
00245
00246
00247
00248
00249
00250
00251
00252
00253