feat(ModelFitting): now support line fitting (#13)

Given a cloud of points this new algorithm is able to fit several lines
utilizing point proximity into the data.

src/main/kotlin/algorithms/ModelFitting.kt

@@ -0,0 +1,59 @@
+package algorithms
+
+import math.distanceLineToPoint
+import model.NeighborhoodGraph
+import model.geometry.Line
+import model.geometry.Point
+import java.util.LinkedList
+
+/** Threshold for line fitting, defaults to 1mm deviation from the line */
+const val THRESHOLD = 1.0 / 1000.0
+
+fun <T : Point> fitLines(graph: NeighborhoodGraph<T>): List<Set<T>> =
+    fitLines(graph.getObjects(), graph = graph)
+
+// this warns, that tailrec parameter defaults will be initialized in reverse order - which doesn't matter here.
+@Suppress("TAILREC_WITH_DEFAULTS")
+tailrec fun <T : Point> fitLines(
+    points: Set<T>,
+    takenPoints: MutableSet<T> = HashSet(),
+    graph: NeighborhoodGraph<T>,
+    lines: MutableList<Set<T>> = mutableListOf()
+): List<Set<T>> {
+    if (points.isEmpty()) return lines
+
+    val startPoint = points.first()
+
+    val initialNeighbors = LinkedList(getNotTakenNeighbors(startPoint, takenPoints, graph))
+    lines.add(findNeighborsOnLine(setOf(startPoint), initialNeighbors, takenPoints, graph))
+
+    return fitLines(graph.getObjects() - takenPoints, takenPoints, graph, lines)
+}
+
+private tailrec fun <T : Point> findNeighborsOnLine(
+    linePoints: Set<T>,
+    nearestPoints: LinkedList<T>,
+    takenPoints: MutableSet<T>,
+    graph: NeighborhoodGraph<T>
+): Set<T> {
+    if (nearestPoints.isEmpty() || takenPoints.size == graph.getSize()) {
+        return linePoints
+    }
+    val nearestPoint = nearestPoints.pop()
+    val newLinePoints = linePoints + nearestPoint
+
+    val line = Line.fromSeveralPoints(newLinePoints)
+    val maxDistance = newLinePoints.map { distanceLineToPoint(line, it) }.max()!!
+
+    return if (maxDistance > THRESHOLD) {
+        findNeighborsOnLine(linePoints, nearestPoints, takenPoints, graph)
+    } else {
+        takenPoints.add(nearestPoint)
+        val notTakenNeighbors = getNotTakenNeighbors(nearestPoint, takenPoints, graph)
+        nearestPoints.addAll(notTakenNeighbors)
+        findNeighborsOnLine(newLinePoints, nearestPoints, takenPoints, graph)
+    }
+}
+
+private fun <T : Point> getNotTakenNeighbors(point: T, takenPoints: Set<T>, graph: NeighborhoodGraph<T>): List<T> =
+    graph.getNeighbors(point).filterNot { takenPoints.contains(it) }

src/main/kotlin/math/Geometry.kt

@@ -34,7 +34,8 @@ fun intersectTwoLines(a: Line, b: Line): Point {
 }
 
 fun distanceLineToPoint(l: Line, p: Point): Double {
-    val inverseSlope = - 1 / l.slope
+    if (l.slope == 0.0) return l.intercept - p.y
+    val inverseSlope = -1 / l.slope
     val perpendicularLine = Line.fromSlopeAndPoint(inverseSlope, p)
     val intersection = intersectTwoLines(l, perpendicularLine)
     return distancePointToPoint(p, intersection)

src/main/kotlin/model/NeighborhoodGraph.kt

@@ -12,6 +12,8 @@ private val logger by lazy { KotlinLogging.logger {} }
 data class NeighborhoodGraph<T : GeometricObject>(
     private val map: Map<T, Set<T>>
 ) {
+    fun getSize(): Int = map.size
+
     fun getObjects(): Set<T> = map.keys
 
     fun getNeighbors(t: T): Set<T> = map.getValue(t)
@@ -36,7 +38,9 @@ data class NeighborhoodGraph<T : GeometricObject>(
         private fun <T> filterOutlier(map: Map<T, Set<T>>): Map<T, Set<T>> {
             val count = AtomicInteger(0)
             val filteredMap = map.filterAndCount(count) { (point, neighbors) ->
-                neighbors.all { neighbor -> map.getValue(neighbor).contains(point) }
+                neighbors.any { neighbor ->
+                    map.getValue(neighbor).contains(point)
+                }
             }
             logger.info { "filtered outliers: $count out of ${map.size}" }
             return filteredMap

src/main/kotlin/model/Scan2D.kt

@@ -4,7 +4,6 @@
 package model
 
 import com.github.doyaaaaaken.kotlincsv.dsl.csvReader
-import model.geometry.Point
 import model.geometry.PolarPoint
 import mu.KotlinLogging
 import java.io.File
@@ -27,7 +26,7 @@ fun readFromTSV(tsv: String): List<Map<String, String>> =
 
 class Scan2D(val pointCloud: List<PolarPoint>, private val scanner: Scanner) {
 
-    fun rotateBy(angle: Double): List<Point> =
+    fun rotateBy(angle: Double): List<PolarPoint> =
         pointCloud.map { it.rotateBy(angle) }
 
     fun toTSV(): String {

src/main/kotlin/model/geometry/Line.kt

@@ -5,7 +5,7 @@ import math.distanceLineToPoint
 import model.mean
 import kotlin.math.pow
 
-data class Line(val slope: Double, val intercept: Double) : GeometricObject {
+class Line(val slope: Double, val intercept: Double) : GeometricObject {
 
     constructor(slope: Int, intercept: Double) : this(slope.toDouble(), intercept)
     constructor(slope: Double, intercept: Int) : this(slope, intercept.toDouble())
@@ -23,6 +23,9 @@ data class Line(val slope: Double, val intercept: Double) : GeometricObject {
     override fun toTSVString(): String =
         "$slope\t$intercept"
 
+    override fun toString(): String =
+        "Line(slope=$slope, intercept=$intercept)"
+
     override fun equals(other: Any?): Boolean {
         if (this === other) return true
         if (javaClass != other?.javaClass) return false
@@ -54,7 +57,7 @@ data class Line(val slope: Double, val intercept: Double) : GeometricObject {
         /**
          * Least Squares following https://www.varsitytutors.com/hotmath/hotmath_help/topics/line-of-best-fit
          */
-        fun fromSeveralPoints(vararg points: Point): Line {
+        fun fromSeveralPoints(points: Collection<Point>): Line {
             val xMean = points.map { it.x }.mean()
             val yMean = points.map { it.y }.mean()
             val slope =

src/test/kotlin/algorithms/ModelFittingTest.kt

@@ -0,0 +1,96 @@
+package algorithms
+
+import io.kotlintest.matchers.collections.shouldContainExactly
+import io.kotlintest.specs.FreeSpec
+import io.kotlintest.shouldBe
+import model.NeighborhoodGraph
+import model.geometry.Line
+import model.geometry.Point
+
+class ModelFittingTest : FreeSpec({
+
+    "initRecursion" - {
+        "will detect a line" {
+            val points = listOf(
+                Point(-2, 2), // will detect line
+                Point(-1, 3),
+                Point(-3, 1)
+            )
+            val targetLines = listOf(
+                Line(1, 4)
+            )
+            val graph = NeighborhoodGraph.usingBruteForce(points)
+            val lines = fitLines(graph)
+            lines.flatMap { it.asIterable() }.size shouldBe points.size
+            lines.map { Line.fromSeveralPoints(it) } shouldContainExactly targetLines
+        }
+
+        "will detect two lines" {
+            val points = listOf(
+                Point(-2, 2),
+                Point(-1, 3),
+                Point(-3, 1),
+
+                Point(1, 2.5),
+                Point(2, 1)
+            )
+            val targetLines = listOf(
+                Line(1, 4),
+                Line(-1.5, 4)
+                )
+            val graph = NeighborhoodGraph.usingBruteForce(points)
+            val lines = fitLines(graph)
+            lines.flatMap { it.asIterable() }.size shouldBe points.size
+            lines.map { Line.fromSeveralPoints(it) } shouldContainExactly targetLines
+        }
+
+        "will detect a line without graph connection" {
+            val points = listOf(
+                Point(-2, 2),
+                Point(-1, 3),
+                Point(-3, 1),
+
+                Point(1, 2.5),
+                Point(2, 1),
+
+                Point(-50, -30),
+                Point(-51, -29),
+                Point(-52, -28)
+            )
+            val targetLines = listOf(
+                Line(1, 4),
+                Line(-1.5, 4),
+                Line(-1, -80)
+            )
+            val graph = NeighborhoodGraph.usingBruteForce(points)
+            val lines = fitLines(graph)
+            lines.flatMap { it.asIterable() }.size shouldBe points.size
+            lines.map { Line.fromSeveralPoints(it) } shouldContainExactly targetLines
+        }
+
+        "will detect out of order points" {
+            val points = listOf(
+                Point(-2, 2),
+                Point(-1, 3),
+
+                Point(1, 2.5),
+                Point(2, 1),
+
+                Point(-3, 1),
+
+                Point(-50, -30),
+                Point(-51, -29),
+                Point(-52, -28)
+            )
+            val targetLines = listOf(
+                Line(1, 4),
+                Line(-1.5, 4),
+                Line(-1, -80)
+            )
+            val graph = NeighborhoodGraph.usingBruteForce(points)
+            val lines = fitLines(graph)
+            lines.flatMap { it.asIterable() }.size shouldBe points.size
+            lines.map { Line.fromSeveralPoints(it) } shouldContainExactly targetLines
+        }
+    }
+})

src/test/kotlin/math/GeometryTest.kt

@@ -69,6 +69,10 @@ internal class GeometryTest : FreeSpec({
                 Line(-1, 0),
                 Point(1, 1),
                 sqrt(2.0)
+            ), row(
+                Line(0, 2),
+                Point(1, 1),
+                1.0
             )
         ) { line, point, distance ->
             distanceLineToPoint(line, point) shouldBe (distance plusOrMinus PRECISION)

src/test/kotlin/model/NeighborhoodGraphTest.kt

@@ -1,18 +1,36 @@
 package model
 
 import io.kotlintest.assertSoftly
+import io.kotlintest.data.forall
 import io.kotlintest.matchers.asClue
 import io.kotlintest.matchers.collections.shouldContainExactlyInAnyOrder
+import io.kotlintest.shouldBe
 import io.kotlintest.shouldThrow
 import io.kotlintest.specs.FreeSpec
 import io.kotlintest.tables.row
+import model.geometry.Point
 import model.geometry.PolarPoint
 import mu.KotlinLogging
 
 private val logger by lazy { KotlinLogging.logger {} }
 
 class NeighborhoodGraphTest : FreeSpec({
 
+    "getSize" {
+        forall(
+            row(
+                listOf(Point(1, 0), Point(2, 0)), 2
+            ), row(
+                listOf(Point(1, 0), Point(2, 0), Point(3, 0)), 3
+            ),
+            row(
+                listOf(Point(1, 0), Point(2, 0), Point(3, 0), Point(4, 0)), 4
+            )
+        ) { points, targetSize ->
+            NeighborhoodGraph.usingBruteForce(points).getSize() shouldBe targetSize
+        }
+    }
+
     "companion object" - {
         val a = PolarPoint(0.0, 1.0, 0)
         val b = PolarPoint(Math.toRadians(-90.0), 1.0, 4)

src/test/kotlin/model/geometry/LineTest.kt

@@ -75,20 +75,26 @@ internal class LineTest : FreeSpec({
         "fromSeveralPoints from 2 points" {
             forall(
                 row(
-                    Point(0, 0),
-                    Point(1, 1),
+                    listOf(
+                        Point(0, 0),
+                        Point(1, 1)
+                    ),
                     Line(1, 0)
                 ), row(
-                    Point(1, -1),
-                    Point(0, 0),
+                    listOf(
+                        Point(1, -1),
+                        Point(0, 0)
+                    ),
                     Line(-1, 0)
                 ), row(
-                    Point(-2, -3),
-                    Point(-1, -3),
+                    listOf(
+                        Point(-2, -3),
+                        Point(-1, -3)
+                    ),
                     Line(0, -3)
                 )
-            ) { pointA, pointB, targetLine ->
-                Line.fromSeveralPoints(pointA, pointB) shouldBe targetLine
+            ) { points, targetLine ->
+                Line.fromSeveralPoints(points) shouldBe targetLine
             }
         }
 
@@ -117,7 +123,7 @@ internal class LineTest : FreeSpec({
                     Line(0, -3)
                 )
             ) { points, targetLine ->
-                Line.fromSeveralPoints(*points.toTypedArray()) shouldBe targetLine
+                Line.fromSeveralPoints(points) shouldBe targetLine
             }
         }