def rotate_3D_x(angle):
"""rotation matrix for rotation in 3d over x-axis with homogeneous coordinates"""
cosa = math.cos(angle)
sina = math.sin(angle)
mat = Mat([[1.0, 0.0, 0.0, 0.0], [0.0, cosa, -sina, 0.0],
[0.0, sina, cosa, 0.0], [0.0, 0.0, 0.0, 1.0]])
return mat
def make_hcs_3d(a, b, c, righthanded=True):
"""build a 3D homogeneous coordiate system from three points. The origin is point a. The x-axis is
b-a, the y axis is c-a, or as close as possible after orthogonormalisation."""
# create orthonormal basis
u = normalised(b - a)
v = normalised(c - a)
nu = vector.norm(u)
nv = vector.norm(v)
if tol_eq(nu, 0.0) and tol_eq(nv, 0.0):
# all points equal, no rotation
u = vector.vector([1.0, 0.0, 0.0])
v = vector.vector([0.0, 1.0, 0.0])
elif tol_eq(nu, 0.0):
# determine u perpendicular from v
u, dummy = perp_3d(v)
elif tol_eq(nv, 0.0):
# determine v perpendicular from u
dummy, v = perp_3d(u)
# ensure that u and v are different
if tol_eq(vector.norm(u - v), 0.0):
dummy, v = perp_3d(u)
# make the basis vectors orthogonal
w = vector.cross(u, v)
v = vector.cross(w, u)
# flip basis if lefthanded desired
if righthanded == False:
w = -w
# create matix with basis vectors + translation as columns
hcs = Mat([[u[0], v[0], w[0], a[0]], [u[1], v[1], w[1], a[1]],
[u[2], v[2], w[2], a[2]], [0.0, 0.0, 0.0, 1.0]])
return hcs
def make_hcs_3d_scaled(a, b, c):
"""build a 3D homogeneus coordiate system from three points, and derive scale from distance between points"""
# create orthonormal basis
u = normalised(b - a)
v = normalised(c - a)
nu = vector.norm(u)
nv = vector.norm(v)
if tol_eq(nu, 0) and tol_eq(nv, 0):
# all points equal, no rotation
u = vector.vector([1.0, 0.0, 0.0])
v = vector.vector([0.0, 1.0, 0.0])
elif tol_eq(nu, 0):
# determine u perpendicular from v
u, dummy = perp_3d(v)[0]
elif tol_eq(nv, 0):
# determine v perpendicular from u
dummy, v = perp_3d(u)[0]
# make the basis vectors orthogonal
w = vector.cross(u, v)
v = vector.cross(w, u)
# scale again
if not tol_eq(vector.norm(b - a), 0.0):
u = u / vector.norm(b - a)
if not tol_eq(vector.norm(c - a), 0.0):
v = v / vector.norm(c - a)
# note: w is not scaled
# create matix with basis vectors + translation as columns
hcs = Mat([[u[0], v[0], w[0], a[0]], [u[1], v[1], w[1], a[1]],
[u[2], v[2], w[2], a[2]], [0.0, 0.0, 0.0, 1.0]])
return hcs
def make_hcs_2d_scaled(a, b):
"""build a 2D homogeneus coordiate system from two points, but scale with distance between input point"""
u = b - a
if tol_eq(vector.norm(u), 0.0):
u = vector.vector([1.0, 0.0])
#else:
# u = u / vector.norm(u)
v = vector.vector([-u[1], u[0]])
hcs = Mat([[u[0], v[0], a[0]], [u[1], v[1], a[1]], [0.0, 0.0, 1.0]])
return hcs
def make_hcs_2d(a, b):
"""build a 2D homogeneus coordiate system from two points"""
u = b - a
if tol_eq(vector.norm(u), 0.0):
u = vector.vector([0.0, 0.0])
else:
u = u / vector.norm(u)
v = vector.vector([-u[1], u[0]])
hcs = Mat([[u[0], v[0], a[0]], [u[1], v[1], a[1]], [0.0, 0.0, 1.0]])
return hcs
def make_hcs_3d(a, b, c):
"""build a 3D homogeneus coordiate system from three vectors"""
u = b - a
u = u / vector.norm(u)
v = c - a
v = v / vector.norm(v)
w = vector.cross(u, v)
v = vector.cross(w, u)
hcs = Mat([[u[0], v[0], w[0], a[0]], [u[1], v[1], w[1], a[1]],
[u[2], v[2], w[2], a[2]], [0.0, 0.0, 0.0, 1.0]])
return hcs
def make_hcs_3d_scaled(a, b, c):
"""build a 3D homogeneus coordiate system from three vectors"""
# create orthnormal basis
u = b - a
u = u / vector.norm(u)
v = c - a
v = v / vector.norm(v)
w = vector.cross(u, v)
v = vector.cross(w, u)
# scale
u = u / vector.norm(u) / vector.norm(b - a)
v = v / vector.norm(v) / vector.norm(c - a)
hcs = Mat([[u[0], v[0], w[0], a[0]], [u[1], v[1], w[1], a[1]],
[u[2], v[2], w[2], a[2]], [0.0, 0.0, 0.0, 1.0]])
return hcs
def uniform_scale_3D(scale):
mat = Mat([[scale, 0.0, 0.0, 0.0], [0.0, scale, 0.0, 0.0],
[0.0, 0.0, scale, 0.0], [0.0, 0.0, 0.0, 1.0]])
return mat
def scale_3D(sx, sy, sz):
mat = Mat([[sx, 0.0, 0.0, 0.0], [0.0, sy, 0.0, 0.0], [0.0, 0.0, sz, 0.0],
[0.0, 0.0, 0.0, 1.0]])
return mat
def translate_3D(dx, dy, dz):
mat = Mat([[1.0, 0.0, 0.0, dx], [0.0, 1.0, 0.0, dy], [0.0, 0.0, 1.0, dz],
[0.0, 0.0, 0.0, 1.0]])
return mat
def rotate_2D(angle):
"""rotation matrix for rotation in 2d with homogeneous coordinates"""
cosa = math.cos(angle)
sina = math.sin(angle)
mat = Mat([[cosa, -sina, 0.0], [sina, cosa, 0.0], [0.0, 0.0, 1.0]])
return mat
def translate_2D(dx, dy):
mat = Mat([[1.0, 0.0, dx], [0.0, 1.0, dy], [0.0, 0.0, 1.0]])
return mat
def rotate_2D(angle):
mat = Mat([[math.sin[angle], math.cos[angle], 0.0],
[math.cos[angle], -math.sin[angle], 0.0], [0.0, 0.0, 1.0]])
return mat
def main(argv=None):
if argv is None:
argv = sys.argv[1:]
parser = OptionParser(description=__doc__, version=__version__)
parser.add_option('-i','--input',
action="store", dest="inputfile", default="",
metavar="FILE",
help="Read list of ground control points from a FILE")
parser.add_option("--similar", action="store_true", dest="similar")
parser.add_option("--noskew", action="store_true", dest="noskew")
parser.add_option("-v", action="store_true", dest="verbose")
#parser.set_defaults(verbose=True)
(options, args) = parser.parse_args( args=argv)
if len(args):
parser.error("incorrect number of arguments, use --help")
if not options.inputfile:
parser.error("no gcps specified, use --help")
if options.verbose:
print "reading %s..." % options.inputfile
destSet = []
sourceSet = []
for line in open(options.inputfile, 'r'):
destSet.append( map( float, line.split()[:2] ) )
sourceSet.append( map( float, line.split()[2:4] ) )
# print(destSet)
# print(sourceSet)
numberOfPoints = min( len(destSet), len(sourceSet))
# print numberOfPoints
# Compute centres of gravity of the two point sets.
## Overflow? Maybe better to use: max() min() and middle point?
#cxDst = min( [dst[0] for dst in destSet] )
cxDst, cyDst, cxSrc, cySrc = 0,0,0,0
for i in range(numberOfPoints):
cxDst += destSet[i][0]
cyDst += destSet[i][1]
cxSrc += sourceSet[i][0]
cySrc += sourceSet[i][1]
cxDst /= numberOfPoints
cyDst /= numberOfPoints
cxSrc /= numberOfPoints
cySrc /= numberOfPoints
#print cxDst, cyDst, cxSrc, cySrc
if not (options.similar or options.noskew):
if options.verbose:
print "- affine transformation"
# create matrices x, y, and A.
x = Mat( [[dst[0]-cxDst for dst in destSet]]).tr()
y = Mat( [[dst[1]-cyDst for dst in destSet]]).tr()
A = Mat( [ [1., src[0]-cxSrc, src[1]-cySrc] for src in sourceSet] )
At = A.tr()
Q = At.mmul(A).inverse()
a = Q.mmul(At).mmul(x)
b = Q.mmul(At).mmul(y)
a1 = a[1][0]
a2 = a[2][0]
a3 = b[1][0]
a4 = b[2][0]
print a1
print a3
print a2
print a4
print cxDst - a1*cxSrc - a2*cySrc
print cyDst - a3*cxSrc - a4*cySrc
sys.exit(0)
else:
if options.verbose:
print "- similarity transformation"
# compute a1 and a2
sumX1_times_x2, sumY1_times_y2 = 0, 0
sumx2_times_x2, sumy2_times_y2 = 0, 0
sumY1_times_x2, sumX1_times_y2 = 0, 0
for i in range(numberOfPoints):
x2 = sourceSet[i][0] - cxSrc
y2 = sourceSet[i][1] - cySrc
sumX1_times_x2 += destSet[i][0] * x2
sumY1_times_y2 += destSet[i][1] * y2
sumx2_times_x2 += x2 * x2
sumy2_times_y2 += y2 * y2
sumY1_times_x2 += destSet[i][1] * x2
sumX1_times_y2 += destSet[i][0] * y2
a1 = (sumX1_times_x2+sumY1_times_y2)/(sumx2_times_x2+sumy2_times_y2)
a2 = (sumY1_times_x2-sumX1_times_y2)/(sumx2_times_x2+sumy2_times_y2)
if options.similar:
print a1
print a2
print -a2
print a1
print cxDst - a1*cxSrc + a2*cySrc
print cyDst - a2*cxSrc - a1*cySrc
sys.exit(0)
if options.verbose:
print "- affine5 transformation (without skew)"
# we need iteration - starting from similarity
# indexes to access values in the array params
TRANSX, TRANSY, SCALEX, SCALEY, ROT = 0,1,2,3,4
# The tolerance that is used to determine whether a new improvement for the
# parameters is small enough to stop the computations.
TOLERANCE = 0.000001
params = [0,0,0,0,0]
params[TRANSX] = 0 # close to 0, since both sets
params[TRANSY] = 0 # of points are centered around 0
params[SCALEX] = math.sqrt(a1*a1+a2*a2) # similarity
params[SCALEY] = params[SCALEX]
rot = math.atan2(a2, a1)
if (rot < 0.):
rot += math.pi * 2.
params[ROT] = rot
dx = [0,0,0,0,0]
dxprev = [1,1,1,1,1]
while (abs(dx[0]-dxprev[0]) > TOLERANCE or
abs(dx[1]-dxprev[1]) > TOLERANCE or
abs(dx[2]-dxprev[2]) > TOLERANCE or
abs(dx[3]-dxprev[3]) > TOLERANCE or
abs(dx[4]-dxprev[4]) > TOLERANCE):
cosRot = math.cos(params[ROT])
sinRot = math.sin(params[ROT])
array_A = []
array_l = []
for i in range(numberOfPoints):
xCosRot = cosRot*sourceSet[i][0]-cxSrc
xSinRot = sinRot*sourceSet[i][0]-cxSrc
yCosRot = cosRot*sourceSet[i][1]-cySrc
ySinRot = sinRot*sourceSet[i][1]-cySrc
estimationX = params[TRANSX] + params[SCALEX]*xCosRot - params[SCALEY]*ySinRot
estimationY = params[TRANSY] + params[SCALEX]*xSinRot + params[SCALEY]*yCosRot
array_l.append( [destSet[i][0] - cxDst - estimationX] )
array_l.append( [destSet[i][1] - cyDst - estimationY] )
array_A.append( [1, 0, xCosRot, -ySinRot, -(destSet[i][1]-cyDst)+params[TRANSY] ])
array_A.append( [0, 1, xSinRot, yCosRot, (destSet[i][0]-cxDst)-params[TRANSX] ])
l = Mat(array_l)
A = Mat(array_A)
At = A.tr()
Q = At.mmul(A).inverse()
x = Q.mmul( At.mmul(l) )
dxprev = dx
dx = [ x[j][0] for j in range(5) ]
params = [ params[j]+dx[j] for j in range(5) ]
#print dx
cosRot = math.cos(params[ROT])
sinRot = math.sin(params[ROT])
print params[SCALEX]*cosRot
print params[SCALEX]*sinRot
print -params[SCALEY]*sinRot
print params[SCALEY]*cosRot
print cxDst - params[SCALEX]*cosRot*cxSrc + params[SCALEY]*sinRot*cySrc
print cyDst - params[SCALEX]*sinRot*cxSrc - params[SCALEY]*cosRot*cySrc
def main(argv=None):
if argv is None:
argv = sys.argv[1:]
parser = OptionParser(description=__doc__, version=__version__)
parser.add_option('-i',
'--input',
action="store",
dest="inputfile",
default="",
metavar="FILE",
help="Read list of ground control points from a FILE")
parser.add_option("--similar", action="store_true", dest="similar")
parser.add_option("--noskew", action="store_true", dest="noskew")
parser.add_option("-v", action="store_true", dest="verbose")
#parser.set_defaults(verbose=True)
(options, args) = parser.parse_args(args=argv)
if len(args):
parser.error("incorrect number of arguments, use --help")
if not options.inputfile:
parser.error("no gcps specified, use --help")
if options.verbose:
print "reading %s..." % options.inputfile
destSet = []
sourceSet = []
for line in open(options.inputfile, 'r'):
destSet.append(map(float, line.split()[:2]))
sourceSet.append(map(float, line.split()[2:4]))
# print(destSet)
# print(sourceSet)
numberOfPoints = min(len(destSet), len(sourceSet))
# print numberOfPoints
# Compute centres of gravity of the two point sets.
## Overflow? Maybe better to use: max() min() and middle point?
#cxDst = min( [dst[0] for dst in destSet] )
cxDst, cyDst, cxSrc, cySrc = 0, 0, 0, 0
for i in range(numberOfPoints):
cxDst += destSet[i][0]
cyDst += destSet[i][1]
cxSrc += sourceSet[i][0]
cySrc += sourceSet[i][1]
cxDst /= numberOfPoints
cyDst /= numberOfPoints
cxSrc /= numberOfPoints
cySrc /= numberOfPoints
#print cxDst, cyDst, cxSrc, cySrc
if not (options.similar or options.noskew):
if options.verbose:
print "- affine transformation"
# create matrices x, y, and A.
x = Mat([[dst[0] - cxDst for dst in destSet]]).tr()
y = Mat([[dst[1] - cyDst for dst in destSet]]).tr()
A = Mat([[1., src[0] - cxSrc, src[1] - cySrc] for src in sourceSet])
At = A.tr()
Q = At.mmul(A).inverse()
a = Q.mmul(At).mmul(x)
b = Q.mmul(At).mmul(y)
a1 = a[1][0]
a2 = a[2][0]
a3 = b[1][0]
a4 = b[2][0]
print a1
print a3
print a2
print a4
print cxDst - a1 * cxSrc - a2 * cySrc
print cyDst - a3 * cxSrc - a4 * cySrc
sys.exit(0)
else:
if options.verbose:
print "- similarity transformation"
# compute a1 and a2
sumX1_times_x2, sumY1_times_y2 = 0, 0
sumx2_times_x2, sumy2_times_y2 = 0, 0
sumY1_times_x2, sumX1_times_y2 = 0, 0
for i in range(numberOfPoints):
x2 = sourceSet[i][0] - cxSrc
y2 = sourceSet[i][1] - cySrc
sumX1_times_x2 += destSet[i][0] * x2
sumY1_times_y2 += destSet[i][1] * y2
sumx2_times_x2 += x2 * x2
sumy2_times_y2 += y2 * y2
sumY1_times_x2 += destSet[i][1] * x2
sumX1_times_y2 += destSet[i][0] * y2
a1 = (sumX1_times_x2 + sumY1_times_y2) / (sumx2_times_x2 +
sumy2_times_y2)
a2 = (sumY1_times_x2 - sumX1_times_y2) / (sumx2_times_x2 +
sumy2_times_y2)
if options.similar:
print a1
print a2
print -a2
print a1
print cxDst - a1 * cxSrc + a2 * cySrc
print cyDst - a2 * cxSrc - a1 * cySrc
sys.exit(0)
if options.verbose:
print "- affine5 transformation (without skew)"
# we need iteration - starting from similarity
# indexes to access values in the array params
TRANSX, TRANSY, SCALEX, SCALEY, ROT = 0, 1, 2, 3, 4
# The tolerance that is used to determine whether a new improvement for the
# parameters is small enough to stop the computations.
TOLERANCE = 0.000001
params = [0, 0, 0, 0, 0]
params[TRANSX] = 0 # close to 0, since both sets
params[TRANSY] = 0 # of points are centered around 0
params[SCALEX] = math.sqrt(a1 * a1 + a2 * a2) # similarity
params[SCALEY] = params[SCALEX]
rot = math.atan2(a2, a1)
if (rot < 0.):
rot += math.pi * 2.
params[ROT] = rot
dx = [0, 0, 0, 0, 0]
dxprev = [1, 1, 1, 1, 1]
while (abs(dx[0] - dxprev[0]) > TOLERANCE
or abs(dx[1] - dxprev[1]) > TOLERANCE
or abs(dx[2] - dxprev[2]) > TOLERANCE
or abs(dx[3] - dxprev[3]) > TOLERANCE
or abs(dx[4] - dxprev[4]) > TOLERANCE):
cosRot = math.cos(params[ROT])
sinRot = math.sin(params[ROT])
array_A = []
array_l = []
for i in range(numberOfPoints):
xCosRot = cosRot * sourceSet[i][0] - cxSrc
xSinRot = sinRot * sourceSet[i][0] - cxSrc
yCosRot = cosRot * sourceSet[i][1] - cySrc
ySinRot = sinRot * sourceSet[i][1] - cySrc
estimationX = params[TRANSX] + params[
SCALEX] * xCosRot - params[SCALEY] * ySinRot
estimationY = params[TRANSY] + params[
SCALEX] * xSinRot + params[SCALEY] * yCosRot
array_l.append([destSet[i][0] - cxDst - estimationX])
array_l.append([destSet[i][1] - cyDst - estimationY])
array_A.append([
1, 0, xCosRot, -ySinRot,
-(destSet[i][1] - cyDst) + params[TRANSY]
])
array_A.append([
0, 1, xSinRot, yCosRot,
(destSet[i][0] - cxDst) - params[TRANSX]
])
l = Mat(array_l)
A = Mat(array_A)
At = A.tr()
Q = At.mmul(A).inverse()
x = Q.mmul(At.mmul(l))
dxprev = dx
dx = [x[j][0] for j in range(5)]
params = [params[j] + dx[j] for j in range(5)]
#print dx
cosRot = math.cos(params[ROT])
sinRot = math.sin(params[ROT])
print params[SCALEX] * cosRot
print params[SCALEX] * sinRot
print -params[SCALEY] * sinRot
print params[SCALEY] * cosRot
print cxDst - params[SCALEX] * cosRot * cxSrc + params[
SCALEY] * sinRot * cySrc
print cyDst - params[SCALEX] * sinRot * cxSrc - params[
SCALEY] * cosRot * cySrc
