diff --git a/src/main/kotlin/algorithms/ModelFitting.kt b/src/main/kotlin/algorithms/ModelFitting.kt new file mode 100644 index 0000000..6b1b87d --- /dev/null +++ b/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 fitLines(graph: NeighborhoodGraph): List> = + 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 fitLines( + points: Set, + takenPoints: MutableSet = HashSet(), + graph: NeighborhoodGraph, + lines: MutableList> = mutableListOf() +): List> { + 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 findNeighborsOnLine( + linePoints: Set, + nearestPoints: LinkedList, + takenPoints: MutableSet, + graph: NeighborhoodGraph +): Set { + 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 getNotTakenNeighbors(point: T, takenPoints: Set, graph: NeighborhoodGraph): List = + graph.getNeighbors(point).filterNot { takenPoints.contains(it) } diff --git a/src/main/kotlin/algorithms/Modelfitting.kt b/src/main/kotlin/algorithms/Modelfitting.kt deleted file mode 100644 index c801ccd..0000000 --- a/src/main/kotlin/algorithms/Modelfitting.kt +++ /dev/null @@ -1,13 +0,0 @@ -package algorithms - -//import model.NeighborhoodGraph -//import model.geometry.Line -//import model.geometry.Point -// -//fun fitLines(graph: NeighborhoodGraph): Map> { -// -//} -// -//fun getContinuousList(): List { -// -//} diff --git a/src/main/kotlin/math/Geometry.kt b/src/main/kotlin/math/Geometry.kt index 7ad121e..3d2c6c5 100644 --- a/src/main/kotlin/math/Geometry.kt +++ b/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) diff --git a/src/main/kotlin/model/NeighborhoodGraph.kt b/src/main/kotlin/model/NeighborhoodGraph.kt index 560ae96..8311dde 100644 --- a/src/main/kotlin/model/NeighborhoodGraph.kt +++ b/src/main/kotlin/model/NeighborhoodGraph.kt @@ -12,6 +12,8 @@ private val logger by lazy { KotlinLogging.logger {} } data class NeighborhoodGraph( private val map: Map> ) { + fun getSize(): Int = map.size + fun getObjects(): Set = map.keys fun getNeighbors(t: T): Set = map.getValue(t) @@ -36,7 +38,9 @@ data class NeighborhoodGraph( private fun filterOutlier(map: Map>): Map> { 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 diff --git a/src/main/kotlin/model/Scan2D.kt b/src/main/kotlin/model/Scan2D.kt index 2aeb96d..ff6630b 100644 --- a/src/main/kotlin/model/Scan2D.kt +++ b/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> = class Scan2D(val pointCloud: List, private val scanner: Scanner) { - fun rotateBy(angle: Double): List = + fun rotateBy(angle: Double): List = pointCloud.map { it.rotateBy(angle) } fun toTSV(): String { diff --git a/src/main/kotlin/model/geometry/Line.kt b/src/main/kotlin/model/geometry/Line.kt index 6808f80..696eb9b 100644 --- a/src/main/kotlin/model/geometry/Line.kt +++ b/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): Line { val xMean = points.map { it.x }.mean() val yMean = points.map { it.y }.mean() val slope = diff --git a/src/test/kotlin/algorithms/ModelFittingTest.kt b/src/test/kotlin/algorithms/ModelFittingTest.kt new file mode 100644 index 0000000..c7be10a --- /dev/null +++ b/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 + } + } +}) diff --git a/src/test/kotlin/math/GeometryTest.kt b/src/test/kotlin/math/GeometryTest.kt index bb20bf5..9d8c3ba 100644 --- a/src/test/kotlin/math/GeometryTest.kt +++ b/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) diff --git a/src/test/kotlin/model/NeighborhoodGraphTest.kt b/src/test/kotlin/model/NeighborhoodGraphTest.kt index 2d5d10a..91a72af 100644 --- a/src/test/kotlin/model/NeighborhoodGraphTest.kt +++ b/src/test/kotlin/model/NeighborhoodGraphTest.kt @@ -1,11 +1,14 @@ 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 @@ -13,6 +16,21 @@ 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) diff --git a/src/test/kotlin/model/geometry/LineTest.kt b/src/test/kotlin/model/geometry/LineTest.kt index e99c920..cd8e6da 100644 --- a/src/test/kotlin/model/geometry/LineTest.kt +++ b/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 } }