Finding an Affine Transform using three 2D point correspondences using Simplex Affine Mapping (SAM) in Swift

import CoreGraphics
import Foundation //for NumberFormatter, used in debug functions

// MARK: - Simple Affine Map function for 2D and supporting types

/// Calculate the affine transform from one set of three 2D points (the "from" triangle) to another set of three 2D points (the "to" triangle).
/// Throws an error if one of the sets of points is colinear.
/// The transform is calculated using the 2D expression of the Simplex Affine Map [SAM] by V. B. Tymchyshyn and A. V. Khlevniuk.
/// https://www.researchgate.net/publication/332410209_Beginner%27s_guide_to_mapping_simplexes_affinely
///
/// The workbook from the authors provides numerous examples showing step-by-step calculation for simplexes in N dimensions.
/// https://www.researchgate.net/publication/332971934_Workbook_on_mapping_simplexes_affinely\
///
/// A triangle is a simplex in 2D space, and hence works using the SAM scheme.
/// https://en.wikipedia.org/wiki/Simplex
///
/// The matrix elements include a mix of vectors and scalars. For this code, use letter names corresponding to the vectors and scalars in the SAM paper.
/// a, b, c are 2D points in the "from" triangle
/// d, e, f are 2D points in the "to" triangle
///
/// |0 d e f |
/// |p ax bx cx | // p called "x1" in the SAM paper
/// det |q ay by cy | // q called "x2" in the SAM paper
/// |1 1 1 1 |
/// (-1) ----------------------
/// |ax bx cx | // ax = point A, x component
/// det |ay by cy |
/// |1 1 1 |
///
/// For image processing applications this technique may not be appropriate if point finding is not sufficiently repeatable.
/// This mapping will be sensitive to noise / inaccuracy in finding any of the "from" or "to" points: any triangle can map to any other triangle!
/// For point registration, typically more than 3 point correspondences are used. The transform may be found
/// using a least squares fitting, and often by eliminating outlier points. Related topics include Singular Value Decomposition (SVD) and
/// Random Sampling and Consensus (RANSAC).
func sam(from: Triangle, to: Triangle) throws -> CGAffineTransform {
if from.colinear() {
throw SamError.colinearPoints(description: "Points in 'from' triangle are colinear. Can not calculate SAM transform.")
}

if to.colinear() {
throw SamError.colinearPoints(description: "Points in 'to' triangle are colinear. Can not calculate SAM transform.")
}

let a = from.vector1
let b = from.vector2
let c = from.vector3

let d = to.vector1
let e = to.vector2
let f = to.vector3

// Determinant of 4x4 in numerator of SAM formula
// Reduce to p, q, and translation. (p, q) here are (x1, x2) in the SAM paper.
// We can find the determinant starting with any row or any column, so we make it easy to isolate p ("x1") and q ("x2")

// +0 * (...)

// -p ( [d] (by - cy) - [e] (ay - cy) + [f] (ay - by) )
let p = CGFloat(-1.0) * (d * (b.dy - c.dy) - e * (a.dy - c.dy) + f * (a.dy - b.dy))

// +q ( [d] (bx - cx) - [e] (ax - cx) + [f] (ax - bx) )
let q = d * (b.dx - c.dx) - e * (a.dx - c.dx) + f * (a.dx - b.dx)

// -1 ( [d] (bx * cy - by * cx) - [e] (ax * cy - ay * cx) + [f] * (ax * by - ay * bx) )
let translation = CGFloat(-1.0) * (d * (b.dx * c.dy - b.dy * c.dx) - e * (a.dx * c.dy - a.dy * c.dx) + f * (a.dx * b.dy - a.dy * b.dx))

// Determinant of 3x3 used in denominator of SAM formula
let denominator = determinant(from)

let pd = CGFloat(-1.0) * p / denominator
let qd = CGFloat(-1.0) * q / denominator
let td = CGFloat(-1.0) * translation / denominator

// plug values into CoreGraphics affine transform
// |a b 0| = |pd.x pd.y 0|
// |c d 0| |qd.x qd.y 0|
// |tx ty 1| |td.x td.y 1|

return CGAffineTransform(a: pd.dx, b: pd.dy, c: qd.dx, d: qd.dy, tx: td.dx, ty: td.dy)
}

enum SamError: Error {
case colinearPoints(description: String)
}

/// Three points nominally defining a triangle, but possibly colinear.
struct Triangle {
var point1: CGPoint
var point2: CGPoint
var point3: CGPoint

var x1: CGFloat { point1.x }
var y1: CGFloat { point1.y }
var x2: CGFloat { point2.x }
var y2: CGFloat { point2.y }
var x3: CGFloat { point3.x }
var y3: CGFloat { point3.y }

/// Point1 as a 2D vector
var vector1: CGVector { CGVector(point1) }

/// Point2 as a 2D vector
var vector2: CGVector { CGVector(point2) }

/// Point3 as a 2D vector
var vector3: CGVector { CGVector(point3) }

/// Return a Triangle after applying an affine transform to self.
func applying(_ t: CGAffineTransform) -> Triangle {
Triangle(
point1: self.point1.applying(t),
point2: self.point2.applying(t),
point3: self.point3.applying(t)
)
}

init(point1: CGPoint, point2: CGPoint, point3: CGPoint) {
self.point1 = point1
self.point2 = point2
self.point3 = point3
}

init(x1: CGFloat, y1: CGFloat, x2: CGFloat, y2: CGFloat, x3: CGFloat, y3: CGFloat) {
point1 = CGPoint(x: x1, y: y1)
point2 = CGPoint(x: x2, y: y2)
point3 = CGPoint(x: x3, y: y3)
}

/// Returns a (Bool, CGFloat) tuple indicating whether the points in the Triangle are colinear, and the angle between vectors tested.
func colinear(degreesTolerance: CGFloat = 0.5) -> Bool {
let v1 = vector2 - vector1
let v2 = vector3 - vector2
let radians = CGVector.angleBetweenVectors(v1: v1, v2: v2)

if radians.isNaN {
return true
}

var degrees = radiansToDegrees(radians)

if degrees > 90 {
degrees = 180 - degrees
}

// code to get around parsing error
return 0 > degrees - degreesTolerance
}
}

func degreesToRadians(_ degrees: CGFloat) -> CGFloat {
degrees * CGFloat.pi / 180.0
}

func radiansToDegrees(_ radians: CGFloat) -> CGFloat {
180.0 * radians / CGFloat.pi
}

extension CGVector {
init(_ point: CGPoint) {
self.init(dx: point.x, dy: point.y)
}

static func + (lhs: CGVector, rhs: CGVector) -> CGVector {
CGVector(dx: lhs.dx + rhs.dx, dy: lhs.dy + rhs.dy)
}

static func - (lhs: CGVector, rhs: CGVector) -> CGVector {
CGVector(dx: lhs.dx - rhs.dx, dy: lhs.dy - rhs.dy)
}

static func * (_ vector: CGVector, _ scalar: CGFloat) -> CGVector {
CGVector(dx: vector.dx * scalar, dy: vector.dy * scalar)
}

static func * (_ scalar: CGFloat, _ vector: CGVector) -> CGVector {
CGVector(dx: vector.dx * scalar, dy: vector.dy * scalar)
}

static func * (lhs: CGVector, rhs: CGVector) -> CGFloat {
lhs.dx * rhs.dx + lhs.dy * rhs.dy
}

static func dotProduct(v1: CGVector, v2: CGVector) -> CGFloat {
v1.dx * v2.dx + v1.dy * v2.dy
}

static func / (_ vector: CGVector, _ scalar: CGFloat) -> CGVector {
CGVector(dx: vector.dx / scalar, dy: vector.dy / scalar)
}

static func / (_ scalar: CGFloat, _ vector: CGVector) -> CGVector {
CGVector(dx: vector.dx / scalar, dy: vector.dy / scalar)
}

// Returns again between vectors in range 0 to 2 * pi [positive]
// a * b = ||a|| ||b|| cos(theta)
// theta = arc cos (a * b / ||a|| ||b||)
static func angleBetweenVectors(v1: CGVector, v2: CGVector) -> CGFloat {
acos( v1 * v2 / (v1.length() * v2.length()) )
}

func length() -> CGFloat {
sqrt(self.dx * self.dx + self.dy * self.dy)
}
}

/// Determinant of three points used in the denominator of the SAM formula.
/// x1 x2 x3
/// y1 y2 y3
/// 1 1 1
func determinant(_ t: Triangle) -> CGFloat {
t.x1 * (t.y2 - t.y3) - t.x2 * (t.y1 - t.y3) + t.x3 * (t.y1 - t.y2)
}

//MARK: - Debug
extension NumberFormatter {
func string(_ value: CGFloat, digits: Int, failText: String = "[?]") -> String {
minimumFractionDigits = max(0, digits)
maximumFractionDigits = minimumFractionDigits

guard let s = string(from: NSNumber(value: Double(value))) else {
return failText
}

return s
}

func string(_ point: CGPoint, digits: Int = 1, failText: String = "[?]") -> String {
let sx = string(point.x, digits: digits, failText: failText)
let sy = string(point.y, digits: digits, failText: failText)
return "(\(sx), \(sy))"
}

func string(_ vector: CGVector, digits: Int = 1, failText: String = "[?]") -> String {
let sdx = string(vector.dx, digits: digits, failText: failText)
let sdy = string(vector.dy, digits: digits, failText: failText)
return "(\(sdx), \(sdy))"
}

func string(_ transform: CGAffineTransform, rotationDigits: Int = 2, translationDigits: Int = 1, failText: String = "[?]") -> String {
let sa = string(transform.a, digits: rotationDigits)
let sb = string(transform.b, digits: rotationDigits)
let sc = string(transform.c, digits: rotationDigits)
let sd = string(transform.d, digits: rotationDigits)
let stx = string(transform.tx, digits: translationDigits)
let sty = string(transform.ty, digits: translationDigits)

var s = "a: \(sa) b: \(sb) 0"
s += "\nc: \(sc) d: \(sd) 0"
s += "\ntx: \(stx) ty: \(sty) 1"
return s
}
}

let formatter = NumberFormatter()

/// Checks whether the three points in the Triangle are colinear.
/// Returns a test message spanning multiple lines.
func testColinearity(_ t: Triangle) -> String {
let co = t.colinear()
let st1 = formatter.string(t.point1, digits: 2)
let st2 = formatter.string(t.point2, digits: 2)
let st3 = formatter.string(t.point3, digits: 2)

var s = "\(st1), \(st2), \(st3): points in triangle"
s += "\n\(co): colinear"

return s
}

/// Checks how close two points are. Used to test whether a transformed point (actual) is equivalent to the expected point.
/// Returns a test message spanning multiple lines.
func testCoincidence(actual: CGPoint, expected: CGPoint, pointTolerance: CGFloat = 0.1) -> String {
let vector = CGVector(actual) - CGVector(expected)
let distance = vector.length()
let sd = formatter.string(distance, digits: 2)
let sa = formatter.string(actual, digits: 2)
let se = formatter.string(expected, digits: 2)

return "\(sd) offset from actual \(sa) to expected \(se)"
}

/// Calculates the affine transform and checks its validity.
/// Returns a test message spanning multiple lines.
func testSAM(from t1: Triangle, to t2: Triangle, pointTolerance: CGFloat = 0.1) -> String {
var s = "from \(t1) to \(t2)"

do {
let transform = try sam(from: t1, to: t2)
s += "\n" + formatter.string(transform)

// apply transform to points in t1
let a = t1.applying(transform)

// get difference between transformed points (actual) and t2 points (expected)
s += "\n" + testCoincidence(actual: a.point1, expected: t2.point1, pointTolerance: pointTolerance)
s += "\n" + testCoincidence(actual: a.point2, expected: t2.point2, pointTolerance: pointTolerance)
s += "\n" + testCoincidence(actual: a.point3, expected: t2.point3, pointTolerance: pointTolerance)

return s
}
catch SamError.colinearPoints(let description) {
return description
}
catch {
return error.localizedDescription
}
}

//MARK: - Tests for Playground
print()
print("** Colinearity tests **")
let c1 = Triangle(x1: 1, y1: 1, x2: 2, y2: 2, x3: 3, y3: 3)
let sc1 = testColinearity(c1)
print(sc1)

print()
let c2 = Triangle(x1: 0, y1: 0, x2: 5, y2: 5, x3: 0.01, y3: 0.02)
let sc2 = testColinearity(c2)
print(sc2)

print()
let c3 = Triangle(x1: 1, y1: 1, x2: 2, y2: 3, x3: 3, y3: 2)
let sc3 = testColinearity(c3)
print(sc3)

print()
print("** Arbitrary translation example from SAM workbook **")
let a1 = Triangle(x1: 1, y1: 1, x2: 2, y2: 3, x3: 3, y3: 2)
let a2 = Triangle(x1: 3, y1: 2, x2: 1, y2: 5, x3: -2, y3: 1)
print(testColinearity(a1))
print()
print(testColinearity(a2))
print()
print(testSAM(from: a1, to: a2))

print()
print("** Pure Translation **")
let t1 = Triangle(x1: 0, y1: 0, x2: -2, y2: 3, x3: -5, y3: 3)
let t2 = Triangle(x1: 3, y1: -2, x2: 1, y2: 1, x3: -2, y3: 1)
print(testSAM(from: t1, to: t2))

print()
print("** Rotation and Translation **")
let r1 = Triangle(x1: 0, y1: 0, x2: 1, y2: 0, x3: 0, y3: 1)
let r2 = Triangle(x1: 6, y1: 5, x2: 5, y2: 5, x3: 6, y3: 4)
print(testSAM(from: r1, to: r2))

print()
print("** Pure Scaling **")
let s1 = Triangle(x1: 0, y1: 0, x2: 1, y2: 0, x3: 0, y3: 1)
let s2 = Triangle(x1: 0, y1: 0, x2: 5, y2: 0, x3: 0, y3: 5)
print(testSAM(from: s1, to: s2))

print()
print("** Test in which a triangle has colinear points")
let k1 = Triangle(x1: 1, y1: 1, x2: -2, y2: -2, x3: 5, y3: 5)
let k2 = Triangle(x1: -5, y1: 3, x2: 2, y2: 8, x3: -3, y3: 1)
print()
print(testColinearity(k1))
print()
print(testColinearity(k2))
print()
print(testSAM(from: k1, to: k2))

--

--

Get the Medium app

A button that says 'Download on the App Store', and if clicked it will lead you to the iOS App store
A button that says 'Get it on, Google Play', and if clicked it will lead you to the Google Play store