worldfile transform

## gistfile1.py
""" Notes :
pointsPairs: array of points pairs, where the first point is in pixel coordinates, the second in world coordinates
lockRot: whether rotation should be locked
lockRatio: whether pixel ratio should be locked to 1/1

Variables A,B,C,D,E,F described here : http://en.wikipedia.org/wiki/World_file
"""

def _leastSquareTransform(self, pointsPairs , lockRot, lockRatio):
        """Computes least squares transform for overdetermined system"""

        if not lockRot and not lockRatio:
            # We do a least square fit
            # We need more than 3 points to be in a overdetermined situation

            assert(len(pointsPairs)>3)

            """
            # Definition of one point
            pWldXn = A*pPixXn + B*pPixYn + C
            pWldYn = D*pPixXn + E*pPixYn + F

            # Definition of one point with errors
            pWldXn = A*pPixXn + B*pPixYn + C + eXn
            pWldYn = D*pPixXn + E*pPixYn + F + eYn

            # Definition of one point's error
            eXn = (pWldXn - A*pPixXn - B*pPixYn - C)
            eYn = (pWldYn - D*pPixXn - E*pPixYn - F)

            # We want the sum of errors squared to converge towards 0

            1) DER_A( SUM( eXn**2 )) = 0
            2) DER_B( SUM( eXn**2 )) = 0
            3) DER_C( SUM( eXn**2 )) = 0

            4) DER_D( SUM( eYn**2 )) = 0
            5) DER_E( SUM( eYn**2 )) = 0
            6) DER_F( SUM( eYn**2 )) = 0


            # Terms :
            o = pWldXn
            p = pWldYn
            x = pPixXn
            y = pPixYn

            eXn = (o - A*x - B*y - C)
            eYn = (p - D*x - E*y - F)

            eXn^2 = C^2  +  2*y*B*C  +  2*x*A*C  -  2*o*C  +  y^2*B^2  +  2*x*y*A*B  -  2*o*y*B  +  x^2*A^2  -  2*o*x*A  +  o^2
            eYn^2 = F^2  +  2*y*E*F  +  2*x*D*F  -  2*p*F  +  y^2*E^2  +  2*x*y*D*E  -  2*p*y*E  +  x^2*D^2  -  2*p*x*D  +  p^2

            # Sums :
            o = SUM(pWldXn)
            p = SUM(pWldYn)
            x = SUM(pPixXn)
            y = SUM(pPixYn)
            u = SUM(pPixXn^2) # x^2
            v = SUM(pPixYn^2) # y^2
            w = SUM(pPixXn*pPixYn) # x*y
            j = SUM(pWldXn*pPixXn) # o*x
            k = SUM(pWldXn*pPixYn) # o*y
            l = SUM(pWldYn*pPixXn) # p*x
            m = SUM(pWldyn*pPixYn) # p*y
            n = SUM(1)

            SUM(eXn^2) = n*C^2  +  2*y*B*C  +  2*x*A*C  -  2*o*C  +  v*B^2  +  2*w*A*B  -  2*k*B  +  u*A^2  -  2*j*A  +  n*o^2
            SUM(eYn^2) = n*F^2  +  2*y*E*F  +  2*x*D*F  -  2*p*F  +  v*E^2  +  2*w*D*E  -  2*m*E  +  u*D^2  -  2*l*D  +  n*p^2

            1) 2*x*C+2*w*B+2*u*A-2*j = 0
            2) 2*y*C+2*v*B+2*w*A-2*k = 0
            3) 2*n*C+2*y*B+2*x*A-2*o = 0

            4) 2*x*F+2*w*E+2*u*D-2*l = 0
            5) 2*y*F+2*v*E+2*w*D-2*m = 0
            6) 2*n*F+2*y*E+2*x*D-2*p = 0

            System1 (A;B;C) : 2*x*C+2*w*B+2*u*A-2*j = 0, 2*y*C+2*v*B+2*w*A-2*k = 0, 2*n*C+2*y*B+2*x*A-2*o = 0
            System2 (D;E;F) : 2*x*F+2*w*E+2*u*D-2*l = 0, 2*y*F+2*v*E+2*w*D-2*m = 0, 2*n*F+2*y*E+2*x*D-2*p = 0

            # Results
            A = -(j*(n*v-y^2)+w*(o*y-k*n)+x*(k*y-o*v))/(u*(y^2-n*v)-2*w*x*y+v*x^2+n*w^2)
            B = (u*(o*y-k*n)+x*(-j*y-o*w)+k*x^2+j*n*w)/(u*(y^2-n*v)-2*w*x*y+v*x^2+n*w^2)
            C = -(u*(o*v-k*y)+j*w*y+(k*w-j*v)*x-o*w^2)/(u*(y^2-n*v)-2*w*x*y+v*x^2+n*w^2)
            D = -(l*(n*v-y^2)+w*(p*y-m*n)+x*(m*y-p*v))/(u*(y^2-n*v)-2*w*x*y+v*x^2+n*w^2)
            E = (u*(p*y-m*n)+x*(-l*y-p*w)+m*x^2+l*n*w)/(u*(y^2-n*v)-2*w*x*y+v*x^2+n*w^2)
            F = -(u*(p*v-m*y)+l*w*y+(m*w-l*v)*x-p*w^2)/(u*(y^2-n*v)-2*w*x*y+v*x^2+n*w^2)
            """

            o,p,x,y,u,v,w,j,k,l,m,n = 0,0,0,0,0,0,0,0,0,0,0,0
            for pair in pointsPairs:
                o += pair[1].x()
                p += pair[1].y()
                x += pair[0].x()
                y += pair[0].y()
                u += pair[0].x()**2
                v += pair[0].y()**2
                w += pair[0].x()*pair[0].y()
                j += pair[1].x()*pair[0].x() # o*x
                k += pair[1].x()*pair[0].y() # o*y
                l += pair[1].y()*pair[0].x() # p*x
                m += pair[1].y()*pair[0].y() # p*y
                n += 1

            self.a = -(j*(n*v-y**2)+w*(o*y-k*n)+x*(k*y-o*v))/(u*(y**2-n*v)-2*w*x*y+v*x**2+n*w**2)
            self.b = (u*(o*y-k*n)+x*(-j*y-o*w)+k*x**2+j*n*w)/(u*(y**2-n*v)-2*w*x*y+v*x**2+n*w**2)
            self.c = -(u*(o*v-k*y)+j*w*y+(k*w-j*v)*x-o*w**2)/(u*(y**2-n*v)-2*w*x*y+v*x**2+n*w**2)
            self.d = -(l*(n*v-y**2)+w*(p*y-m*n)+x*(m*y-p*v))/(u*(y**2-n*v)-2*w*x*y+v*x**2+n*w**2)
            self.e = (u*(p*y-m*n)+x*(-l*y-p*w)+m*x**2+l*n*w)/(u*(y**2-n*v)-2*w*x*y+v*x**2+n*w**2)
            self.f = -(u*(p*v-m*y)+l*w*y+(m*w-l*v)*x-p*w**2)/(u*(y**2-n*v)-2*w*x*y+v*x**2+n*w**2)


        elif lockRot and not lockRatio:
            # same developement, but lockRot means we have no rotation, aka B=0 and D=0, so we remove all terms containing those

            # We do a least square fit
            # We need more than 2 points to be in a overdetermined situation

            assert(len(pointsPairs)>2)

            """
            System1 (A;C) : 2*x*C+2*u*A-2*j = 0,2*n*C+2*x*A-2*o = 0
            System2 (E;F) : 2*y*F+2*v*E-2*m = 0,2*n*F+2*y*E-2*p = 0

            # Results
            A = (o*x-j*n)/(x^2-n*u)
            B = 0
            C = (j*x-o*u)/(x^2-n*u)
            D = 0
            E = (p*y-m*n)/(y^2-n*v)
            F = (m*y-p*v)/(y^2-n*v)
            """

            o,p,x,y,u,v,w,j,k,l,m,n = 0,0,0,0,0,0,0,0,0,0,0,0
            for pair in pointsPairs:
                o += pair[1].x()
                p += pair[1].y()
                x += pair[0].x()
                y += pair[0].y()
                u += pair[0].x()**2
                v += pair[0].y()**2
                j += pair[1].x()*pair[0].x() # o*x
                m += pair[1].y()*pair[0].y() # p*y
                n += 1

            self.a = (o*x-j*n)/(x**2-n*u)
            self.b = 0
            self.c = (j*x-o*u)/(x**2-n*u)
            self.d = 0
            self.e = (p*y-m*n)/(y**2-n*v)
            self.f = (m*y-p*v)/(y**2-n*v)


        elif not lockRot and lockRatio:
            # same developement, but lockRatio means we have square pixels, aka A=-E and B=D, so we replace those terms

            # We do a least square fit
            # We need more than 2 points to be in a overdetermined situation

            assert(len(pointsPairs)>2)

            """
            System1 (A;C;D;F) : 2*x*C+2*w*D+2*u*A-2*j = 0, 2*n*C+2*y*D+2*x*A-2*o = 0, 2*x*F+2*w*(-A)+2*u*D-2*l = 0, 2*n*F+2*y*(-A)+2*x*D-2*p = 0

            # Results
            A = (x^2*(-p*y-j*n)+x*(l*n*y+n*p*w-n*o*u)+o*x^3-l*n^2*w+j*n^2*u)/(x^2*(y^2-2*n*u)-2*n*w*x*y+x^4+n^2*w^2+n^2*u^2)
            C = (x*(j*y^2+u*(p*y-j*n)+w*(l*n-o*y))-j*n*w*y-l*n*u*y+j*x^3+(-p*w-o*u)*x^2+n*o*w^2+n*o*u^2)/(x^2*(y^2-2*n*u)-2*n*w*x*y+x^4+n^2*w^2+n^2*u^2)
            D = (x^2*(o*y-l*n)+x*(-j*n*y-n*o*w-n*p*u)+p*x^3+j*n^2*w+l*n^2*u)/(x^2*(y^2-2*n*u)-2*n*w*x*y+x^4+n^2*w^2+n^2*u^2)
            F = (x*(l*y^2+w*(-p*y-j*n)+u*(-o*y-l*n))-l*n*w*y+j*n*u*y+l*x^3+(o*w-p*u)*x^2+n*p*w^2+n*p*u^2)/(x^2*(y^2-2*n*u)-2*n*w*x*y+x^4+n^2*w^2+n^2*u^2)
            """

            o,p,x,y,u,v,w,j,k,l,m,n = 0,0,0,0,0,0,0,0,0,0,0,0
            for pair in pointsPairs:
                o += pair[1].x()
                p += pair[1].y()
                x += pair[0].x()
                y += pair[0].y()
                u += pair[0].x()**2
                w += pair[0].x()*pair[0].y()
                j += pair[1].x()*pair[0].x() # o*x
                l += pair[1].y()*pair[0].x() # p*x
                n += 1

            self.a = (x**2*(-p*y-j*n)+x*(l*n*y+n*p*w-n*o*u)+o*x**3-l*n**2*w+j*n**2*u)/(x**2*(y**2-2*n*u)-2*n*w*x*y+x**4+n**2*w**2+n**2*u**2)
            self.c = (x*(j*y**2+u*(p*y-j*n)+w*(l*n-o*y))-j*n*w*y-l*n*u*y+j*x**3+(-p*w-o*u)*x**2+n*o*w**2+n*o*u**2)/(x**2*(y**2-2*n*u)-2*n*w*x*y+x**4+n**2*w**2+n**2*u**2)
            self.d = (x**2*(o*y-l*n)+x*(-j*n*y-n*o*w-n*p*u)+p*x**3+j*n**2*w+l*n**2*u)/(x**2*(y**2-2*n*u)-2*n*w*x*y+x**4+n**2*w**2+n**2*u**2)
            self.f = (x*(l*y**2+w*(-p*y-j*n)+u*(-o*y-l*n))-l*n*w*y+j*n*u*y+l*x**3+(o*w-p*u)*x**2+n*p*w**2+n*p*u**2)/(x**2*(y**2-2*n*u)-2*n*w*x*y+x**4+n**2*w**2+n**2*u**2)

            self.b = self.d
            self.e = -self.a

        else:#elif lockRot and lockRatio
            # same developement, but lockRot and lockRatio means A=-E and B=D = 0, so we replace those terms

            # We need more than 2 points to be in a overdetermined situation

            assert(len(pointsPairs)>2)

            """
            System1 (A;C;F) : 2*x*C+2*u*A-2*j = 0, 2*n*C+2*x*A-2*o = 0, 2*n*F+2*y*(-A)-2*p = 0

            # Results
            A = (o*x-j*n)/(x^2-n*u)
            C = (j*x-o*u)/(x^2-n*u)
            F = (o*x*y-j*n*y+p*x^2-n*p*u)/(n*x^2-n^2*u)
            """

            o,p,x,y,u,v,w,j,k,l,m,n = 0,0,0,0,0,0,0,0,0,0,0,0
            for pair in pointsPairs:
                o += pair[1].x()
                p += pair[1].y()
                x += pair[0].x()
                y += pair[0].y()
                u += pair[0].x()**2
                j += pair[1].x()*pair[0].x() # o*x
                n += 1

            self.a = (o*x-j*n)/(x**2-n*u)
            self.b = 0
            self.c = (j*x-o*u)/(x**2-n*u)
            self.d = 0
            self.e = -self.a
            self.f = (o*x*y-j*n*y+p*x**2-n*p*u)/(n*x**2-n**2*u)
	""" Notes :
	pointsPairs: array of points pairs, where the first point is in pixel coordinates, the second in world coordinates
	lockRot: whether rotation should be locked
	lockRatio: whether pixel ratio should be locked to 1/1

	Variables A,B,C,D,E,F described here : http://en.wikipedia.org/wiki/World_file
	"""

	def _leastSquareTransform(self, pointsPairs , lockRot, lockRatio):
	"""Computes least squares transform for overdetermined system"""

	if not lockRot and not lockRatio:
	# We do a least square fit
	# We need more than 3 points to be in a overdetermined situation

	assert(len(pointsPairs)>3)

	"""
	# Definition of one point
	pWldXn = ApPixXn + BpPixYn + C
	pWldYn = DpPixXn + EpPixYn + F

	# Definition of one point with errors
	pWldXn = ApPixXn + BpPixYn + C + eXn
	pWldYn = DpPixXn + EpPixYn + F + eYn

	# Definition of one point's error
	eXn = (pWldXn - ApPixXn - BpPixYn - C)
	eYn = (pWldYn - DpPixXn - EpPixYn - F)

	# We want the sum of errors squared to converge towards 0

	1) DER_A( SUM( eXn**2 )) = 0
	2) DER_B( SUM( eXn**2 )) = 0
	3) DER_C( SUM( eXn**2 )) = 0

	4) DER_D( SUM( eYn**2 )) = 0
	5) DER_E( SUM( eYn**2 )) = 0
	6) DER_F( SUM( eYn**2 )) = 0


	# Terms :
	o = pWldXn
	p = pWldYn
	x = pPixXn
	y = pPixYn

	eXn = (o - Ax - By - C)
	eYn = (p - Dx - Ey - F)

	eXn^2 = C^2 + 2yBC + 2xAC - 2oC + y^2B^2 + 2xyAB - 2oyB + x^2A^2 - 2oxA + o^2
	eYn^2 = F^2 + 2yEF + 2xDF - 2pF + y^2E^2 + 2xyDE - 2pyE + x^2D^2 - 2pxD + p^2

	# Sums :
	o = SUM(pWldXn)
	p = SUM(pWldYn)
	x = SUM(pPixXn)
	y = SUM(pPixYn)
	u = SUM(pPixXn^2) # x^2
	v = SUM(pPixYn^2) # y^2
	w = SUM(pPixXnpPixYn) # xy
	j = SUM(pWldXnpPixXn) # ox
	k = SUM(pWldXnpPixYn) # oy
	l = SUM(pWldYnpPixXn) # px
	m = SUM(pWldynpPixYn) # py
	n = SUM(1)

	SUM(eXn^2) = nC^2 + 2yBC + 2xAC - 2oC + vB^2 + 2wAB - 2kB + uA^2 - 2jA + n*o^2
	SUM(eYn^2) = nF^2 + 2yEF + 2xDF - 2pF + vE^2 + 2wDE - 2mE + uD^2 - 2lD + n*p^2

	1) 2xC+2wB+2uA-2*j = 0
	2) 2yC+2vB+2wA-2*k = 0
	3) 2nC+2yB+2xA-2*o = 0

	4) 2xF+2wE+2uD-2*l = 0
	5) 2yF+2vE+2wD-2*m = 0
	6) 2nF+2yE+2xD-2*p = 0

	System1 (A;B;C) : 2xC+2wB+2uA-2j = 0, 2yC+2vB+2wA-2k = 0, 2nC+2yB+2xA-2*o = 0
	System2 (D;E;F) : 2xF+2wE+2uD-2l = 0, 2yF+2vE+2wD-2m = 0, 2nF+2yE+2xD-2*p = 0

	# Results
	A = -(j(nv-y^2)+w(oy-kn)+x(ky-ov))/(u(y^2-nv)-2wxy+vx^2+n*w^2)
	B = (u(oy-kn)+x(-jy-ow)+kx^2+jnw)/(u(y^2-nv)-2wxy+vx^2+nw^2)
	C = -(u(ov-ky)+jwy+(kw-jv)x-ow^2)/(u(y^2-nv)-2wxy+vx^2+nw^2)
	D = -(l(nv-y^2)+w(py-mn)+x(my-pv))/(u(y^2-nv)-2wxy+vx^2+n*w^2)
	E = (u(py-mn)+x(-ly-pw)+mx^2+lnw)/(u(y^2-nv)-2wxy+vx^2+nw^2)
	F = -(u(pv-my)+lwy+(mw-lv)x-pw^2)/(u(y^2-nv)-2wxy+vx^2+nw^2)
	"""

	o,p,x,y,u,v,w,j,k,l,m,n = 0,0,0,0,0,0,0,0,0,0,0,0
	for pair in pointsPairs:
	o += pair[1].x()
	p += pair[1].y()
	x += pair[0].x()
	y += pair[0].y()
	u += pair[0].x()**2
	v += pair[0].y()**2
	w += pair[0].x()*pair[0].y()
	j += pair[1].x()pair[0].x() # ox
	k += pair[1].x()pair[0].y() # oy
	l += pair[1].y()pair[0].x() # px
	m += pair[1].y()pair[0].y() # py
	n += 1

	self.a = -(j(nv-y*2)+w(oy-kn)+x(ky-ov))/(u(y*2-nv)-2wxy+vx*2+nw**2)
	self.b = (u(oy-kn)+x(-jy-ow)+kx2+jnw)/(u(y*2-nv)-2wxy+vx*2+nw**2)
	self.c = -(u(ov-ky)+jwy+(kw-jv)x-ow2)/(u(y*2-nv)-2wxy+vx*2+nw**2)
	self.d = -(l(nv-y*2)+w(py-mn)+x(my-pv))/(u(y*2-nv)-2wxy+vx*2+nw**2)
	self.e = (u(py-mn)+x(-ly-pw)+mx2+lnw)/(u(y*2-nv)-2wxy+vx*2+nw**2)
	self.f = -(u(pv-my)+lwy+(mw-lv)x-pw2)/(u(y*2-nv)-2wxy+vx*2+nw**2)


	elif lockRot and not lockRatio:
	# same developement, but lockRot means we have no rotation, aka B=0 and D=0, so we remove all terms containing those

	# We do a least square fit
	# We need more than 2 points to be in a overdetermined situation

	assert(len(pointsPairs)>2)

	"""
	System1 (A;C) : 2xC+2uA-2j = 0,2nC+2xA-2o = 0
	System2 (E;F) : 2yF+2vE-2m = 0,2nF+2yE-2p = 0

	# Results
	A = (ox-jn)/(x^2-n*u)
	B = 0
	C = (jx-ou)/(x^2-n*u)
	D = 0
	E = (py-mn)/(y^2-n*v)
	F = (my-pv)/(y^2-n*v)
	"""

	o,p,x,y,u,v,w,j,k,l,m,n = 0,0,0,0,0,0,0,0,0,0,0,0
	for pair in pointsPairs:
	o += pair[1].x()
	p += pair[1].y()
	x += pair[0].x()
	y += pair[0].y()
	u += pair[0].x()**2
	v += pair[0].y()**2
	j += pair[1].x()pair[0].x() # ox
	m += pair[1].y()pair[0].y() # py
	n += 1

	self.a = (ox-jn)/(x*2-nu)
	self.b = 0
	self.c = (jx-ou)/(x*2-nu)
	self.d = 0
	self.e = (py-mn)/(y*2-nv)
	self.f = (my-pv)/(y*2-nv)


	elif not lockRot and lockRatio:
	# same developement, but lockRatio means we have square pixels, aka A=-E and B=D, so we replace those terms

	# We do a least square fit
	# We need more than 2 points to be in a overdetermined situation

	assert(len(pointsPairs)>2)

	"""
	System1 (A;C;D;F) : 2xC+2wD+2uA-2j = 0, 2nC+2yD+2xA-2o = 0, 2xF+2w(-A)+2uD-2l = 0, 2nF+2y(-A)+2xD-2p = 0

	# Results
	A = (x^2(-py-jn)+x(lny+npw-nou)+ox^3-ln^2w+jn^2u)/(x^2(y^2-2nu)-2nwxy+x^4+n^2w^2+n^2u^2)
	C = (x(jy^2+u(py-jn)+w(ln-oy))-jnwy-lnuy+jx^3+(-pw-ou)x^2+now^2+nou^2)/(x^2(y^2-2nu)-2nwxy+x^4+n^2w^2+n^2*u^2)
	D = (x^2(oy-ln)+x(-jny-now-npu)+px^3+jn^2w+ln^2u)/(x^2(y^2-2nu)-2nwxy+x^4+n^2w^2+n^2u^2)
	F = (x(ly^2+w(-py-jn)+u(-oy-ln))-lnwy+jnuy+lx^3+(ow-pu)x^2+npw^2+npu^2)/(x^2(y^2-2nu)-2nwxy+x^4+n^2w^2+n^2*u^2)
	"""

	o,p,x,y,u,v,w,j,k,l,m,n = 0,0,0,0,0,0,0,0,0,0,0,0
	for pair in pointsPairs:
	o += pair[1].x()
	p += pair[1].y()
	x += pair[0].x()
	y += pair[0].y()
	u += pair[0].x()**2
	w += pair[0].x()*pair[0].y()
	j += pair[1].x()pair[0].x() # ox
	l += pair[1].y()pair[0].x() # px
	n += 1

	self.a = (x*2(-py-jn)+x(lny+npw-nou)+ox*3-ln*2w+jn2u)/(x*2(y*2-2nu)-2nwxy+x4+n2w2+n2u*2)
	self.c = (x(jy*2+u(py-jn)+w(ln-oy))-jnwy-lnuy+jx*3+(-pw-ou)x*2+now2+nou2)/(x2(y*2-2nu)-2nwxy+x4+n2w2+n2u*2)
	self.d = (x*2(oy-ln)+x(-jny-now-npu)+px*3+jn*2w+ln2u)/(x*2(y*2-2nu)-2nwxy+x4+n2w2+n2u*2)
	self.f = (x(ly*2+w(-py-jn)+u(-oy-ln))-lnwy+jnuy+lx*3+(ow-pu)x*2+npw2+npu2)/(x2(y*2-2nu)-2nwxy+x4+n2w2+n2u*2)

	self.b = self.d
	self.e = -self.a

	else:#elif lockRot and lockRatio
	# same developement, but lockRot and lockRatio means A=-E and B=D = 0, so we replace those terms

	# We need more than 2 points to be in a overdetermined situation

	assert(len(pointsPairs)>2)

	"""
	System1 (A;C;F) : 2xC+2uA-2j = 0, 2nC+2xA-2o = 0, 2nF+2y(-A)-2*p = 0

	# Results
	A = (ox-jn)/(x^2-n*u)
	C = (jx-ou)/(x^2-n*u)
	F = (oxy-jny+px^2-npu)/(nx^2-n^2*u)
	"""

	o,p,x,y,u,v,w,j,k,l,m,n = 0,0,0,0,0,0,0,0,0,0,0,0
	for pair in pointsPairs:
	o += pair[1].x()
	p += pair[1].y()
	x += pair[0].x()
	y += pair[0].y()
	u += pair[0].x()**2
	j += pair[1].x()pair[0].x() # ox
	n += 1

	self.a = (ox-jn)/(x*2-nu)
	self.b = 0
	self.c = (jx-ou)/(x*2-nu)
	self.d = 0
	self.e = -self.a
	self.f = (oxy-jny+px2-npu)/(nx2-n2*u)