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
00030
00031
00032
00033
00034
00035
00036 import numpy
00037 import numpy as np
00038 import scipy
00039 import scipy.linalg
00040
00041 def ransac(data,model,n,k,t,d,debug=False,return_all=False):
00042 print 'INFO: running RANSAC for k=',k,'iterations'
00043 """fit model parameters to data using the RANSAC algorithm
00044
00045 This implementation written from pseudocode found at
00046 http://en.wikipedia.org/w/index.php?title=RANSAC&oldid=116358182
00047
00048 {{{
00049 Given:
00050 data - a set of observed data points
00051 model - a model that can be fitted to data points
00052 n - the minimum number of data values required to fit the model
00053 k - the maximum number of iterations allowed in the algorithm
00054 t - a threshold value for determining when a data point fits a model
00055 d - the number of close data values required to assert that a model fits well to data
00056 Return:
00057 bestfit - model parameters which best fit the data (or nil if no good model is found)
00058 iterations = 0
00059 bestfit = nil
00060 besterr = something really large
00061 while iterations < k {
00062 maybeinliers = n randomly selected values from data
00063 maybemodel = model parameters fitted to maybeinliers
00064 alsoinliers = empty set
00065 for every point in data not in maybeinliers {
00066 if point fits maybemodel with an error smaller than t
00067 add point to alsoinliers
00068 }
00069 if the number of elements in alsoinliers is > d {
00070 % this implies that we may have found a good model
00071 % now test how good it is
00072 bettermodel = model parameters fitted to all points in maybeinliers and alsoinliers
00073 thiserr = a measure of how well model fits these points
00074 if thiserr < besterr {
00075 bestfit = bettermodel
00076 besterr = thiserr
00077 }
00078 }
00079 increment iterations
00080 }
00081 return bestfit
00082 }}}
00083 """
00084
00085
00086
00087
00088
00089
00090
00091
00092
00093
00094
00095
00096
00097
00098
00099
00100
00101
00102
00103
00104
00105
00106
00107
00108
00109
00110
00111
00112
00113
00114
00115
00116
00117
00118
00119
00120
00121
00122 iterations = 0
00123 bestfit = None
00124 besterr = numpy.inf
00125 best_inlier_idxs = None
00126 while iterations < k:
00127
00128 maybe_idxs, test_idxs = random_partition(n,data.shape[0])
00129 maybeinliers = data[maybe_idxs,:]
00130 test_points = data[test_idxs]
00131 maybemodel = model.fit(maybeinliers)
00132 test_err = model.get_error( test_points, maybemodel)
00133 also_idxs = test_idxs[test_err < t]
00134 alsoinliers = data[also_idxs,:]
00135 if debug:
00136 print 'test_err.min()',test_err.min()
00137 print 'test_err.max()',test_err.max()
00138 print 'numpy.mean(test_err)',numpy.mean(test_err)
00139 print 'iteration %d:len(alsoinliers) = %d'%(
00140 iterations,len(alsoinliers))
00141 if len(alsoinliers) > d:
00142 betterdata = numpy.concatenate( (maybeinliers, alsoinliers) )
00143 bettermodel = model.fit(betterdata)
00144 better_errs = model.get_error( betterdata, bettermodel)
00145 thiserr = numpy.mean( better_errs )
00146 if thiserr < besterr:
00147 bestfit = bettermodel
00148 besterr = thiserr
00149 best_inlier_idxs = numpy.concatenate( (maybe_idxs, also_idxs) )
00150 iterations+=1
00151 if bestfit is None:
00152 raise ValueError("did not meet fit acceptance criteria")
00153 if return_all:
00154 return bestfit, {'inliers':best_inlier_idxs}
00155 else:
00156 return bestfit
00157
00158
00159 def random_partition(n,n_data):
00160 """return n random rows of data (and also the other len(data)-n rows)"""
00161 all_idxs = numpy.arange( n_data )
00162 numpy.random.shuffle(all_idxs)
00163 idxs1 = all_idxs[:n]
00164 idxs2 = all_idxs[n:]
00165 return idxs1, idxs2
00166
00167 class LinearLeastSquaresModel:
00168 """linear system solved using linear least squares
00169
00170 This class serves as an example that fulfills the model interface
00171 needed by the ransac() function.
00172
00173 """
00174 def __init__(self,input_columns,output_columns,debug=False):
00175 self.input_columns = input_columns
00176 self.output_columns = output_columns
00177 self.debug = debug
00178 def fit(self, data):
00179 A = numpy.vstack([data[:,i] for i in self.input_columns]).T
00180 B = numpy.vstack([data[:,i] for i in self.output_columns]).T
00181 x,resids,rank,s = scipy.linalg.lstsq(A,B)
00182 return x
00183 def get_error( self, data, model):
00184 A = numpy.vstack([data[:,i] for i in self.input_columns]).T
00185 B = numpy.vstack([data[:,i] for i in self.output_columns]).T
00186 B_fit = scipy.dot(A,model)
00187 err_per_point = numpy.sum((B-B_fit)**2,axis=1)
00188 print err_per_point
00189 return err_per_point
00190
00191 class PlaneLeastSquaresModel:
00192
00193 def __init__(self, debug=False):
00194 self.debug = debug
00195 def fit(self, data):
00196
00197 model = [data[0],numpy.cross(data[1] - data[0], data[2] - data[1])]
00198 model[1] = model[1] / numpy.linalg.norm(model[1])
00199 return model
00200 def get_error( self, data, model):
00201
00202
00203 max_angle = 30.0 * np.pi / 180.0
00204 angle = np.arccos(scipy.dot(np.array([0,0,1]),model[1].T))
00205
00206
00207 if abs(angle) > max_angle:
00208 return np.ones(np.shape(data)[0]) * 999999999999999999999999999999
00209
00210
00211 d = scipy.dot(model[0],model[1].T)
00212
00213 s = scipy.dot(data, model[1].T) - d
00214
00215
00216
00217 return abs(s)
00218
00219 def test():
00220
00221
00222 n_samples = 500
00223 n_inputs = 1
00224 n_outputs = 1
00225 A_exact = 20*numpy.random.random((n_samples,n_inputs) )
00226 perfect_fit = 60*numpy.random.normal(size=(n_inputs,n_outputs) )
00227 B_exact = scipy.dot(A_exact,perfect_fit)
00228 assert B_exact.shape == (n_samples,n_outputs)
00229
00230
00231 A_noisy = A_exact + numpy.random.normal(size=A_exact.shape )
00232 B_noisy = B_exact + numpy.random.normal(size=B_exact.shape )
00233
00234 if 1:
00235
00236 n_outliers = 100
00237 all_idxs = numpy.arange( A_noisy.shape[0] )
00238 numpy.random.shuffle(all_idxs)
00239 outlier_idxs = all_idxs[:n_outliers]
00240 non_outlier_idxs = all_idxs[n_outliers:]
00241 A_noisy[outlier_idxs] = 20*numpy.random.random((n_outliers,n_inputs) )
00242 B_noisy[outlier_idxs] = 50*numpy.random.normal(size=(n_outliers,n_outputs) )
00243
00244
00245
00246 all_data = numpy.hstack( (A_noisy,B_noisy) )
00247 input_columns = range(n_inputs)
00248 output_columns = [n_inputs+i for i in range(n_outputs)]
00249 debug = False
00250 model = LinearLeastSquaresModel(input_columns,output_columns,debug=debug)
00251
00252 linear_fit,resids,rank,s = scipy.linalg.lstsq(all_data[:,input_columns],
00253 all_data[:,output_columns])
00254
00255
00256 ransac_fit, ransac_data = ransac(all_data,model,
00257 50, 1000, 7e3, 300,
00258 debug=debug,return_all=True)
00259 if 1:
00260 import pylab
00261
00262 sort_idxs = numpy.argsort(A_exact[:,0])
00263 A_col0_sorted = A_exact[sort_idxs]
00264
00265 if 1:
00266 pylab.plot( A_noisy[:,0], B_noisy[:,0], 'k.', label='data' )
00267 pylab.plot( A_noisy[ransac_data['inliers'],0], B_noisy[ransac_data['inliers'],0], 'bx', label='RANSAC data' )
00268 else:
00269 pylab.plot( A_noisy[non_outlier_idxs,0], B_noisy[non_outlier_idxs,0], 'k.', label='noisy data' )
00270 pylab.plot( A_noisy[outlier_idxs,0], B_noisy[outlier_idxs,0], 'r.', label='outlier data' )
00271 pylab.plot( A_col0_sorted[:,0],
00272 numpy.dot(A_col0_sorted,ransac_fit)[:,0],
00273 label='RANSAC fit' )
00274 pylab.plot( A_col0_sorted[:,0],
00275 numpy.dot(A_col0_sorted,perfect_fit)[:,0],
00276 label='exact system' )
00277 pylab.plot( A_col0_sorted[:,0],
00278 numpy.dot(A_col0_sorted,linear_fit)[:,0],
00279 label='linear fit' )
00280 pylab.legend()
00281 pylab.show()
00282
00283
00284 def testPlane():
00285
00286
00287
00288 debug = True
00289 model = PlaneLeastSquaresModel(debug)
00290 data = numpy.array([[0,0,0],[0,1,0],[0.1,12,0.1],[0,0,12],[1,0,0],[1,2,13]])
00291
00292 ransac_fit, ransac_data = ransac(data,model,
00293 3, 1000, 1, 2,
00294 debug=debug,return_all=True)
00295 print ransac_fit
00296 print ransac_data
00297
00298 def testPlanePointcloud():
00299 import roslib; roslib.load_manifest('laser_camera_segmentation')
00300 import laser_camera_segmentation.processor as processor
00301 import laser_camera_segmentation.configuration as configuration
00302
00303 cfg = configuration.configuration('/home/martin/robot1_data/usr/martin/laser_camera_segmentation/labeling')
00304
00305 pc = processor.processor(cfg)
00306
00307 pc.load_data('2009Nov04_141226')
00308 pc.process_raw_data()
00309
00310
00311 debug = False
00312 model = PlaneLeastSquaresModel(debug)
00313 data = numpy.asarray(pc.pts3d_bound).T
00314
00315 ransac_fit, ransac_data = ransac(data,model,
00316 3, 1000, 0.02, 300,
00317 debug=debug,return_all=True)
00318 print ransac_fit
00319 print ransac_data
00320 print 'len inlier',len(ransac_data['inliers']),'shape pts',np.shape(pc.pts3d_bound)
00321 pc.pts3d_bound = pc.pts3d_bound[:,ransac_data['inliers']]
00322 pc.display_3d('height')
00323
00324 if __name__=='__main__':
00325
00326 testPlanePointcloud()