Skip to content

Commit 937cd9d

Browse files
YdrMasterYdrMaster
committed
利用四元数实现三维位姿的累计
1 parent 1f5eaf0 commit 937cd9d

5 files changed

Lines changed: 143 additions & 74 deletions

File tree

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

Lines changed: 76 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,76 @@
1+
package org.mechdancer.geometry.transformation
2+
3+
import org.mechdancer.algebra.function.vector.times
4+
import org.mechdancer.algebra.implement.matrix.builder.matrix
5+
import org.mechdancer.algebra.implement.vector.*
6+
import org.mechdancer.geometry.angle.toAngle
7+
import org.mechdancer.geometry.angle.toRad
8+
import org.mechdancer.geometry.angle.toVector
9+
import kotlin.math.cos
10+
import kotlin.math.sin
11+
12+
fun pose(x: Number = 0, y: Number = 0, theta: Number = 0) =
13+
Pose2D(vector2DOf(x, y), theta.toRad())
14+
15+
fun odometry(x: Number = 0, y: Number = 0, theta: Number = 0) =
16+
Pose2D(vector2DOf(x, y), theta.toRad())
17+
18+
fun quaternion(a: Number = 0, b: Number = 0, c: Number = 0, d: Number = 0) =
19+
Quaternion(a.toDouble(), b.toDouble(), c.toDouble(), d.toDouble())
20+
21+
fun quaternion(r: Number = 0, v: Vector3D = vector3DOfZero()) =
22+
Quaternion(r.toDouble(), v.x, v.y, v.z)
23+
24+
fun Transformation.toPose2D(): Pose2D {
25+
require(dim == 2) { "2d transformation is required" }
26+
val p = invoke(vector2DOfZero()).to2D()
27+
val d = invokeLinear(.0.toRad().toVector()).to2D().toAngle()
28+
return Pose2D(p, d)
29+
}
30+
31+
fun Pose2D.toTransformation(): Transformation {
32+
val (x, y) = p
33+
val (cos, sin) = d.toVector()
34+
return Transformation(
35+
matrix {
36+
row(+cos, -sin, x)
37+
row(+sin, +cos, y)
38+
row(0, 0, 1)
39+
})
40+
}
41+
42+
fun Transformation.toPose3D(): Pose3D {
43+
require(dim == 3) { "3d transformation is required" }
44+
val p = invoke(vector3DOfZero()).to3D()
45+
TODO()
46+
}
47+
48+
fun Pose3D.toTransformation(): Transformation {
49+
val (x, y, z) = p
50+
val half = theta.asRadian() / 2
51+
val a = cos(half)
52+
val (b, c, d) = u * sin(half)
53+
54+
val ab = a * b
55+
val ac = a * c
56+
val ad = a * d
57+
val bc = b * c
58+
val bd = b * d
59+
val cd = c * d
60+
61+
val b2 = b * b
62+
val c2 = c * c
63+
val d2 = d * d
64+
65+
return Transformation(
66+
matrix {
67+
row(1 - 2 * c2 - 2 * d2, 2 * bc - 2 * ad, 2 * ac + 2 * bd, x)
68+
row(2 * bc + 2 * ad, 1 - 2 * b2 - 2 * d2, 2 * cd - 2 * ab, y)
69+
row(2 * bd - 2 * ac, 2 * ab + 2 * cd, 1 - 2 * b2 - 2 * c2, z)
70+
row(0, 0, 0, 1)
71+
})
72+
}
73+
74+
fun Pose2D.toPose3D(): Pose3D =
75+
Pose3D(p = vector3DOf(p.x, p.y, 0),
76+
d = vector3DOf(0, 0, d.asRadian()))

‎src/main/kotlin/org/mechdancer/geometry/transformation/Calculate.kt‎

Lines changed: 4 additions & 49 deletions
Original file line numberDiff line numberDiff line change
@@ -4,71 +4,26 @@ import org.mechdancer.algebra.core.Matrix
44
import org.mechdancer.algebra.core.Vector
55
import org.mechdancer.algebra.function.equation.solve
66
import org.mechdancer.algebra.function.matrix.times
7-
import org.mechdancer.algebra.function.vector.*
7+
import org.mechdancer.algebra.function.vector.DistanceType
8+
import org.mechdancer.algebra.function.vector.centre
9+
import org.mechdancer.algebra.function.vector.div
10+
import org.mechdancer.algebra.function.vector.minus
811
import org.mechdancer.algebra.implement.equation.builder.EquationSetBuilder
912
import org.mechdancer.algebra.implement.matrix.builder.foldToRows
1013
import org.mechdancer.algebra.implement.matrix.builder.matrix
1114
import org.mechdancer.algebra.implement.vector.Vector2D
1215
import org.mechdancer.algebra.implement.vector.to2D
1316
import org.mechdancer.algebra.implement.vector.toListVector
14-
import org.mechdancer.algebra.implement.vector.vector2DOfZero
1517
import org.mechdancer.algebra.uniqueValue
1618
import org.mechdancer.geometry.angle.toAngle
17-
import org.mechdancer.geometry.angle.toRad
1819
import org.mechdancer.geometry.angle.toVector
1920
import kotlin.collections.component1
2021
import kotlin.collections.component2
2122
import kotlin.math.abs
22-
import kotlin.math.cos
23-
import kotlin.math.sin
2423

2524
fun Transformation.transform(pose: Pose2D) =
2625
Pose2D(invoke(pose.p).to2D(), invokeLinear(pose.d.toVector()).to2D().toAngle())
2726

28-
fun Transformation.toPose2D(): Pose2D {
29-
require(dim == 2) { "pose is a 2d transformation" }
30-
val p = invoke(vector2DOfZero()).to2D()
31-
val d = invokeLinear(.0.toRad().toVector()).to2D().toAngle()
32-
return Pose2D(p, d)
33-
}
34-
35-
fun Pose2D.toTransformation(): Transformation {
36-
val (x, y) = p
37-
val (cos, sin) = d.toVector()
38-
return Transformation(
39-
matrix {
40-
row(+cos, -sin, x)
41-
row(+sin, +cos, y)
42-
row(0, 0, 1)
43-
})
44-
}
45-
46-
fun Pose3D.toTransformation(): Transformation {
47-
val (x, y, z) = p
48-
val half = theta.asRadian() / 2
49-
val a = cos(half)
50-
val (b, c, d) = u * sin(half)
51-
52-
val ab = a * b
53-
val ac = a * c
54-
val ad = a * d
55-
val bc = b * c
56-
val bd = b * d
57-
val cd = c * d
58-
59-
val b2 = b * b
60-
val c2 = c * c
61-
val d2 = d * d
62-
63-
return Transformation(
64-
matrix {
65-
row(1 - 2 * c2 - 2 * d2, 2 * bc - 2 * ad, 2 * ac + 2 * bd, x)
66-
row(2 * bc + 2 * ad, 1 - 2 * b2 - 2 * d2, 2 * cd - 2 * ab, y)
67-
row(2 * bd - 2 * ac, 2 * ab + 2 * cd, 1 - 2 * b2 - 2 * c2, z)
68-
row(0, 0, 0, 1)
69-
})
70-
}
71-
7227
/**
7328
* 最小二乘法从点对集推算空间变换子
7429
* 任意维无约束

‎src/main/kotlin/org/mechdancer/geometry/transformation/Pose2D.kt‎

Lines changed: 4 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -3,8 +3,10 @@ package org.mechdancer.geometry.transformation
33
import org.mechdancer.algebra.function.vector.minus
44
import org.mechdancer.algebra.function.vector.plus
55
import org.mechdancer.algebra.implement.vector.Vector2D
6-
import org.mechdancer.algebra.implement.vector.vector2DOf
7-
import org.mechdancer.geometry.angle.*
6+
import org.mechdancer.geometry.angle.Angle
7+
import org.mechdancer.geometry.angle.adjust
8+
import org.mechdancer.geometry.angle.rotate
9+
import org.mechdancer.geometry.angle.unaryMinus
810

911
/**
1012
* 二维位姿（二维里程）
@@ -31,12 +33,4 @@ data class Pose2D(
3133
d.rotate(-mark.d).adjust())
3234

3335
override fun toString() = "(${p.x}, ${p.y})($d)"
34-
35-
companion object {
36-
fun pose(x: Number = 0, y: Number = 0, theta: Number = 0) =
37-
Pose2D(vector2DOf(x, y), theta.toRad())
38-
39-
fun odometry(x: Number = 0, y: Number = 0, theta: Number = 0) =
40-
Pose2D(vector2DOf(x, y), theta.toRad())
41-
}
4236
}

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

Lines changed: 42 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -1,9 +1,14 @@
11
package org.mechdancer.geometry.transformation
22

3+
import org.mechdancer.algebra.doubleEquals
34
import org.mechdancer.algebra.function.vector.normalize
5+
import org.mechdancer.algebra.function.vector.times
46
import org.mechdancer.algebra.implement.vector.Vector3D
57
import org.mechdancer.geometry.angle.Angle
68
import org.mechdancer.geometry.angle.toRad
9+
import kotlin.math.atan2
10+
import kotlin.math.cos
11+
import kotlin.math.sin
712

813
/**
914
* 三维位姿（三维里程）
@@ -14,12 +19,45 @@ data class Pose3D(
1419
val p: Vector3D,
1520
val d: Vector3D
1621
) {
17-
val u: Vector3D by lazy { d.normalize() }
18-
val theta: Angle by lazy { d.length.toRad() }
22+
/*
23+
存储方案是这样设计的：
24+
p 表示位置，没什么可说的；
25+
d 的方向表示姿态的旋转轴，长度则是绕此轴转动的弧度的一半；
26+
这是为了方便将姿态转换到四元数形式，来进行变换；
27+
若没有进行任何旋转，则 d 是一个零向量；
28+
*/
29+
30+
/** 旋转轴 */
31+
val u: Vector3D get() = d.normalize()
32+
33+
/** 转角 */
34+
val theta: Angle get() = (2 * d.length).toRad()
35+
36+
/** 增量 [delta] 累加到里程 */
37+
infix fun plusDelta(delta: Pose3D): Pose3D {
38+
val (v0, q0) = toQuaternions()
39+
val (v1, q1) = delta.toQuaternions()
40+
return pose3D(v0 + q0 * v1 * q0.conjugate,
41+
q0 * q1)
42+
}
1943

2044
override fun toString() = "p = ${p.simpleString()}, u = ${u.simpleString()}, θ = $theta"
2145

22-
companion object {
23-
private fun Vector3D.simpleString() = "($x, $y, $z)"
46+
private companion object {
47+
fun Vector3D.simpleString() = "($x, $y, $z)"
48+
49+
fun Pose3D.toQuaternions(): Pair<Quaternion, Quaternion> {
50+
val (x, y, z) = p
51+
val a = cos(d.length)
52+
val (b, c, d) = d.normalize() * sin(d.length)
53+
return Quaternion(.0, x, y, z) to Quaternion(a, b, c, d)
54+
}
55+
56+
fun pose3D(p: Quaternion, d: Quaternion): Pose3D {
57+
assert(doubleEquals(p.r, .0))
58+
val u = d.v
59+
val half = atan2(u.length, d.r)
60+
return Pose3D(p.v, u.normalize() * half)
61+
}
2462
}
2563
}

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

Lines changed: 17 additions & 11 deletions
Original file line numberDiff line numberDiff line change
@@ -1,8 +1,5 @@
11
package org.mechdancer.geometry.transformation
22

3-
import org.mechdancer.algebra.function.matrix.times
4-
import org.mechdancer.algebra.implement.matrix.builder.matrix
5-
import org.mechdancer.algebra.implement.vector.listVectorOf
63
import org.mechdancer.algebra.implement.vector.vector3DOf
74
import kotlin.math.sqrt
85

@@ -13,13 +10,22 @@ data class Quaternion(
1310
val c: Double,
1411
val d: Double
1512
) {
13+
/** 实部 */
1614
val r get() = a
15+
16+
/** 虚部 */
1717
val v get() = vector3DOf(b, c, d)
1818

19+
/** 平方模长 */
1920
val square by lazy { a * a + b * b + c * c + d * d }
21+
22+
/** 模长 */
2023
val length by lazy { sqrt(square) }
2124

25+
/** 共轭 */
2226
val conjugate get() = Quaternion(a, -b, -c, -d)
27+
28+
/** 求逆 */
2329
val inverse get() = conjugate / square
2430

2531
operator fun plus(others: Quaternion) =
@@ -40,12 +46,12 @@ data class Quaternion(
4046
operator fun div(k: Double) =
4147
Quaternion(a / k, b / k, c / k, d / k)
4248

43-
operator fun times(others: Quaternion) =
44-
(matrix {
45-
row(+a, -b, -c, -d)
46-
row(+b, +a, -d, +c)
47-
row(+c, +d, +a, -b)
48-
row(+d, -c, +b, +a)
49-
} * listVectorOf(others.a, others.b, others.c, others.d)
50-
).let { Quaternion(it[0], it[1], it[2], it[3]) }
49+
operator fun times(others: Quaternion): Quaternion {
50+
val (e, f, g, h) = others
51+
return Quaternion(
52+
a * e - b * f - c * g - d * h,
53+
b * e + a * f - d * g + c * h,
54+
c * e + d * f + a * g - b * h,
55+
d * e - c * f + b * g + a * h)
56+
}
5157
}

0 commit comments

Comments
 (0)