利用四元数实现三维里程的加减，并在 xy 面上测试

Author: YdrMaster

src/main/kotlin/org/mechdancer/geometry/transformation/Builder.kt

@@ -9,12 +9,15 @@ import org.mechdancer.geometry.angle.toVector
 import kotlin.math.cos
 import kotlin.math.sin
 
-fun pose(x: Number = 0, y: Number = 0, theta: Number = 0) =
+fun pose2D(x: Number = 0, y: Number = 0, theta: Number = 0) =
     Pose2D(vector2DOf(x, y), theta.toRad())
 
 fun odometry(x: Number = 0, y: Number = 0, theta: Number = 0) =
     Pose2D(vector2DOf(x, y), theta.toRad())
 
+fun pose3D(x: Number = 0, y: Number = 0, z: Number = 0, d: Vector3D = vector3DOfZero()) =
+    Pose3D(vector3DOf(x, y, z), d)
+
 fun quaternion(a: Number = 0, b: Number = 0, c: Number = 0, d: Number = 0) =
     Quaternion(a.toDouble(), b.toDouble(), c.toDouble(), d.toDouble())
 
@@ -71,6 +74,6 @@ fun Pose3D.toTransformation(): Transformation {
         })
 }
 
-fun Pose2D.toPose3D(): Pose3D =
+fun Pose2D.to3D(): Pose3D =
     Pose3D(p = vector3DOf(p.x, p.y, 0),
-           d = vector3DOf(0, 0, d.asRadian()))
+           d = vector3DOf(0, 0, d.asRadian() / 2))

src/main/kotlin/org/mechdancer/geometry/transformation/Pose3D.kt

@@ -38,7 +38,23 @@ data class Pose3D(
         val (v0, q0) = toQuaternions()
         val (v1, q1) = delta.toQuaternions()
         return pose3D(v0 + q0 * v1 * q0.conjugate,
-                      q0 * q1)
+                      q1 * q0)
+    }
+
+    /** 里程回滚到增量 [delta] 之前 */
+    infix fun minusDelta(delta: Pose3D): Pose3D {
+        val (v0, q0) = toQuaternions()
+        val (v1, q1) = delta.toQuaternions()
+        val qi = q1.conjugate * q0
+        return pose3D(v0 - qi * v1 * qi.conjugate, qi)
+    }
+
+    /** 计算里程从标记 [mark] 到当前状态的增量 */
+    infix fun minusState(mark: Pose3D): Pose3D {
+        val (v0, q0) = mark.toQuaternions()
+        val (v1, q1) = toQuaternions()
+        return pose3D(q0.conjugate * (v1 - v0) * q0,
+                      q1 * q0.conjugate)
     }
 
     override fun toString() = "p = ${p.simpleString()}, u = ${u.simpleString()}, θ = $theta"

src/main/kotlin/org/mechdancer/geometry/transformation/Quaternion.kt

@@ -28,6 +28,9 @@ data class Quaternion(
     /** 求逆 */
     val inverse get() = conjugate / square
 
+    operator fun unaryMinus() =
+        Quaternion(-a, -b, -c, -d)
+
     operator fun plus(others: Quaternion) =
         Quaternion(a + others.a,
                    b + others.b,

src/test/kotlin/org/mechdancer/geometry/transformation/TestPose2D.kt

@@ -4,7 +4,6 @@ import org.junit.Test
 import org.mechdancer.algebra.implement.vector.vector2DOf
 import org.mechdancer.algebra.implement.vector.vector2DOfZero
 import org.mechdancer.geometry.angle.toRad
-import org.mechdancer.geometry.transformation.Pose2D.Companion.pose
 import kotlin.math.PI
 
 class TestPose2D {
@@ -13,7 +12,7 @@ class TestPose2D {
      */
     @Test
     fun test() {
-        val step0 = pose()
+        val step0 = pose2D()
         assert(step0 == Pose2D(vector2DOfZero(), .0.toRad())) {
             "里程计初始化错误：$step0"
         }

src/test/kotlin/org/mechdancer/geometry/transformation/TestPose3D.kt

@@ -0,0 +1,40 @@
+package org.mechdancer.geometry.transformation
+
+import org.junit.Test
+import org.mechdancer.algebra.implement.vector.vector2DOf
+import org.mechdancer.algebra.implement.vector.vector3DOfZero
+import org.mechdancer.geometry.angle.toRad
+import kotlin.math.PI
+
+class TestPose3D {
+    /**
+     * 测试里程计算法
+     */
+    @Test
+    fun test() {
+        val step0 = pose3D()
+        assert(step0 == Pose3D(vector3DOfZero(), vector3DOfZero())) {
+            "里程计初始化错误：$step0"
+        }
+
+        val delta1 = Pose2D(vector2DOf(3.0, 4.0), (PI / 2).toRad()).to3D()
+
+        val step1 = step0 plusDelta delta1
+        assert(step1 == delta1) {
+            "里程计累加错误：$step1 ≠ $delta1"
+        }
+
+        val step2 = step1 minusDelta delta1
+        assert(step2 == step0) {
+            "里程计回滚错误：$step2 ≠ $step0"
+        }
+
+        val delta2 = Pose2D(vector2DOf(1.0, 1.0), (-PI / 4).toRad()).to3D()
+
+        val step3 = step2 plusDelta delta1
+        val step4 = step3 plusDelta delta2 minusState step3
+        assert(step4 == delta2) {
+            "里程计标记错误：$step4 ≠ $delta2"
+        }
+    }
+}