gaussian_curvature.py
Go to the documentation of this file.
00001 #
00002 # Copyright (c) 2010, Georgia Tech Research Corporation
00003 # All rights reserved.
00004 #
00005 # Redistribution and use in source and binary forms, with or without
00006 # modification, are permitted provided that the following conditions are met:
00007 #     * Redistributions of source code must retain the above copyright
00008 #       notice, this list of conditions and the following disclaimer.
00009 #     * Redistributions in binary form must reproduce the above copyright
00010 #       notice, this list of conditions and the following disclaimer in the
00011 #       documentation and/or other materials provided with the distribution.
00012 #     * Neither the name of the Georgia Tech Research Corporation nor the
00013 #       names of its contributors may be used to endorse or promote products
00014 #       derived from this software without specific prior written permission.
00015 #
00016 # THIS SOFTWARE IS PROVIDED BY GEORGIA TECH RESEARCH CORPORATION ''AS IS'' AND
00017 # ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00018 # WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00019 # DISCLAIMED. IN NO EVENT SHALL GEORGIA TECH BE LIABLE FOR ANY DIRECT, INDIRECT,
00020 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
00021 # LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
00022 # OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
00023 # LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
00024 # OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
00025 # ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00026 #
00027 
00028 #  \author Hai Nguyen (Healthcare Robotics Lab, Georgia Tech.)
00029 import numpy as np
00030 from hrl_lib.util import getTime
00031     
00032 ##
00033 # Fits a plane
00034 # 
00035 # @param points 3xn numpy matrix representing points to be fitted
00036 # @param d direction that normal calculated should point in
00037 # @return normal and eigen value points (in descending order) 
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         #select the 3rd eigenvector, assoc. with the smallest eigenvalue
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 # Calculates a coordinate system  & eigen values of given points matrix 
00072 #
00073 # @param points 3xn numpy matrix representing points to be fitted
00074 # @param x first principle direction to match to
00075 # @param z second direction to match to
00076 # @return (3x3 matrix describe coordinate frame, list with associated eigen values) 
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     #svd
00080     c = np.cov(points)
00081     u, s, vh = np.linalg.svd(c)
00082     u = np.matrix(u)  
00083       
00084     choices = range(3)
00085     #find correspondences
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 #    print z.T, z_distances, z_choice
00089 #    print 'remaining', choices, 'removing', z_choice
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 #    print x.T, x_distances, x_choice
00095 #    print 'remaining', choices, 'removing', x_choice
00096     choices.remove(x_choice)    
00097     
00098     #reorder
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     #check directions
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     #find third vector
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     #Generate random points on a disc
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 #    #U is unordered, we reorder it according to given x and z
00202 #    u      = np.matrix(u)
00203 #    vecs   = [u[:,i] / np.linalg.norm(u[:,i]) for i in range(3)]
00204 #    s_list = [s[i] for i in range(3)]
00205 #    canon  = [v / np.linalg.norm(v) for v in [x, z]]
00206 #
00207 #    ordered_vecs = []
00208 #    ordered_s = [](x.T * u[
00209 #    for idx, v in enumerate(vecs):    
00210 #        angles = []        
00211 #        for d in canon:
00212 #            sep = np.degrees(np.arccos(d.T * v))[0,0]
00213 #            if sep == 180:
00214 #                sep = 0
00215 #            angles.append(sep)            
00216 #        min_idx = np.argmin(angles)        
00217 #        ordered_vecs.append(vecs.pop(min_idx))
00218 #        ordered_s.append(s_list.pop(min_idx))
00219 #        
00220 #    #pdb.set_trace()
00221     #ordered_vecs.append(np.matrix(np.cross(ordered_vecs[0].T, ordered_vecs[1].T)).T)
00222 #    xe = ordered_vecs[0]
00223 #    ye = ordered_vecs[2]
00224 #    ze = ordered_vecs[1]
00225 #    ordered_vecs = [xe, ye, ze]    
00226 #        
00227 #    ordered_s.append(s_list[0])
00228 #    ordered_s = [ordered_s[0], ordered_s[2], ordered_s[1]]
00229 #    ordered_s = np.matrix(ordered_s).T
00230 #    print ordered_s
00231 #    
00232 #    reordered = np.concatenate(ordered_vecs, axis=1)
00233 #    if np.linalg.det(reordered) < 0:
00234 #        reordered[:,1] = -reordered[:,1]
00235 #        ordered_s[1,0] = -ordered_s[1,0]
00236 #        
00237 #    return reordered, ordered_s     
00238     
00239     
00240     
00241     
00242     
00243     
00244     
00245     
00246     
00247     
00248     
00249     
00250     
00251     
00252     
00253     


laser_camera_segmentation
Author(s): Martin Schuster, Advisor: Prof. Charlie Kemp, Lab: Healthcare Robotics Lab at Georgia Tech
autogenerated on Wed Nov 27 2013 11:56:44