def fit_homography(self, pts1, pts2, homography_type):
"""Fit special class of homomgraphy.
:param homography_type:
:param homography_type: Integer indicating the type of homography to
fit (0 - translation, 1 - rigid, 2 - similarity, 3 - affine, 4 -
fully homography).
:type homography_type: int
"""
if homography_type == 0:
# Translation.
if pts1 is None or pts2 is None or len(pts1) < 1:
self._warn_need_at_least_n_points(1, 'translation')
return
H = np.identity(3)
delta = np.mean(pts2 - pts1, 0)
H = np.identity(3)
H[:2,2] = delta
elif homography_type == 1:
# Rigid.
if pts1 is None or pts2 is None or len(pts1) < 2:
self._warn_need_at_least_n_points(2, 'rigid')
return
H = transformations.affine_matrix_from_points(pts1.T, pts2.T,
shear=False,
scale=False)
elif homography_type == 2:
# Similarity.
if pts1 is None or pts2 is None or len(pts1) < 2:
self._warn_need_at_least_n_points(2, 'rigid')
return
H = transformations.affine_matrix_from_points(pts1.T, pts2.T,
shear=False,
scale=True)
elif homography_type == 3:
# Affine.
if pts1 is None or pts2 is None or len(pts1) < 3:
self._warn_need_at_least_n_points(3, 'affine')
return
H = transformations.affine_matrix_from_points(pts1.T, pts2.T,
shear=True,
scale=True)
elif homography_type == 4:
# Homography.
if pts1 is None or pts2 is None or len(pts1) < 4:
self._warn_need_at_least_n_points(4, 'homography')
return
H = cv2.findHomography(pts1.reshape(-1,1,2),
pts2.reshape(-1,1,2))[0]
else:
raise Exception()
return H
def matrixFromShear(self, shear, hcoord):
from transformations import affine_matrix_from_points
# sfaces and tfaces are the face coordinates
sfaces = np.zeros((3, 2), float)
tfaces = np.zeros((3, 2), float)
for n in range(3):
(vn1, vn2, sfaces[n, 0], sfaces[n, 1]) = shear[n]
tfaces[n, 0] = hcoord[vn1][n]
tfaces[n, 1] = hcoord[vn2][n]
# sverts and tverts are the vertex coordinates
sverts = []
tverts = []
for i in [0, 1]:
for j, k in [(0, 0), (0, 1), (1, 1), (1, 0)]:
sverts.append(
np.array((sfaces[0, i], sfaces[1, j], sfaces[2, k])))
tverts.append(
np.array((tfaces[0, i], tfaces[1, j], tfaces[2, k])))
sbox = vertsToNumpy(sverts)
tbox = vertsToNumpy(tverts)
mat = affine_matrix_from_points(sbox, tbox)
return mat[:3, :3]
def get_SVD(self):
"""
Get the affine transformation matrix that best matches the moelcules cxns
to each rod cxn by SVD
We do a Euclidean (rigid) transform
"""
print("\n\nCalculating intial affine transformations:")
print(
"\n\nM = affine transformation for best fit of mol cxn -> rod cxn:"
)
for i in range(len(self.cxns)):
for it in self.mol.permutations:
for dc in self.dc_ints:
pass
a = self.mol.cxns[0:3, :]
b = self.cxn_xyz[i]
M = trans.affine_matrix_from_points(a,
b,
shear=False,
scale=False,
usesvd=True)
alpha, beta, gamma = trans.euler_from_matrix(M)
translations = M[0:3, 3]
conn_start_ind = 1 + len(self.rods) + i * 6
self.opt_vec[conn_start_ind + 0] = alpha
self.opt_vec[conn_start_ind + 1] = beta
self.opt_vec[conn_start_ind + 2] = gamma
self.opt_vec[conn_start_ind + 3] = translations[0]
self.opt_vec[conn_start_ind + 4] = translations[1]
self.opt_vec[conn_start_ind + 5] = translations[2]
print(M)
def get_SVD(self):
"""
Get the affine transformation matrix that best matches the moelcules cxns
to each rod cxn by SVD
We do a Euclidean (rigid) transform
"""
print("\n\nCalculating intial affine transformations:")
print("\n\nM = affine transformation for best fit of mol cxn -> rod cxn:")
for i in range(len(self.cxns)):
for it in self.mol.permutations:
for dc in self.dc_ints:
pass
a = self.mol.cxns[0:3,:]
b = self.cxn_xyz[i]
M = trans.affine_matrix_from_points(a, b, shear = False, scale = False, usesvd = True)
alpha, beta, gamma = trans.euler_from_matrix(M)
translations = M[0:3,3]
conn_start_ind = 1 + len(self.rods) + i*6
self.opt_vec[conn_start_ind+0] = alpha
self.opt_vec[conn_start_ind+1] = beta
self.opt_vec[conn_start_ind+2] = gamma
self.opt_vec[conn_start_ind+3] = translations[0]
self.opt_vec[conn_start_ind+4] = translations[1]
self.opt_vec[conn_start_ind+5] = translations[2]
print(M)
def solve_rotation():
global calib_points,points
global pts
pts = []
for rimg,p in zip(rimgs,calib_points):
pts2 = []
for cp in p:
pts2.append(rimg.xyz[int(cp[1]),int(cp[0]),:])
pts.append(pts2)
pts = np.array(pts)
# Register each to the first
t0 = pts[0].mean(1)
fixes = []
import transformations
global mats
mats = [np.eye(4)]
for p in pts[1:]:
v0 = np.concatenate((pts[0],pts[0]))
v1 = np.concatenate((p,p))
mat = transformations.affine_matrix_from_points(v0.T,v1.T,shear=False,scale=False)
mats.append(mat)
print v0 - v1
print (np.dot(mat[:3,:3],v0.T).T+mat[:3,3].T) - v1
points[1].RT = np.linalg.inv(mats[1])
return mats
def matrixFromShear(self, shear, hcoord):
from transformations import affine_matrix_from_points
# sfaces and tfaces are the face coordinates
sfaces = np.zeros((3, 2), float)
tfaces = np.zeros((3, 2), float)
for n in range(3):
(vn1, vn2, sfaces[n, 0], sfaces[n, 1], side) = shear[n]
tfaces[n, 0] = hcoord[vn1][n]
tfaces[n, 1] = hcoord[vn2][n]
# sverts and tverts are the vertex coordinates
sverts = []
tverts = []
for i in [0, 1]:
for j, k in [(0, 0), (0, 1), (1, 1), (1, 0)]:
sverts.append(
np.array((sfaces[0, i], sfaces[1, j], sfaces[2, k])))
tverts.append(
np.array((tfaces[0, i], tfaces[1, j], tfaces[2, k])))
sbox = vertsToNumpy(sverts)
tbox = vertsToNumpy(tverts)
mat = affine_matrix_from_points(sbox, tbox)
return mat[:3, :3]
def update_matrix(self, state):
buffer_len = min(self.max_points, self.npoints)
self.transform = affine_matrix_from_points(
self.realsense_buffer[:, :buffer_len],
self.marvelmind_buffer[:, :buffer_len],
shear=False,
scale=True)
self.angle = np.arctan2(self.transform[1, 0], self.transform[0, 0])
state.rs_to_mm_scale = norm(self.transform[:2, 0])
state.rs_to_mm_offset = self.transform[:2, 2]
state.rs_to_mm_angle = self.angle
self.last_update_time = state.time
def scale_and_translate(Tlistgroundtruth, Tlistartoolkit):
ground_truth_pointcloud = np.asarray([utils.apply_transform(T, np.zeros(3))
for T in Tlistgroundtruth])
artoolkit_pointcloud = np.asarray([utils.apply_transform(T, np.zeros(3))
for T in Tlistartoolkit])
Tscale = tf.affine_matrix_from_points(artoolkit_pointcloud.T,
ground_truth_pointcloud.T,
shear=False,
scale=True)
#print("Scaling by {0}".format(tf.scale_from_matrix(Tscale)[0]))
try:
print("translation by {0}".format(tf.translation_from_matrix(Tscale)))
print("rotation by {0}".format(tf.rotation_from_matrix(Tscale)[0]))
except ValueError:
pass
Tlistartoolkit = [tf.concatenate_matrices(Tscale, T) for T in Tlistartoolkit]
return Tlistartoolkit
cal.min_temp = min_temp
cal.max_temp = max_temp
if len(mag_data):
mag_array = np.array(mag_data, dtype=np.float64)
print 'mag_array:', mag_array
#mag_len = mag_array.shape[0]
#mag_min = mag_array[:,0:3].min() # note: [:,start_index:length]
#mag_max = mag_array[:,0:3].max()
#print "mag range:", mag_min, mag_max
ideal_array = mag_array[:,3:6]
sense_array = mag_array[:,0:3]
affine = transformations.affine_matrix_from_points(sense_array.T, ideal_array.T, usesparse=True)
mag_cal.mag_affine = affine
print "affine:"
np.set_printoptions(precision=10,suppress=True)
print affine
scale, shear, angles, translate, perspective = transformations.decompose_matrix(affine)
print ' scale:', scale
print ' shear:', shear
print ' angles:', angles
print ' trans:', translate
print ' persp:', perspective
cal.save_xml(cal_file)
# ============================= PLOTS ======================================
m.calibrate_bulk(sense_array)
print "b:", m.b
print "A_1:", m.A_1
ef_data = []
for s in sense_array:
ef = m.map(s)
#norm = np.linalg.norm(ef)
#ef /= norm
ef_data.append(ef)
#print 's:', s, 'ef:', ef
ef_array = np.array(ef_data)
print "ready to compute affine transformation"
affine = transformations.affine_matrix_from_points(sense_array.T, ef_array.T, usesparse=True)
print "affine ef:"
np.set_printoptions(precision=10,suppress=True)
print affine
scale, shear, angles, translate, perspective = transformations.decompose_matrix(affine)
print ' scale:', scale
print ' shear:', shear
print ' angles:', angles
print ' trans:', translate
print ' persp:', perspective
cal_dir = os.path.join(args.cal, imu_sn)
if not os.path.exists(cal_dir):
os.makedirs(cal_dir)
cal_file = os.path.join(cal_dir, "imucal.json")
cal = imucal.Calibration()
import mag
m = mag.Magnetometer(F=1.0)
m.calibrate_bulk(sense_array)
print "b:", m.b
print "A_1:", m.A_1
ef_data = []
for s in sense_array:
ef = m.map(s)
norm = np.linalg.norm(ef)
#ef /= norm
ef_data.append(ef)
#print 's:', s, 'ef:', ef
ef_array = np.array(ef_data)
affine = transformations.affine_matrix_from_points(sense_array.T, ef_array.T)
print "affine ef:"
np.set_printoptions(precision=10,suppress=True)
print affine
scale, shear, angles, translate, perspective = transformations.decompose_matrix(affine)
print ' scale:', scale
print ' shear:', shear
print ' angles:', angles
print ' trans:', translate
print ' persp:', perspective
cal_dir = os.path.join(args.cal, imu_sn)
if not os.path.exists(cal_dir):
os.makedirs(cal_dir)
cal_file = os.path.join(cal_dir, "imucal.xml")
cal = imucal.Calibration(cal_file)
#!/usr/bin/python
import sys
sys.path.append('../lib')
import transformations
v0 = [[0, 1031, 1031, 0], [0, 0, 1600, 1600]]
v1 = [[675, 826, 826, 677], [55, 52, 281, 277]]
#weights = [1.0, 1.0, 1.0, 1.0]
weights = [0.1, 0.01, 0.1, 0.2]
print "original"
print transformations.affine_matrix_from_points(v0, v1, shear=False)
print "weighted"
print transformations.affine_matrix_from_points_weighted(v0,
v1,
weights,
shear=False)
#!/usr/bin/env python
import transformations
v0 = [[
-12.0397663116, -12.0397663116, 37.4785995483, 33.3264122009,
-2.31048417091, 48.1299972534, 17.9676646186
],
[
-4.88692569, -4.88692569733, 36.8298416138, 55.7662467957,
-7.9343585968, 35.7186012268, 69.0059051514
]]
v1 = [[
38.432256, 38.432053, 38.431678, 38.431578, 38.432225, 38.431622, 38.431589
],
[
-78.876043, -78.876251, -78.876168, -78.875985, -78.876110,
-78.876254, -78.875746
]]
affine = transformations.affine_matrix_from_points(v0, v1)
print(affine)
#!/usr/bin/python
import sys
sys.path.append('../lib')
import transformations
v0 = [[0, 1031, 1031, 0], [0, 0, 1600, 1600]]
v1 = [[675, 826, 826, 677], [55, 52, 281, 277]]
#weights = [1.0, 1.0, 1.0, 1.0]
weights = [0.1, 0.01, 0.1, 0.2]
print "original"
print transformations.affine_matrix_from_points(v0, v1, shear=False)
print "weighted"
print transformations.affine_matrix_from_points_weighted(v0, v1, weights, shear=False)
def ICP(self, i1, i2):
if i1 == i2:
return []
# create a new copy of i2.coord_list
coord_list2 = np.array(i2.coord_list, copy=True).T
#print coord_list2
# make homogeneous
newcol = np.ones( (1, coord_list2.shape[1]) )
#print newcol
#print coord_list2.shape, newcol.shape
coord_list2 = np.vstack((coord_list2, newcol))
done = False
while not done:
# find a pairing for the closest points. If the closest point
# is already taken, then if the distance is less, the old
# pairing is dropped and the new one accepted. If the distance
# is greater than a previous pairing, the new pairing is
# skipped
i1.icp_index = np.zeros( len(i1.coord_list), dtype=int )
i1.icp_index.fill(-1)
i1.icp_dist = np.zeros( len(i1.coord_list) )
i1.icp_dist.fill(np.inf) # a big number
for i in range(coord_list2.shape[1]):
c2 = coord_list2[:3,i]
(dist, index) = i1.kdtree.query(c2, k=1)
if dist < i1.icp_dist[index]:
i1.icp_dist[index] = dist
i1.icp_index[index] = i
pairs = []
for i, index in enumerate(i1.icp_index):
if index >= 0:
c1 = i1.coord_list[i]
c2 = coord_list2[:3,index]
#print "c1=%s c2=%s" % (c1, c2)
pairs.append( [c1, c2] )
do_plot = False
if do_plot:
# This can be plotted in gnuplot with:
# plot "c1.txt", "c2.txt", "vector.txt" u 1:2:($3-$1):($4-$2) title "pairs" with vectors
f = open('c1.txt', 'w')
for c1 in i1.coord_list:
f.write("%.3f %.3f %.3f\n" % (c1[1], c1[0], -c1[2]))
f.close()
f = open('c2.txt', 'w')
for i in range(coord_list2.shape[1]):
c2 = coord_list2[:3,i]
f.write("%.3f %.3f %.3f\n" % (c2[1], c2[0], -c2[2]))
f.close()
f = open('vector.txt', 'w')
for pair in pairs:
c1 = pair[0]
c2 = pair[1]
f.write("%.3f %.3f %.3f %.3f %.3f %.3f\n" % ( c2[1], c2[0], -c2[2], c1[1], c1[0], -c1[2] ))
f.close()
# find the affine transform matrix that brings the paired
# points together
#print "icp pairs =", len(pairs)
v0 = np.zeros( (3, len(pairs)) )
v1 = np.zeros( (3, len(pairs)) )
weights = np.zeros( len(pairs) )
weights.fill(1.0)
for i, pair in enumerate(pairs):
v0[:,i] = pair[0]
v1[:,i] = pair[1]
#print "v0\n", v0
#print "v1\n", v1
#print "weights\n", weights
M = transformations.affine_matrix_from_points(v1, v0, shear=False, scale=False)
#M = transformations.affine_matrix_from_points_weighted(v0, v1, weights, shear=False, scale=False)
#print M
scale, shear, angles, trans, persp = transformations.decompose_matrix(M)
#print "scale=", scale
#print "shear=", shear
#print "angles=", angles
#print "trans=", trans
#print "persp=", persp
coord_list2 = np.dot(M, coord_list2)
coord_list2 /= coord_list2[3]
# print coord_list2
rot = np.linalg.norm(angles)
dist = np.linalg.norm(trans)
print "rot=%.6f dist=%.6f" % (rot, dist)
if rot < 0.001 and dist < 0.001:
done = True
a = raw_input("Press Enter to continue...")
def _absor_from_transformation(pointsL, pointsR, tol=TOL):
return tf.affine_matrix_from_points(np.array(pointsL).T,
np.array(pointsR).T,
shear=False, scale=False, usesvd=True)
final[root][e1] = np.linalg.inv(np.dot(np.linalg.inv(mats[e0][e1]), np.linalg.inv(final[root][e0])))
edges.remove((e0,e1))
break
if final[root][e1] is not None and final[root][e0] is None:
final[root][e0] = np.linalg.inv(np.dot(np.linalg.inv(mats[e1][e0]), np.linalg.inv(final[root][e1])))
edges.remove((e0,e1))
break
return final
def estimate_rotation((depth0, points0), (depth1, points1)):
def metric_points(depth, points):
assert len(points) == 3
assert depth.dtype == np.uint16
rimg = RangeImage(depth, kinect_camera())
rimg.compute_points()
pts = []
for x,y in points:
pts.append(rimg.xyz[y,x,:])
pts = np.concatenate((pts,pts))
assert pts.shape == (6,3)
return pts
import transformations
v0 = metric_points(depth0, points0)
v1 = metric_points(depth1, points1)
mat = transformations.affine_matrix_from_points(v0.T,v1.T,shear=False,scale=False)
err = (np.dot(mat[:3,:3],v0.T).T + mat[:3,3].T) - v1
mse = np.mean(np.power(err,2))
mat = np.linalg.inv(mat)
return mat, mse
def fit_model(data_sample):
pointsL = data_sample[:, :3]
pointsR = data_sample[:, 3:6]
T = tf.affine_matrix_from_points(pointsL, pointsR,
shear=False, scale=False)
return T
ideal_data.append(mag_ideal[:].tolist())
sense_data.append(mag_sense[:].tolist())
elif filter['alt'] >= alt_cutoff:
print(mag_sense, mag_ideal)
ideal_data.append(mag_ideal[:].tolist())
sense_data.append(mag_sense[:].tolist())
ideal_array = np.array(ideal_data, dtype=np.float64)
sense_array = np.array(sense_data, dtype=np.float64)
# compute affine transformation between sensed data and ideal data,
# this is our best estimate of the ideal magnetometer calibration
# using the EKF inertial only solution.
affine = transformations.affine_matrix_from_points(sense_array.T,
ideal_array.T,
shear=True,
scale=True)
print("affine:")
np.set_printoptions(precision=10, suppress=True)
print(affine)
scale, shear, angles, translate, perspective = transformations.decompose_matrix(
affine)
print(' scale:', scale)
print(' shear:', shear)
print(' angles:', angles)
print(' trans:', translate)
print(' persp:', perspective)
# write mag calibration data points to file (so we can aggregate over
# multiple flights later
file2 = open('pts_w.txt', 'r')
pts_w = []
for line in file2:
lineele = line.split()
pts_w.append([float(lineele[0]), float(lineele[1]), float(lineele[2])])
print pts_w
pts_c_rel = []
pts_w_rel = []
for i in range(len(pts_c) - 1):
pts_c_rel.append([
pts_c[i][0] - pts_c[len(pts_c) - 1][0],
pts_c[i][1] - pts_c[len(pts_c) - 1][1],
pts_c[i][2] - pts_c[len(pts_c) - 1][2]
])
pts_w_rel.append([
pts_w[i][0] - pts_w[len(pts_c) - 1][0],
pts_w[i][1] - pts_w[len(pts_c) - 1][1],
pts_w[i][2] - pts_w[len(pts_c) - 1][2]
])
print pts_c_rel
print pts_w_rel
pts_c_rel_np = np.array(pts_c_rel)
pts_w_rel_np = np.array(pts_w_rel)
M = tr.affine_matrix_from_points(pts_c_rel_np.T, pts_w_rel_np.T, shear=False)
const = -np.dot(M, np.array([pts_c[len(pts_c)-1][0], pts_c[len(pts_c)-1][1], pts_c[len(pts_c)-1][2], 1]))+\
np.array([pts_w[len(pts_c)-1][0], pts_w[len(pts_c)-1][1], pts_w[len(pts_c)-1][2], 0])
print const
print np.dot(M, np.array([pts_c[0][0], pts_c[0][1], pts_c[0][2], 1])) + const
import mag
m = mag.Magnetometer(F=1.0)
m.calibrate_bulk(sense_array)
print "b:", m.b
print "A_1:", m.A_1
ef_data = []
for s in sense_array:
ef = m.map(s)
norm = np.linalg.norm(ef)
#ef /= norm
ef_data.append(ef)
#print 's:', s, 'ef:', ef
ef_array = np.array(ef_data)
affine = transformations.affine_matrix_from_points(sense_array.T, ef_array.T)
print "affine ef:"
np.set_printoptions(precision=10, suppress=True)
print affine
scale, shear, angles, translate, perspective = transformations.decompose_matrix(
affine)
print ' scale:', scale
print ' shear:', shear
print ' angles:', angles
print ' trans:', translate
print ' persp:', perspective
cal_dir = os.path.join(args.cal, imu_sn)
if not os.path.exists(cal_dir):
os.makedirs(cal_dir)
cal_file = os.path.join(cal_dir, "imucal.json")
file = open('pts_c.txt', 'r')
pts_c = []
for line in file:
lineele = line.split()
pts_c.append([float(lineele[0]), float(lineele[1]), float(lineele[2])])
print pts_c
file2 = open('pts_w.txt', 'r')
pts_w = []
for line in file2:
lineele = line.split()
pts_w.append([float(lineele[0]), float(lineele[1]), float(lineele[2])])
print pts_w
pts_c_rel = []
pts_w_rel = []
for i in range(len(pts_c)-1):
pts_c_rel.append([pts_c[i][0]-pts_c[len(pts_c)-1][0], pts_c[i][1]-pts_c[len(pts_c)-1][1], pts_c[i][2]-pts_c[len(pts_c)-1][2]])
pts_w_rel.append([pts_w[i][0]-pts_w[len(pts_c)-1][0], pts_w[i][1]-pts_w[len(pts_c)-1][1], pts_w[i][2]-pts_w[len(pts_c)-1][2]])
print pts_c_rel
print pts_w_rel
pts_c_rel_np = np.array(pts_c_rel)
pts_w_rel_np = np.array(pts_w_rel)
M = tr.affine_matrix_from_points(pts_c_rel_np.T, pts_w_rel_np.T, shear = False)
const = -np.dot(M, np.array([pts_c[len(pts_c)-1][0], pts_c[len(pts_c)-1][1], pts_c[len(pts_c)-1][2], 1]))+\
np.array([pts_w[len(pts_c)-1][0], pts_w[len(pts_c)-1][1], pts_w[len(pts_c)-1][2], 0])
print const
print np.dot(M, np.array([pts_c[0][0], pts_c[0][1], pts_c[0][2], 1]))+const
