def calibrate(self):
with open("calib.gnuplot", "w") as fh:
print >> fh, """
set xlabel "measured"
set ylabel "predicted"
"""
X = np.array(self.X)
Y = np.array(self.Y)
model = LinearModel()
for i in xrange(3):
print "-- gyro", i, "--"
data = np.vstack((X[:, i], Y[:, i])).T
self.dump_data("calib_%i.out" % (i), X[:, i], Y[:, i])
try:
# ransac(data, model, n, k, t, d, debug=False,return_all=False)
out, ret = ransac(data,
model,
2,
1000,
50,
0.75 * len(data),
return_all=True)
# n=min num data, k=max iter, t=error thresh, d=num close values to assert fit
inliers = ret["inliers"]
print len(inliers), "inliers in", len(data)
print "model:\n", out
print >> fh, """
set term x11 %d persist
set title "Axis %d"
plot [] [-250:250] "calib_%d.out" using 1:2 notitle, (%f)*x+(%f) notitle
""" % (i, i, i, out[0], out[1])
except ValueError:
print "Unable to perform regression"
def findAffineTransform(test_srcs, ref_srcs, max_pix_tol = 2., min_matches_fraction = 0.8, invariantMap=None):
if len(test_srcs) < 3:
raise Exception("Test sources has less than the minimum value of points (3).")
if invariantMap is None:
invMap = InvariantTriangleMapping()
if len(ref_srcs) < 3:
raise Exception("Test sources has less than the minimum value of points (3).")
#generateInvariants should return a list of the invariant tuples for each asterism and
# a corresponding list of the indices that make up the asterism
ref_invariants, ref_asterisms = invMap.generateInvariants(ref_srcs, nearest_neighbors = 7)
ref_invariant_tree = KDTree(ref_invariants)
test_invariants, test_asterisms = invMap.generateInvariants(test_srcs, nearest_neighbors = 5)
test_invariant_tree = KDTree(test_invariants)
#0.03 is just an empirical number that returns about the same number of matches than inputs
matches_list = test_invariant_tree.query_ball_tree(ref_invariant_tree, 0.03)
matches = []
#t1 is an asterism in test, t2 in ref
for t1, t2_list in zip(test_asterisms, matches_list):
for t2 in np.array(ref_asterisms)[t2_list]:
matches.append(zip(t2, t1))
matches = np.array(matches)
invModel = invMap.matchTransform(ref_srcs, test_srcs)
nInvariants = len(matches)
max_iter = nInvariants
min_matches = min(10, int(nInvariants * min_matches_fraction))
bestM = ransac.ransac(matches, invModel, 1, max_iter, max_pix_tol, min_matches)
return bestM
def findParametersNilsNew(rs,N=4,useFeatures=True,dz=None):
# problem in bestMatch() solved?
params =[[1.,0.,0.,0.,1.,0.,0.,0.,1.,0.,0.,0.]]
for ibeads in range(1,len(rs)):
tmpWorked = False
for featureFunc in [calcFeatures,calcFeaturesFallback]:
if tmpWorked: break
if useFeatures and dz is None:
scales = n.append(n.linspace(0.3,1,10),n.linspace(1,10,30),0)
elif not dz is None:
scales = [float(dz)]
else: scales = [1.]
for scale in scales:
scale = n.array([scale,1.,1.])
#print scale
tmpbeads = scaleBeads(rs[ibeads],scale,verbose=True)
refbeads = scaleBeads(rs[0],scale)
if useFeatures:
f0 = featureFunc(refbeads,N=N)
# find initial matches and calculate approximate transform
tmpf = featureFunc(tmpbeads,N=N)
else:
f0 = refbeads
tmpf = tmpbeads
if len(f0)>=len(tmpf): indList = [0,1]
else: indList = [1,0]
indices,distances = bestMatch(*tuple([[f0,tmpf][i] for i in indList]))
number = 20
number = n.max([len(indices[0])/10,number])
number = n.min([len(indices[0]),number])
model = ransac.AffineTransformationModel(rs[0][indices[indList[0]]],rs[ibeads][indices[indList[1]]])
data = n.array(range(number))
#print indices
#print number
if useFeatures: maxIterations = number*10
else: maxIterations = number*1000
try:
fit,inliers = ransac.ransac(data,model,4,maxIterations,50,number/2,return_all=1)
tmpWorked = True
print 'OK: found parameters'
break
except ValueError:
print 'try again with different z scale factor...'
pass
if not tmpWorked:
raise Exception('Could not register bead images.')
# find better transform using redefined matches
tmpArgs = [[scaleBeads(transform(fit,rs[0]),scale),scaleBeads(rs[ibeads],scale)][i] for i in indList]
indices2,distances2 = bestMatch(*tuple(tmpArgs))
inds = n.where(distances2<5.)[0]
if len(inds)<number:
print '#'*10+'\ncheck bead registration since only a few beads are well aligned\n'+'#'*10
inds = n.array(range(number))
fit2 = affine_fit.Affine_Fit(rs[0][indices2[indList[0],inds]],rs[ibeads][indices2[indList[1],inds]]).Matrix()
params.append(fit2)
indices3,distances3 = bestMatch(transform(fit2,rs[0]),rs[ibeads])
print 'mean bead distance in pixels after alignment is\n%s' %n.mean(distances3)
return params
def estimateRigidCorrectionRANSAC(A, B, pointsPerModel=300, iterations=10, outlierThreshold=0.6, minPointsPercent=0.5):
n_dim = 12
input_columns = np.linspace(0,11,n_dim)
output_columns = np.linspace(12,23,n_dim)
translation_offset = 4
model = LsqModel(input_columns, output_columns, translation_offset);
A_np = np.zeros((n_dim,0))
for pose in A:
A_np = np.c_[A_np, np.reshape(pose[:3,:], (12,1))]
B_np = np.zeros((n_dim,0))
for pose in B:
B_np = np.c_[B_np, np.reshape(pose[:3,:], (12,1))]
A_np = A_np.T
B_np = B_np.T
data = np.c_[A_np, B_np]
min_points_accept = data.shape[0] * minPointsPercent
vec = ransac.ransac(data, model, pointsPerModel, iterations, outlierThreshold, minPointsPercent, debug=False)
pose = tfx.pose(vec[:3], vec[3:])
tf = np.array(pose.matrix)
return tf
def _calc_stats(self, robust=True):
self.inliers = np.array([]).astype(bool)
self.stats = np.empty((0, 3))
if ransac:
meanstd = lambda data: [np.mean(data), np.std(data)]
azscore = lambda data, mdl: np.abs((data - mdl[0]) / mdl[1])
mse = lambda data, mdl: np.mean((data - mdl[0])**2)
# for freq in tqdm(np.unique(self.cgain[:, 0])):
for freq in np.unique(self.cgain[:, 0]):
rundata = self.cgain[self.cgain[:, 0] == freq, 1]
flydata = np.mean(np.reshape(rundata, (self.nTrials, -1)),
axis=0) # Average per fly over trials
if robust:
mdl = ransac(flydata,
meanstd,
azscore,
mse,
thresh=self.rs_thresh,
max_iters=5e4)
mean = mdl[0]
stderr = stats.sem(flydata[np.abs(flydata - mdl[0]) /
mdl[1] < self.rs_thresh])
self.inliers = np.append(
self.inliers,
np.abs(rundata - mdl[0]) / mdl[1] < self.rs_thresh)
else:
mean = np.nanmean(flydata)
stderr = stats.sem(flydata)
self.inliers = np.append(self.inliers,
np.ones(rundata.shape).astype(bool))
self.stats = np.append(self.stats, [[freq, mean, stderr]], axis=0)
def length(msgs, size, noise_ratio, num_iters, eps=1e-10):
fields = []
min_size = min(map(len, msgs))
for i in range(0, min_size - size + 1, size):
gap_in_data = False
X = []
for msg in msgs:
data = msg[i:i+size]
if 256 in data:
gap_in_data = True
break
X.append(_get_value_from_bytes(data))
if gap_in_data:
fields.append(False)
continue
Y = [len(msg) for msg in msgs]
XY = np.asarray([X, Y]).T
try:
(params, _, res) = ransac(XY, _model, 2, (1 - noise_ratio), num_iters, eps)
k = params[0]
m = params[1]
success = k > (1 - eps) and m > -eps
except ValueError:
success = False
fields.append(success)
return fields
def postprocess(self, labels):
debug = False
model = ransac.PlaneLeastSquaresModel(debug)
data_idx = np.where(np.asarray(labels) == processor.LABEL_SURFACE)[0]
data = np.asarray(self.processor.pts3d_bound).T[data_idx]
n, _ = np.shape(data)
if n < 5000:
k = 700
else:
k = 2000
# run RANSAC algorithm
ransac_fit, ransac_data = ransac.ransac(data,model,
3, k, 0.04, len(data_idx)/2.5, # misc. parameters
debug=debug,return_all=True)
print 'ransac: model',ransac_fit
print 'ransac:',ransac_data
print 'len inlier',len(ransac_data['inliers']),'shape pts',np.shape(self.processor.pts3d_bound)
#labels[data_idx[ransac_data['inliers']]] = processor.LABEL_CLUTTER #all non-plane pts
fancy = np.zeros(len(np.asarray(labels))).astype(bool)
fancy[data_idx] = True
fancy[data_idx[ransac_data['inliers']]] = False
labels[fancy] = processor.LABEL_CLUTTER #all surface-labeled non-plane pts
return labels
def callRansac():
numPoints = 20
isValid =np.zeros(numPoints)
successCounter = 0
centerCoordinates = np.zeros([numPoints,3])
for frameNumber in range(0,numPoints):
print('Run Ransac:'+str(frameNumber))
fr = 'frame'
frNum = str (frameNumber)
fileName = fr+frNum+'.npy'
m_pts=np.load(os.path.join('frames',fileName))
x, y = np.shape(m_pts)
ransacItr = x*4
result = ransac(m_pts,30,1,ransacItr,0.03)
r, cx, cy, cz= result[0]
if (r>0.065 and r<0.085):
isValid[frameNumber] = 1;
successCounter = successCounter+1
print('Success!')
centerCoordinates[frameNumber,0]=cx
centerCoordinates[frameNumber,1]=cy
centerCoordinates[frameNumber,2]=cz
np.save("centerCoordinates",centerCoordinates)
np.savetxt("originCenters.txt",centerCoordinates)
print('Number of successful points'+str(successCounter))
percentage=successCounter/numPoints
print('That is a '+str(percentage)+'success rate')
def do_ransac(self):
point_cloud = k.get_pointcloud()
point_cloud = ransac.ransac(point_cloud, 300, 10, 8000)
#with self.cloud_lock:
# self.pc = point_cloud
return point_cloud
def calibrate(self):
with open("calib.gnuplot", "w") as fh:
print >>fh, """
set xlabel "measured"
set ylabel "predicted"
"""
X = np.array(self.X)
Y = np.array(self.Y)
model = LinearModel()
for i in xrange(3):
print "-- gyro", i, "--"
data = np.vstack( (X[:,i], Y[:,i]) ).T
self.dump_data("calib_%i.out"%(i), X[:,i], Y[:,i])
try:
# ransac(data, model, n, k, t, d, debug=False,return_all=False)
out, ret = ransac(data, model, 2, 1000, 50, 0.75*len(data), return_all=True)
# n=min num data, k=max iter, t=error thresh, d=num close values to assert fit
inliers = ret["inliers"]
print len(inliers), "inliers in", len(data)
print "model:\n", out
print >>fh, """
set term x11 %d persist
set title "Axis %d"
plot [] [-250:250] "calib_%d.out" using 1:2 notitle, (%f)*x+(%f) notitle
"""%(i, i, i, out[0], out[1])
except ValueError:
print "Unable to perform regression"
def calculate_image_points(image_message):
cv_image = BRIDGE.imgmsg_to_cv2(image_message, desired_encoding="mono8")
_, thresh = cv2.threshold(cv_image, 100, 255, cv2.THRESH_BINARY)
thresh[MASK == 0] = 0
image_points_list = []
for r in MASK_RANGES:
ys, xs = np.nonzero(thresh[r[0]:r[1], r[2]:r[3]] != 0)
outliers = list(zip(xs, ys))
for i in range(4):
model = ransac.ransac(outliers, 500, 16, 300)
p1 = model.inliers[0]
p2 = model.inliers[1]
cv2.line(
cv_image,
(p1[0] + r[2], p1[1] + r[0]),
(p2[0] + r[2], p2[1] + r[0]),
0,
4,
)
print(model)
m = -model.a / model.b
b = -model.c / model.b
print("m: %f, b: %f" % (m, b))
outliers = model.outliers
#binarized_img_publisher.publish(
# BRIDGE.cv2_to_imgmsg(cv_image, encoding="passthrough")
#)
line_img_publisher.publish(
BRIDGE.cv2_to_imgmsg(cv_image, encoding="passthrough"))
for p in image_points_list:
thresh[p[0] - 1:p[0] + 1, p[1] - 1:p[1] + 1] = 0
binarized_img_publisher.publish(
BRIDGE.cv2_to_imgmsg(thresh, encoding="passthrough"))
return image_points_list
def stitch_images(im1, im2,shape_method, frac_inliers=0.5, error_margin=0.01,dx=0,dy=0,shape_predifined=(600,700)):
"""
Stitch two images im1 and im2 together.
Parameters:
- frac_inliers: estimated fraction of matches that are inliers.
- error_margin: probability of not having three inliers when
picking random matches (determines accuracy
of perspective transform matrix).
- threshold: maximum error in euclidian norm to determine
inliers (for RANSAC).
- dx: translation in x direction of images after applying
affine transform.
- dy: translation in y direction of images after applying
affine transform.
- shape_method: 'function' for using an algorithm to determine the size of stiched image
'precalculated' for specifing a size for stitched image
- shape_predefined
"""
# Get the keypoints and calculate the matches
# of the descriptors in the two images.
matches, kps1, kps2 = keypoint_matching(im1, im2)
# Determine the number of tests.
min_tests = int(np.log(error_margin) / np.log(1 - frac_inliers ** 3))
# Determine the projective matrix (RANSAC and SVD tric).
P, _ = ransac(kps1, kps2, matches, num_tests=min_tests)
if P is None:
print("Not enough inliers found")
return
# Set up translation matrix
translate = np.array([[1, 0, dx],
[0, 1, dy],
[0, 0, 1]])
# Translate after projective transform (multiply matrices)
P = translate @ P
P_inverse = np.linalg.inv(P)
if shape_method=='function':
shape = estimate_bounding_box(im2, P_inverse)
elif shape_method=='precalculated':
shape=shape_predifined
# Warp second image.
f_stitched = cv2.warpAffine(im1, P[:2], dsize=shape)
M, N = im2.shape[:2]
print(f'M,N : {M, N}')
# Paste im1 onto the stiched image in the upper left corner
# and apply same translation as image 1.
dx = np.abs(dx);
dy = np.abs(dy)
f_stitched[dy:M + dy, dx:N + dx, :] = im2
# Show image.
fig = plt.gcf()
fig.set_figheight(20)
fig.set_figwidth(20)
plt.imshow(f_stitched[:, :, ::-1])
plt.axis('off')
plt.show()
def filter_grids(grids, weights):
if len(grids) < 5:
return []
model = ransac.ransac(grids, weights, ransac.LinearFit, 10, 3, 0.6)
if model is None:
return []
return sorted(model.points)
def findParametersLessAccurate(rs,N=4):
# less accurate
beads = []
features = []
#distancess = []
params =[[1.,0.,0.,0.,1.,0.,0.,0.,1.,0.,0.,0.]]
for ibeads in range(1,len(rs)):
for scale in n.linspace(1,10,50):
scale = n.array([scale,1.,1.])
#print scale
tmpbeads = scaleBeads(rs[ibeads],scale,verbose=True)
refbeads = scaleBeads(rs[0],scale)
f0 = calcFeatures(refbeads,N=N)
# find initial matches and calculate approximate transform
tmpf = calcFeatures(tmpbeads,N=N)
indices,distances = bestMatch(f0,tmpf)
#distancess.append(distances)
number = n.max([len(indices[0])/5,40])
inds = n.array(range(number))
model = ransac.AffineTransformationModel(rs[0][indices[0]],rs[ibeads][indices[1]])
data = n.array(range(number))
try:
fit,inliers = ransac.ransac(data,model,4,number*10,10,number/2,return_all=1)
print 'OK: found parameters'
break
except:
print 'try again with different z scale factor...'
pass
# find better transform using redefined matches
#indices2,distances2 = bestMatch(scaleBeads(transform(fit,rs[0]),scale),scaleBeads(rs[ibeads],scale))
#inds = n.where(distances2<5.)[0]
#fit2 = affine_fit.Affine_Fit(rs[0][indices2[0,inds]],rs[ibeads][indices2[1,inds]]).Matrix()
params.append(fit)
return params
def postprocess(self, labels):
debug = False
model = ransac.PlaneLeastSquaresModel(debug)
data_idx = np.where(np.asarray(labels) == processor.LABEL_SURFACE)[0]
data = np.asarray(self.processor.pts3d_bound).T[data_idx]
n, _ = np.shape(data)
if n < 5000:
k = 700
else:
k = 2000
# run RANSAC algorithm
ransac_fit, ransac_data = ransac.ransac(data,model,
3, k, 0.04, len(data_idx)/2.5, # misc. parameters
debug=debug,return_all=True)
print 'ransac: model',ransac_fit
print 'ransac:',ransac_data
print 'len inlier',len(ransac_data['inliers']),'shape pts',np.shape(self.processor.pts3d_bound)
#labels[data_idx[ransac_data['inliers']]] = processor.LABEL_CLUTTER #all non-plane pts
fancy = np.zeros(len(np.asarray(labels))).astype(bool)
fancy[data_idx] = True
fancy[data_idx[ransac_data['inliers']]] = False
labels[fancy] = processor.LABEL_CLUTTER #all surface-labeled non-plane pts
#DEBUG:
#from enthought.mayavi import mlab
#inliers = np.asarray(self.processor.pts3d_bound).T[data_idx[ransac_data['inliers']]].T
#mlab.points3d(inliers[0,:],inliers[1,:],inliers[2,:],inliers[0,:],mode='sphere',resolution=8,scale_factor=0.0015,scale_mode='none',scale_factor=0.01,colormap='jet')#,colormap='winter'
#mlab.quiver3d([ransac_fit[0][0]], [ransac_fit[0][1]], [ransac_fit[0][2]], [ransac_fit[1][0]], [ransac_fit[1][1]], [ransac_fit[1][2]], scale_factor=0.4, color=(1,0,0))
return labels
def hypothetical_node(self, sub):
hypo_nodes = []
addition_weight = 0
data = np.vstack([ self.pos_vec(n) for n in sub ])
d = (len(sub) - 2)//2-1# alsoinliers > d
if d < 0:
d = 0
result = ransac.ransac( data, self.model, 2, len(sub)*2, self.Hypothetical_threshold,
d, return_all=True, by_count = True )
if result is None:
return None
(a1, a0), inliner = result
ideal_count = len(sub)
if self.full_hypo:
ideal_count = len(self.cluster)
inliner = set(inliner)
ideal = set(range(ideal_count))
predict_idx = sorted(list(ideal.difference( inliner )))
for idx in predict_idx:
hypo_node = HypoNode()
hypo_node.x, hypo_node.y = (a1*idx + a0).tolist()
hypo_node.index = idx
hypo_nodes.append( hypo_node )
return hypo_nodes
def F_from_ransac(x1,x2,model,maxiter=5000,match_theshold=1e-6): """ Robust estimation of a fundamental matrix F from point correspondences using RANSAC (ransac.py from http://www.scipy.org/Cookbook/RANSAC). input: x1,x2 (3*n arrays) points in hom. coordinates. """ import ransac data = vstack((x1,x2)) # compute F and return with inlier index F,ransac_data = ransac.ransac(data.T,model,8,maxiter,match_theshold,20,return_all=True) return F, ransac_data['inliers']
def __match(im1, kp1, des1, im2, kp2, des2, atlas_level):
global __DEBUG_PLATE
__DEBUG_PLATE = atlas_level
if des1 is None or des2 is None:
logger.debug("Des1 or Des2 are empty. Cannot match")
return None
if len(des2) <= 1:
logger.debug("Des2 has 1 or no features")
return None
if config.MATCH_WITH_FLANN:
matches = FLANN.knnMatch(des1, des2, k=2)
else:
matches = BF.knnMatch(des1, des2, k=2)
# Apply Ratio Test
matches = [
m[0] for m in matches
if len(m) == 2 and m[0].distance < m[1].distance *
config.DISTANCE_RATIO
]
if len(matches) < config.MIN_MATCH_COUNT:
logger.debug("Matches lower than threshold. {0} < {1}", len(matches),
config.MIN_MATCH_COUNT)
return None
# For homography calculation
src_pts = np.float32([kp1[m.queryIdx].pt for m in matches])
dst_pts = np.float32([kp2[m.trainIdx].pt for m in matches])
src_kp = np.array([kp1[m.queryIdx] for m in matches])
dst_kp = np.array([kp2[m.trainIdx] for m in matches])
im1_h, im1_w = im1.shape[:2]
corners = np.float32([[0, 0], [0, im1_h - 1], [im1_w - 1, im1_h - 1],
[im1_w - 1, 0]]).reshape(-1, 1, 2)
# Calculate the homography using RANSAC
ransac_results = ransac.ransac(src_pts, dst_pts, corners, src_kp, dst_kp,
im1, im2, config.RANSAC_REPROJ_TRESHHOLD,
config.RANSAC_MAX_ITERS)
extra_data = __is_good_match(ransac_results['homography'], im2, corners)
if not extra_data:
return None
dict.update(
extra_data, {
'images':
__get_extra_images(im1, kp1, im2, kp2, matches, ransac_results,
corners),
'corners':
corners
})
return Match(atlas_level, matches, ransac_results, extra_data)
def test():
# Generate input data
mean = np.array([0.0, 0.0, 0.0])
cov = np.array([[1.0, -0.5, 0.8], [-0.5, 1.1, 0.0], [0.8, 0.0, 1.0]])
data = np.random.multivariate_normal(mean, cov, 50)
print " Shape of generated data: {}".format(data.shape)
# Create an instance of the RansacModel class and fit a plane to the noisy
# data
debug = False
plane = RansacModel(debug)
param = plane.fit(data)
print "Plane : {}".format(param)
# Create a regular grid to evaluate the fitted model
X, Y = np.meshgrid(np.arange(-3.0, 3.0, 0.5), np.arange(-3.0, 3.0, 0.5))
Z = (-param[0] * X - param[1] * Y - param[3]) / param[2]
# Plot the result
fig = plt.figure()
ax = fig.gca(projection='3d')
ax.plot_surface(X, Y, Z, rstride=1, cstride=1, alpha=0.2)
ax.scatter(data[:, 0], data[:, 1], data[:, 2], c='r', s=50)
plt.xlabel('X')
plt.ylabel('Y')
ax.set_zlabel('Z')
ax.axis('equal')
ax.axis('tight')
plt.show()
# Return error of the plane fitting
print "Error: {}".format(np.mean(np.absolute(plane.get_error(data, param))))
print "Error: {}".format(plane.get_error(data, param))
# Now, use the RANSAC algorithm
ransac_fit, ransac_data = ransac.ransac(data, plane, 4, 100, 1e-2, 10,
debug=debug, return_all=True)
# RANSAC returns a dictionary indicating the index of the inliers and
# outliers. We can use it to draw them from the original data
inliers = ransac_data['inliers']
Z = (-ransac_fit[0] * X - ransac_fit[1] * Y - ransac_fit[3]) / ransac_fit[2]
# outliers = ransac_data['outliers']
fig = plt.figure()
ax = fig.gca(projection='3d')
ax.plot_surface(X, Y, Z, rstride=1, cstride=1, alpha=0.2)
ax.scatter(data[inliers, 0], data[inliers, 1], data[inliers, 2], c='r',
s=50)
# ax.scatter(data[outliers, 0], data[outliers, 1], data[outliers, 2], c='b',
# s=50)
plt.show()
print "type of inliers: {}".format(type(data[inliers]))
print "shape of inliers: {}".format(np.shape(data[inliers]))
print "type of data: {}".format(type(data))
print "shape of data: {}".format(np.shape(data))
print "Error RANSAC: {}".format(np.mean(np.absolute(plane.get_error(data[inliers],
ransac_fit))))
print "Error RANSAC: {}".format(plane.get_error(data[inliers],
ransac_fit))
def test_ConstantModel():
true_inliers = np.random.rand(10,3)
true_outliers = np.zeros((3,3)) + 100
data = np.concatenate([true_inliers, true_outliers])
true_inlier_inds = np.arange(10,dtype=int)
est_model, est_inlier_inds, est_error = ransac(data, ConstantModel, 1, 10, 1, 5)
assert np.all(est_inlier_inds == true_inlier_inds)
def test_ransac():
model = translation_model.TranslationModel()
points1 = np.array([np.array([i, i]) for i in range(10)])
points2 = np.array([np.array([i, i]) for i in range(10, 20)])
points2[:3] = np.array([np.array([i, i]) for i in range(3)])
data = np.column_stack((points1, points2))
fit_model = ransac(data, model, 2, 100, 1, 4, False)
assert fit_model == (-10, -10)
def H_from_ransac(fp,tp,model,maxiter=1000,match_theshold=10): """ Robust estimation of homography H from point correspondences using RANSAC (ransac.py from http://www.scipy.org/Cookbook/RANSAC). input: fp,tp (3*n arrays) points in hom. coordinates. """ import ransac # group corresponding points data = vstack((fp,tp)) # compute H and return H,ransac_data = ransac.ransac(data.T,model,4,maxiter,match_theshold,10,return_all=True) return H,ransac_data['inliers']
def H_from_ransac(fp, tp, model, maxiter=1000, match_threshold=10):
"""使用RANSAC稳健性估计点对间的单应性矩阵H"""
import ransac
data = numpy.vstack((fp, tp)) # 对应点组
# 计算H并返回
H, ransac_data = ransac.ransac(data.T, model, 4, maxiter, match_threshold, 10, return_all=True)
return H, ransac_data['inliers']
def findParametersSheared(rs,N=4,shearxs=n.linspace(0.2,1,10)):
# problem in bestMatch() solved?
rs[0] = rs[0][:,[2,1,0]]
rs[1] = rs[1][:,[2,1,0]]
tmpWorked=False
params =[[1.,0.,0.,0.,1.,0.,0.,0.,1.,0.,0.,0.]]
shearMatricesL = [n.array([1.,0,x,0,1,0,0,0,1,0,0,0]) for x in range(2*len(shearxs))]
shearMatricesR = [n.array([1.,0,x,0,1,0,0,0,1,0,0,0]) for x in range(2*len(shearxs))]
for ix,x in enumerate(shearxs):
shearMatricesL[2*ix][2] = x
shearMatricesL[2*ix+1][2] = -x
shearMatricesR[2*ix][2] = -x
shearMatricesR[2*ix+1][2] = +x
for ix in range(len(shearMatricesL)):
refbeads = transform(shearMatricesL[ix],rs[0])
tmpbeads = transform(shearMatricesR[ix],rs[1])
print shearMatricesL[ix]
f0 = calcFeatures(refbeads,N=N)
# find initial matches and calculate approximate transform
tmpf = calcFeatures(tmpbeads,N=N)
if len(f0)>=len(tmpf): indList = [0,1]
else: indList = [1,0]
indices,distances = bestMatch(*tuple([[f0,tmpf][i] for i in indList]))
number = 20
number = n.max([len(indices[0])/10,number])
number = n.min([len(indices[0]),number])
model = ransac.AffineTransformationModel(rs[0][indices[indList[0]]],rs[1][indices[indList[1]]])
data = n.array(range(number))
#print indices
#print number
maxIterations = number*10
try:
fit,inliers = ransac.ransac(data,model,4,maxIterations,50,number/2,return_all=1)
tmpWorked = True
print 'OK: found parameters'
break
except ValueError:
print 'try again with different shearing parameter'
pass
if not tmpWorked:
raise Exception('Could not register bead images.')
# find better transform using redefined matches
tmpArgs = [[scaleBeads(transform(fit,rs[0]),scale),scaleBeads(rs[ibeads],scale)][i] for i in indList]
indices2,distances2 = bestMatch(*tuple(tmpArgs))
inds = n.where(distances2<5.)[0]
if len(inds)<number:
print '#'*10+'\ncheck bead registration since only a few beads are well aligned\n'+'#'*10
inds = n.array(range(number))
fit2 = affine_fit.Affine_Fit(rs[0][indices2[indList[0],inds]],rs[ibeads][indices2[indList[1],inds]]).Matrix()
params.append(fit2)
indices3,distances3 = bestMatch(transform(fit2,rs[0]),rs[ibeads])
print 'mean bead distance in pixels after alignment is\n%s' %n.mean(distances3)
return params
def H_from_ransac(fp, tp, model, maxiter = 1000, match_theshold =10):
"""use RANSAC to compute the H"""
#input :Homogeneous represent points fp, tp(3*n array)
import ransac
#correspond points
data = vstack((fp,tp))
#compute H,return
H,ransac_data = ransac.ransac(data.T, model, 4, maxiter, match_threshold, 10, return_all=True)
return H,ransac_data['inliers']
def test_ConstantModel():
true_inliers = np.random.rand(10, 3)
true_outliers = np.zeros((3, 3)) + 100
data = np.concatenate([true_inliers, true_outliers])
true_inlier_inds = np.arange(10, dtype=int)
est_model, est_inlier_inds, est_error = ransac(data, ConstantModel, 1, 10,
1, 5)
assert np.all(est_inlier_inds == true_inlier_inds)
def ransac_ellipses(detections,
ga,
model="sphere",
nb_iterations=100,
return_indices=False):
"""
RANSAC is performed in world coordinates
"""
OFD = get_OFD(ga)
centers = []
for ellipse_center, region in detections:
centers.append([
ellipse_center[0], # x
ellipse_center[1], # y
ellipse_center[2], # z
region[:, 0].min(),
region[:, 0].max(),
region[:, 1].min(),
region[:, 1].max(),
region[:, 2].min(),
region[:, 2].max()
])
centers = np.array(centers, dtype='float')
if model == "sphere":
model = SphereModel(OFD, debug=True)
else:
model = BoxModel(OFD, debug=True)
# run RANSAC algorithm
ransac_fit, ransac_data = ransac.ransac(
centers,
model,
5,
nb_iterations,
10.0,
10, # misc. parameters
debug=True,
return_all=True)
if ransac_fit is None:
print "RANSAC fiting failed"
exit(1)
inliers = ransac_data['inliers']
if return_indices:
return ransac_fit, inliers
selected_regions = []
for i, (ellipse_center, region) in enumerate(detections):
if i in inliers:
selected_regions.extend(region)
return ransac_fit, selected_regions
def RANSACFitTransformation(OffsetsAndPixels):
numInputCols = OffsetsAndPixels.shape[1]-2
data = np.concatenate( (OffsetsAndPixels[:,0:numInputCols], OffsetsAndPixels[:,numInputCols:] ) , axis=1)
model = LinearLeastSquaresModel(range(numInputCols), (numInputCols,numInputCols+1))
minSeedSize = 5
iterations = 800
maxInlierError = 240 #**2
HT = ransac.ransac(data, model, minSeedSize, iterations, maxInlierError, RANSAC_MIN_INLIERS)
return HT
def fit(data, iterations, threshold):
model = PolynomialLeastSquaresModel([0,1], [2], 2)
ransac_fit, ransac_data = ransac(data,model,
n=20,
k=iterations,
t=threshold, # threshold
d=7000,
m=200,
return_all=True)
return data[ransac_data['inliers']]
def RANSACFitTransformation(OffsetsAndPixels):
numInputCols = OffsetsAndPixels.shape[1]-2
data = np.concatenate( (OffsetsAndPixels[:,0:numInputCols], OffsetsAndPixels[:,numInputCols:] ) , axis=1)
model = LinearLeastSquaresModel(range(numInputCols), (numInputCols,numInputCols+1))
minSeedSize = 5
iterations = 800
maxInlierError = 240 #**2
HT = ransac.ransac(data, model, minSeedSize, iterations, maxInlierError, RANSAC_MIN_INLIERS)
return HT
def Haffine_from_ransac(fp, tp, model, maxiter=1000, match_threshold=10):
import ransac
data = numpy.vstack((fp, tp))
H, ransac_data = ransac.ransac(data.T,
model,
3,
maxiter,
match_threshold,
7,
return_all=True)
return H, ransac_data['inliers']
def findParametersTest(rs,N=4,useFeatures=True,infoDict=None):
origins = infoDict['origins']
positions = infoDict['positions']
spacing = infoDict['spacing']
centerOfRotation = infoDict['centerOfRotation']
axisOfRotation = infoDict['axisOfRotation']
sizes = infoDict['sizes']
# problem in bestMatch() solved?
params =[[1.,0.,0.,0.,1.,0.,0.,0.,1.,0.,0.,0.]]
for ibeads in range(1,len(rs)):
tmpWorked = False
for featureFunc in [calcFeatures,calcFeaturesFallback]:
if tmpWorked: break
for scale in n.linspace(1,10,50):
scale = n.array([scale,1.,1.])
#print scale
tmpbeads = scaleBeads(rs[ibeads],scale,verbose=True)
refbeads = scaleBeads(rs[0],scale)
if useFeatures:
f0 = featureFunc(refbeads,N=N)
# find initial matches and calculate approximate transform
tmpf = featureFunc(tmpbeads,N=N)
else:
f0 = refbeads
tmpf = tmpbeads
indices,distances = bestMatch(f0,tmpf)
number = n.max([len(indices[0])/10,20])
model = ransac.AffineTransformationModel(rs[0][indices[0]],rs[ibeads][indices[1]])
data = n.array(range(number))
try:
fit,inliers = ransac.ransac(data,model,4,number*10,50,number/2,return_all=1)
tmpWorked = True
print 'OK: found parameters'
break
except:
print 'try again with different z scale factor...'
pass
if not tmpWorked:
raise Exception('Could not register bead images.')
# find better transform using redefined matches
indices2,distances2 = bestMatch(scaleBeads(transform(fit,rs[0]),scale),scaleBeads(rs[ibeads],scale))
inds = n.where(distances2<5.)[0]
if len(inds)<5:
print '#'*10+'\ncheck bead registration since only a few beads are well aligned\n'+'#'*10
inds = n.array(range(5))
fit2 = affine_fit.Affine_Fit(rs[0][indices2[0,inds]],rs[ibeads][indices2[1,inds]]).Matrix()
params.append(fit2)
indices3,distances3 = bestMatch(transform(fit2,rs[0]),rs[ibeads])
print 'mean bead distance in pixels after alignment is\n%s' %n.mean(distances3)
return params
def estimate_tranform_ransac(top_matched_points, ransac_min_required_points,
ransac_iters, ransac_threshold,
ransac_min_inliers):
data = top_matched_points[:, 3:]
model, inliers, _, _, _ = ransac(data, ransac_min_required_points,
ransac_iters, ransac_threshold,
ransac_min_inliers)
affine_transform_matrix = np.array(
[[model[0][0], model[1][0], model[2][0]],
[model[3][0], model[4][0], model[5][0]], [0, 0, 1]])
return affine_transform_matrix, inliers
def run():
global params, residual, mean_time
gc.disable()
start_time = time()
for i in xrange(num_iterations):
try:
(params, inliers, residual) = ransac(data, model, 2, (1 - noise_ratio) * num_samples)
except ValueError:
pass
end_time = time()
mean_time = (end_time - start_time) / num_iterations
gc.enable()
def F_from_ransac(x1,x2,model,maxiter=5000,match_theshold=1e-6):
""" RANSAC(http://www.scipy.org/Cookbook/RANSAC のransac.py)
を使って点の対応から基礎行列Fをロバスト推定する。
入力: x1,x2 (3*n配列) 同次座標系の点群 """
import ransac
data = vstack((x1,x2))
# Fを計算しインライアのインデクスといっしょに返す
F,ransac_data = ransac.ransac(data.T,model,8,maxiter,match_theshold,20,return_all=True)
return F, ransac_data['inliers']
def F_from_ransac(x1,x2,model,maxiter=5000,match_theshold=1e-6):
""" Robust estimation of a fundamental matrix F from point
correspondences using RANSAC (ransac.py from
http://www.scipy.org/Cookbook/RANSAC).
input: x1,x2 (3*n arrays) points in hom. coordinates. """
import ransac
data = vstack((x1,x2))
# compute F and return with inlier index
F,ransac_data = ransac.ransac(data.T,model,8,maxiter,match_theshold,20,return_all=True)
return F, ransac_data['inliers']
def H_from_ransac(fp, tp, model, maxiter=1000, match_threshold=10):
"""使用RANSAC稳健性估计点对间的单应性矩阵H"""
import ransac
data = numpy.vstack((fp, tp)) # 对应点组
# 计算H并返回
H, ransac_data = ransac.ransac(data.T,
model,
4,
maxiter,
match_threshold,
10,
return_all=True)
return H, ransac_data['inliers']
def index():
if request.method == 'POST':
file = request.files['query_img']
# Save query image
img = Image.open(file.stream) # PIL image
uploaded_img_path = "static/uploaded/" + datetime.now().isoformat(
).replace(":", ".") + "_" + file.filename
img.save(uploaded_img_path)
# Run search
start = time.time()
query = fe.extract(img)
dists = np.linalg.norm(features - query,
axis=1) # L2 distances to features
ids = np.argsort(dists)[:30] # Top 30 results
ran = []
ids_new = []
# get des, kp from query img
img1 = cv.imread(uploaded_img_path, 0) # queryImage
# Initiate SIFT detector
sift = cv.SIFT_create()
# find the keypoints and descriptors with SIFT
kp1, des1 = sift.detectAndCompute(img1, None)
for id in ids:
ran.append(ransac(img1, des1, kp1, img_paths[id]))
ran = np.argsort(ran)[::-1]
for i in ran:
ids_new.append(ids[i])
re = len(ids)
# #
scores = []
for id in ids:
name_product, link, price = get_in4_from_positon(id)
scores.append(
[dists[id], img_paths[id], name_product, link, price])
end = time.time()
run_time = end - start
return render_template('home-03.html',
query_path=uploaded_img_path,
scores=scores,
re=re,
run_time=run_time)
# name_products=name_products,
# links=links,
# prices=prices)
else:
return render_template('home-03.html')
def match(img1, des1, kp1, img2, des2, kp2):
des1 = np.array(des1)
des2 = np.array(des2)
height1, width1, depth1 = img1.shape
height2, width2, depth2 = img2.shape
FLANN_INDEX_KDTREE = 0
index_params = dict(algorithm=FLANN_INDEX_KDTREE, trees=5)
search_params = dict(checks=50) # or pass empty dictionary
flann = cv.FlannBasedMatcher(index_params, search_params)
matches = flann.knnMatch(des1, des2, k=2)
goodMatches = []
# ratio test as per Lowe's paper
for i, (m, n) in enumerate(matches):
if m.distance < 0.7 * n.distance:
goodMatches.append(
(matches[i][0].queryIdx, matches[i][0].trainIdx))
matches = goodMatches
matches, model = ransac.ransac(matches, kp1, kp2)
print len(matches)
im1 = cv.cvtColor(img1, cv.COLOR_BGR2GRAY)
im2 = cv.cvtColor(img2, cv.COLOR_BGR2GRAY)
img3 = np.zeros((max(height1, height2), width1 + width2), np.uint8)
img3[:height1, :width1] = im1
img3[:height2, width1:width1 + width2] = im2
img3 = cv.cvtColor(img3, cv.COLOR_GRAY2BGR)
xScale = 0.4
yScale = 0.4
img3 = cv.resize(img3, (0, 0), fx=xScale, fy=yScale)
for match in matches:
ind1, ind2 = match
imgId = str(ind1) + str(ind2)
pt1 = kp1[ind1]
pt2 = kp2[ind2]
pt1 = pt1.pt
pt2 = pt2.pt
pt1 = (int(int(pt1[0]) * xScale), int(int(pt1[1]) * yScale))
pt2 = (int((int(pt2[0]) + width1) * xScale), int(int(pt2[1]) * yScale))
cv.line(img3, pt1, pt2, 255)
imgId = str(ind1) + str(ind2)
print "... displaying matches ... "
cv.imshow('img' + imgId, img3)
# plt.imshow(img3, cmap = 'gray', interpolation = 'bicubic')
# plt.xticks([]), plt.yticks([])
# plt.show()
return matches
def main():
if len(sys.argv) != 2:
print "ERROR! Correct usage is:"
print "\tpython test_ransac.py [gps_point_collection.dat]"
return
with open(sys.argv[1], "rb") as fin:
point_collection = cPickle.load(fin)
# Setup model
model = ransac.LinearLeastSquaresModel([0, 1], [2])
point_ind = random.randint(0, len(point_collection))
box_size = 100
nearby_point = []
for pt in point_collection:
if pt[0] >= point_collection[point_ind][0] - box_size\
and pt[0] <= point_collection[point_ind][0] + box_size\
and pt[1] >= point_collection[point_ind][1] - box_size\
and pt[1] <= point_collection[point_ind][1] + box_size:
nearby_point.append((pt[0], pt[1], -100000))
all_data = np.array(nearby_point)
print all_data.shape
d = math.floor(0.2 * len(nearby_point))
ransac_fit, ransac_data = ransac.ransac(all_data,
model,
d,
1000,
100,
d,
return_all=True)
sort_idxs = np.argsort(all_data[:, 0])
data_sorted = all_data[sort_idxs]
fit_y = (-100000 - ransac_fit[0] * data_sorted[:, 0]) / ransac_fit[1]
fig = plt.figure(figsize=(16, 16))
ax = fig.add_subplot(111, aspect='equal')
ax.plot([pt[0] for pt in point_collection],
[pt[1] for pt in point_collection],
'.',
color='gray')
ax.plot([pt[0] for pt in nearby_point], [pt[1] for pt in nearby_point],
'.')
ax.plot(data_sorted[:, 0], fit_y, 'r-')
plt.show()
def match(img1, des1, kp1, img2, des2, kp2):
des1 = np.array(des1)
des2 = np.array(des2)
height1, width1, depth1 = img1.shape
height2, width2, depth2 = img2.shape
FLANN_INDEX_KDTREE = 0
index_params = dict(algorithm = FLANN_INDEX_KDTREE, trees = 5)
search_params = dict(checks=50) # or pass empty dictionary
flann = cv.FlannBasedMatcher(index_params,search_params)
matches = flann.knnMatch(des1,des2,k=2)
goodMatches = []
# ratio test as per Lowe's paper
for i,(m,n) in enumerate(matches):
if m.distance < 0.7*n.distance:
goodMatches.append((matches[i][0].queryIdx,matches[i][0].trainIdx))
matches = goodMatches
matches, model = ransac.ransac(matches, kp1, kp2)
print len(matches)
im1 = cv.cvtColor(img1,cv.COLOR_BGR2GRAY)
im2 = cv.cvtColor(img2,cv.COLOR_BGR2GRAY)
img3 = np.zeros((max(height1,height2),width1+width2), np.uint8)
img3[:height1, :width1] = im1
img3[:height2, width1:width1+width2] = im2
img3 = cv.cvtColor(img3,cv.COLOR_GRAY2BGR)
xScale = 0.4
yScale = 0.4
img3 = cv.resize(img3, (0,0), fx=xScale, fy=yScale)
for match in matches:
ind1, ind2 = match
imgId = str(ind1)+str(ind2)
pt1 = kp1[ind1]
pt2 = kp2[ind2]
pt1 = pt1.pt
pt2 = pt2.pt
pt1 = (int(int(pt1[0])*xScale), int(int(pt1[1])*yScale))
pt2 = (int((int(pt2[0]) + width1)*xScale), int(int(pt2[1])*yScale))
cv.line(img3, pt1, pt2, 255)
imgId = str(ind1)+str(ind2)
print "... displaying matches ... "
cv.imshow('img' + imgId, img3)
# plt.imshow(img3, cmap = 'gray', interpolation = 'bicubic')
# plt.xticks([]), plt.yticks([])
# plt.show()
return matches
def H_from_ransac(fp,tp,model,maxiter=1000,match_theshold=10):
""" Robust estimation of homography H from point
correspondences using RANSAC (ransac.py from
http://www.scipy.org/Cookbook/RANSAC).
input: fp,tp (3*n arrays) points in hom. coordinates. """
import ransac
# group corresponding points
data = vstack((fp,tp))
# compute H and return
H,ransac_data = ransac.ransac(data.T,model,4,maxiter,match_theshold,10,return_all=True)
return H,ransac_data['inliers']
def pcl_callback(data):
global best_plane
global is_best_plane_found
if not is_best_plane_found:
print("Best plane found")
is_best_plane_found = True
cloud_points = list(
pc2.read_points(data, field_names=("x", "y", "z"), skip_nans=True))
length_of_sublist = int(.0015 * len(cloud_points))
cloud_points_sublist = random.sample(set(cloud_points),
length_of_sublist)
best_plane = ransac.ransac(cloud_points_sublist, 15)
def H_from_ransac(fp, tp, model, maxiter=1000, match_threshold=10):
""" RANSACを用いて対応点からホモグラフィー行列Hをロバストに推定する
(ransac.py は http://www.scipy.org/Cookbook/RANSAC を使用)
入力: fp,tp (3*n 配列) 同次座標での点群 """
# 対応点をグループ化する
data = np.vstack((fp, tp))
# Hを計算して返す
H, ransac_data = ransac.ransac(data.T,
model,
4,
maxiter,
match_threshold,
10,
return_all=True)
return H, ransac_data['inliers']
def H_from_ransac(fp,tp,model,maxiter=1000,match_threshold=10):
""" RANSACを用いて対応点からホモグラフィー行列Hをロバストに推定する
(ransac.py は http://www.scipy.org/Cookbook/RANSAC を使用)
入力: fp,tp (3*n 配列) 同次座標での点群 """
import ransac
# 対応点をグループ化する
data = vstack((fp,tp))
# Hを計算して返す
H,ransac_data = ransac.ransac(data.T,model,4,maxiter,match_threshold,10,
return_all=True)
return H,ransac_data['inliers']
def ransac_ellipses(detections, ga, model="sphere", nb_iterations=100, return_indices=False):
"""
RANSAC is performed in world coordinates
"""
OFD = get_OFD(ga)
centers = []
for ellipse_center, region in detections:
centers.append(
[
ellipse_center[0], # x
ellipse_center[1], # y
ellipse_center[2], # z
region[:, 0].min(),
region[:, 0].max(),
region[:, 1].min(),
region[:, 1].max(),
region[:, 2].min(),
region[:, 2].max(),
]
)
centers = np.array(centers, dtype="float")
if model == "sphere":
model = SphereModel(OFD, debug=True)
else:
model = BoxModel(OFD, debug=True)
# run RANSAC algorithm
ransac_fit, ransac_data = ransac.ransac(
centers, model, 5, nb_iterations, 10.0, 10, debug=True, return_all=True # misc. parameters
)
if ransac_fit is None:
print "RANSAC fiting failed"
exit(1)
inliers = ransac_data["inliers"]
if return_indices:
return ransac_fit, inliers
selected_regions = []
for i, (ellipse_center, region) in enumerate(detections):
if i in inliers:
selected_regions.extend(region)
return ransac_fit, selected_regions
def ransac_ellipses( detected_centers,
detected_regions,
img,
ga,
nb_iterations=100,
debug=False ):
"""
RANSAC is performed in mm (isotropic), but not in world coordinates
"""
OFD = get_OFD(ga,centile=50)
centers = []
for center, region in zip(detected_centers,detected_regions):
c = center[::-1]*img.header['pixelSize'][:3]
r = np.hstack( (region, # region is xy in image coordinates
[[center[0]]]*region.shape[0]) )*img.header['pixelSize'][:3]
centers.append([c[0], # x
c[1], # y
c[2], # z
r[:,0].min(),r[:,0].max(),
r[:,1].min(),r[:,1].max(),
r[:,2].min(),r[:,2].max()])
centers = np.array(centers,dtype='float')
model = BoxModel(OFD,debug=debug)
# run RANSAC algorithm
ransac_fit, inliers = ransac.ransac( centers,
model,
4, # minimum number of data values required to fit the model
nb_iterations, # maximum number of iterations
10.0, # threshold for determining when a data point fits a model
4, # number of close data values required to assert that a model fits well to data
debug=debug,
return_all=True )
if ransac_fit is None:
print "RANSAC fiting failed"
exit(1)
(center, u, ofd) = ransac_fit
center /= img.header['pixelSize'][:3]
return img.ImageToWorld(center), inliers
def H_from_ransac(fp,tp,model,maxiter=100,match_threshold=10):
""" RANSACを用いて対応点からホモグラフィー行列Hをロバストに推定する(ransac.pyを使用)
入力：fp,tp(3*n 配列) 同次座標での点群 """
#対応点をグループ化する
data = np.vstack((fp,tp))
#Hを計算して返す
H, ransac_data = ransac.ransac(data.T,model,4,maxiter,match_threshold,10,return_all=True)
return H, ransac_data['inliers']
#H:結果のホモグラフィー行列 inlier:インライア。外れ値でないと判断された点。
def extract_ransac_line(data,
min_samples: int) -> Optional[Tuple[Line, np.ndarray]]:
model, inliers = ransac(data,
WallModel,
min_samples=min_samples,
residual_threshold=0.5,
max_trials=1000)
if model is None:
return None
# remove inliers
updated_data = data[~inliers]
start, end = find_bounds(model, data[inliers])
line = Line(model, start, end, data[inliers])
return (line, updated_data)
def main():
if len(sys.argv) != 2:
print "ERROR! Correct usage is:"
print "\tpython test_ransac.py [gps_point_collection.dat]"
return
with open(sys.argv[1], "rb") as fin:
point_collection = cPickle.load(fin)
# Setup model
model = ransac.LinearLeastSquaresModel([0,1],[2])
point_ind = random.randint(0, len(point_collection))
box_size = 100
nearby_point = []
for pt in point_collection:
if pt[0] >= point_collection[point_ind][0] - box_size\
and pt[0] <= point_collection[point_ind][0] + box_size\
and pt[1] >= point_collection[point_ind][1] - box_size\
and pt[1] <= point_collection[point_ind][1] + box_size:
nearby_point.append((pt[0], pt[1], -100000))
all_data = np.array(nearby_point)
print all_data.shape
d = math.floor(0.2*len(nearby_point))
ransac_fit, ransac_data = ransac.ransac(all_data,
model, d, 1000, 100, d,
return_all=True)
sort_idxs = np.argsort(all_data[:,0])
data_sorted = all_data[sort_idxs]
fit_y = (-100000 - ransac_fit[0]*data_sorted[:,0])/ransac_fit[1]
fig = plt.figure(figsize=(16,16))
ax = fig.add_subplot(111, aspect='equal')
ax.plot([pt[0] for pt in point_collection],
[pt[1] for pt in point_collection],
'.', color='gray')
ax.plot([pt[0] for pt in nearby_point],
[pt[1] for pt in nearby_point],
'.')
ax.plot(data_sorted[:,0],
fit_y, 'r-')
plt.show()
def H_from_ransac(fp, tp, model, maxiter=1000, match_theshold=10):
""" 使用RANSAC 稳健性估计点对应间的单应性矩阵H（ransac.py 为从
http://www.scipy.org/Cookbook/RANSAC 下载的版本）"""
# 输入：齐次坐标表示的点fp，tp（3×n 的数组）
import ransac
# 对应点组
data = vstack((fp, tp))
# 计算H，并返回
H, ransac_data = ransac.ransac(data.T,
model,
4,
maxiter,
match_theshold,
10,
return_all=True)
return H #, ransac_data['inliers']
def F_from_ransac(x1, x2, model, maxiter=5000, match_threshold=1e-6):
"""
ransacを使って点の対応から基礎行列Fをロバスト推定する。
Args:
x1 (array):(3*n配列)同次座標系の点群
"""
data = np.vstack((x1, x2))
# Fを計算し、インライアのインデックスと一緒に返す。
F, ransac_data = ransac.ransac(data.T,
model,
n=8,
k=maxiter,
t=match_threshold,
d=20,
return_all=True)
return F, ransac_data['inliers']
def F_from_ransac(x1, x2, model, maxiter=5000, match_theshold=1e-6):
""" 使用 RANSAN 方法(ransac.py，来自 http://www.scipy.org/Cookbook/RANSAC)，
从点对应中稳健地估计基础矩阵 F 输入:使用齐次坐标表示的点 x1，x2(3×n 的数组)"""
import ransac
data = vstack((x1, x2))
# 计算F，并返回正确点索引
F, ransac_data = ransac.ransac(data.T,
model,
8,
maxiter,
match_theshold,
20,
return_all=True)
return F, ransac_data['inliers']
def ransac_dlt(X3d, x2d,
n = 6, # six is minimum
k = 200, # do it 200 times
t = 15.0, # mean reprojection error should be less than 15
d = 8,
debug=False,
):
"""perform the DLT in RANSAC
Params
------
n: the minimum number of data values required to fit the model
k: the maximum number of iterations allowed in the algorithm
t: a threshold value for determining when a data point fits a model
d: the number of close data values required to assert that a model fits well to data
"""
model = DltRansacModel(X3d,x2d)
data = np.arange( len(X3d) )
return ransac.ransac(data,model,n,k,t,d,debug=debug,return_all=True)
scale = n.array([scale,1.,1.])
print scale
tmpbeads = scaleBeads(rs[ibeads],scale)
refbeads = scaleBeads(rs[0],scale)
f0 = calcFeatures(refbeads,N=N)
# find initial matches and calculate approximate transform
tmpf = calcFeatures(tmpbeads,N=N)
indices,distances = bestMatch(f0,tmpf)
distancess.append(distances)
number = n.max([len(indices)/10,20])
#number = len(indices[0])
inds = n.array(range(number))
model = ransac.AffineTransformationModel(rs[0][indices[0]],rs[1][indices[1]])
data = n.array(range(number))
try:
fit,inliers = ransac.ransac(data,model,4,number*10,50,number/2,return_all=1)
break
except:
print 'did not work'
pass
# find better transform using redefined matches
indices2,distances2 = bestMatch(transform(fit,rs[0]),rs[1])
number = n.max([len(indices2[0])/2,10])
inds = n.array(range(number))
model = ransac.AffineTransformationModel(rs[0][indices2[0]],rs[1][indices2[1]])
data = n.array(range(number))
fit2 = ransac.ransac(data,model,number/5,100,2,number/2)
params.append(fit2)
def run():
match = Matcher()
img = CVImage('/home/cesar/Documentos/Computer_Vision/01/image_0')
# img = CVImage('/home/cesar/Documentos/Computer_Vision/images_test')
# Load images
img.read_image()
img.copy_image()
img.acquire()
# t = threading.Thread(target=plot_image, args=(img.new_image, ))
# t.start()
# Correlate
p1, p2 = correlate_image(match, img, 2, 7)
print ("Total number of keypoints in second image: \
{}".format(len(match.global_kpts1)))
print ("Total number of keypoints in first image: \
{}".format(len(match.global_kpts2)))
if not match.is_minmatches:
print "There aren't matches after filtering. Iterate to next image..."
return
# Plot keypoints
plot_matches(match, img)
# t.__stop()
# Now, estimate F
vo = VisualOdometry()
match.global_kpts1, match.global_kpts2 = \
vo.EstimateF_multiprocessing(match.global_kpts2, match.global_kpts1)
# Get structure of the scene, up to a projectivity
scene = get_structure(match, img, vo)
# Optimize F
# param_opt, param_cov = vo.optimize_F(match.global_kpts1, match.global_kpts2)
# vo.cam2.set_P(param_opt[:9].reshape((3, 3)))
# scene = vo.recover_structure(param_opt)
# Plot it
plot_scene(scene)
# Get the Essential matrix
vo.E_from_F()
print vo.F
print vo.E
# Recover pose
R, t = vo.get_pose(match.global_kpts1, match.global_kpts2,
vo.cam1.focal, vo.cam1.pp)
print R
print t
# Compute camera matrix 2
print "CAM2", vo.cam2.P
vo.cam2.compute_P(R, t)
print "CAM2", vo.cam2.P
# Get the scene
scene = get_structure_normalized(match, img, vo)
plot_scene(scene)
# What have we stored?
print ("Permanent Keypoints in the first image stored: \
{}".format(type(match.curr_kp[0])))
print ("Permanent descriptors in the first image stored: \
{}".format(len(match.curr_dsc)))
print ("Format of global keypoints: \
{}".format(type(match.global_kpts1)))
print ("Format of global keypoints: \
{}".format(type(match.global_kpts1[0])))
print ("Shape of global kpts1: {}".format(np.shape(match.global_kpts1)))
# print ("global keypoint: \
# {}".format(match.global_kpts1[0]))
# Acquire image
img.copy_image()
img.acquire()
d, prev_points, points_tracked = match.lktracker(img.prev_image, \
img.new_image,
match.global_kpts2)
print ("Points tracked: \ {}".format(len(points_tracked)))
plot_two_points(np.reshape(match.global_kpts2,
(len(match.global_kpts2), 2)),
prev_points, img)
test = []
for (x, y), good_flag in zip(match.global_kpts2, d):
if not good_flag:
continue
test.append((x, y))
# plot_two_points(np.reshape(match.global_kpts2, (len(match.global_kpts2), 2)),
# np.asarray(points_tracked), img)
plot_two_points(np.asarray(test), np.asarray(points_tracked), img)
# points, st, err = cv2.calcOpticalFlowPyrLK(img.prev_grey, img.new_image,
# match.global_kpts2, None,
# **lk_params)
# print len(points)
print "Shape of p1: {}".format(np.shape(p1))
plane = vo.opt_triangulation(p1, p2,
vo.cam1.P, vo.cam2.P)
plot_scene(plane)
print "Shpe of plane: {}".format(np.shape(plane))
print "Type of plane: {}".format(type(plane))
print np.transpose(plane[:, :3])
plane = np.transpose(plane)
print "shape plane: {}".format(np.shape(plane))
plane_inhomogeneous = np.delete(plane, 3, 1)
print "shape plane: {}".format(np.shape(plane_inhomogeneous))
print plane_inhomogeneous[:3, :]
# Use ransac to fit a plane
debug = False
plane_model = RansacModel(debug)
ransac_fit, ransac_data = ransac.ransac(plane_inhomogeneous,
plane_model,
4, 1000, 1e-4, 50,
debug=debug, return_all=True)
print "Ransac fit: {}".format(ransac_fit)
# PLot the plane
X, Y = np.meshgrid(np.arange(-0.3, 0.7, 0.1), np.arange(0, 0.5, 0.1))
Z = -(ransac_fit[0] * X - ransac_fit[1] * Y - ransac_fit[3]) / ransac_fit[2]
plot_plane(X, Y, Z, plane_inhomogeneous[ransac_data['inliers']])
def Haffine_from_ransac(fp, tp, model, maxiter=1000, match_threshold=10): import ransac data = numpy.vstack((fp, tp)) H, ransac_data = ransac.ransac(data.T, model, 3, maxiter, match_threshold, 7, return_all=True) return H, ransac_data['inliers']
def align_by_sift(self,xy1,xy2,window=70):
"""take two points in the image, and calculate SIFT features image around those two points
cutting out size window
keywords)
xy1) a (x,y) tuple specifying point 1, the point that should be fixed
xy2) a (x,y) tuple specifiying point 2, the point that should be moved
window) the size of the patch to cutout (+/- window around the points) for calculating the correlation (default = 70 um)
returns) (maxC,dxy_um)
maxC)the maximal correlation measured
dxy_um) the (x,y) tuple which contains the shift in microns necessary to align point xy2 with point xy1
"""
print "starting align by sift"
pixsize=self.imgCollection.get_pixel_size()
#cutout the images around the two points
(x1,y1)=xy1
(x2,y2)=xy2
one_cut=self.cutout_window(x1,y1,window)
two_cut=self.cutout_window(x2,y2,window)
#one_cuta=np.minimum(one_cut*256.0/self.maxvalue,255.0).astype(np.uint8)
#two_cuta=np.minimum(two_cut*256.0/self.maxvalue,255.0).astype(np.uint8)
one_cuta=cv2.equalizeHist(one_cut)
two_cuta=cv2.equalizeHist(two_cut)
sift = cv2.SIFT(nfeatures=500,contrastThreshold=.2)
kp1, des1 = sift.detectAndCompute(one_cuta,None)
kp2, des2 = sift.detectAndCompute(two_cuta,None)
print "features1:%d"%len(kp1)
print "features2:%d"%len(kp2)
img_one = cv2.drawKeypoints(one_cuta,kp1)
img_two = cv2.drawKeypoints(two_cuta,kp2)
#image2=self.two_axis.imshow(img_two)
# FLANN parameters
FLANN_INDEX_KDTREE = 0
index_params = dict(algorithm = FLANN_INDEX_KDTREE, trees = 5)
search_params = dict(checks=50) # or pass empty dictionary
flann = cv2.FlannBasedMatcher(index_params,search_params)
matches = flann.knnMatch(des1,des2,k=2)
# Need to draw only good matches, so create a mask
matchesMask = np.zeros(len(matches))
kp1matchIdx=[]
kp2matchIdx=[]
# ratio test as per Lowe's paper
for i,(m,n) in enumerate(matches):
if m.distance < 0.9*n.distance:
kp1matchIdx.append(m.queryIdx)
kp2matchIdx.append(m.trainIdx)
p1 = np.array([kp1[i].pt for i in kp1matchIdx])
p2 = np.array([kp2[i].pt for i in kp2matchIdx])
# p1c = [pt-np.array[window,window] for pt in p1]
# p2c = [pt-np.array[window,window] for pt in p2]
kp1m = [kp1[i] for i in kp1matchIdx]
kp2m = [kp2[i] for i in kp2matchIdx]
#print "kp1matchshape"
#print matchesMask
#print len(kp1match)
#print len(kp2match)
#draw_params = dict(matchColor = (0,255,0),
# singlePointColor = (255,0,0),
# matchesMask = matchesMask,
# flags = 0)
#img3 = cv2.drawMatches(one_cut,kp1,two_cut,kp2,matches,None,**draw_params)
transModel=ransac.RigidModel()
bestModel,bestInlierIdx=ransac.ransac(p1,p2,transModel,2,300,20.0,3,debug=True,return_all=True)
if bestModel is not None:
the_center = np.array([[one_cut.shape[0]/2,one_cut.shape[1]/2]])
trans_center=transModel.transform_points(the_center,bestModel)
offset=the_center-trans_center
xc=x2+offset[0,0]*pixsize
yc=y2-offset[0,1]*pixsize
#newcenter=Line2D([trans_center[0,0]+one_cut.shape[1]],[trans_center[0,1]],marker='+',markersize=7,markeredgewidth=1.5,markeredgecolor='r')
#oldcenter=Line2D([the_center[0,0]],[the_center[0,1]],marker='+',markersize=7,markeredgewidth=1.5,markeredgecolor='r')
dx_um=-bestModel.t[0]*pixsize
dy_um=-bestModel.t[1]*pixsize
print "matches:%d"%len(kp1matchIdx)
print "inliers:%d"%len(bestInlierIdx)
print ('translation',bestModel.t)
print ('rotation',bestModel.R)
mask = np.zeros(len(p1), np.bool_)
mask[bestInlierIdx]=1
#img3 = self.explore_match(one_cuta,kp1m,two_cuta,kp2m,mask)
#self.corr_axis.cla()
#self.corr_axis.imshow(img3)
#self.corr_axis.add_line(newcenter)
#self.corr_axis.add_line(oldcenter)
#self.repaint()
#self.paintImageOne(img_one,xy=xy1)
#paint the patch around the second point in its axis
#self.paintImageTwo(img_two,xy=xy2)
#paint the correlation matrix in its axis
#self.paintCorrImage(corrmat, dxy_pix,skip)
print (dx_um,dy_um)
self.paintImageOne(img_one,xy=xy1)
self.paintImageTwo(img_two,xy=xy2,xyp=(x2-dx_um,y2-dy_um))
return ((dx_um,dy_um),len(bestInlierIdx))
else:
self.paintImageOne(img_one,xy=xy1)
self.paintImageTwo(img_two,xy=xy2)
return ((0.0,0.0),0)