mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-07-04 03:11:43 +00:00
[wpimath] Fix Pose3d exponential and clean up Pose3d logarithm (#4970)
Implementation based on this paper: https://ethaneade.org/lie.pdf
This commit is contained in:
Jordan McMichael
@@ -14,6 +14,7 @@ import edu.wpi.first.math.Nat;
|
||||
import edu.wpi.first.math.VecBuilder;
|
||||
import edu.wpi.first.math.Vector;
|
||||
import edu.wpi.first.math.interpolation.Interpolatable;
|
||||
import edu.wpi.first.math.numbers.N1;
|
||||
import edu.wpi.first.math.numbers.N3;
|
||||
import java.util.Objects;
|
||||
|
||||
@@ -200,34 +201,44 @@ public class Pose3d implements Interpolatable<Pose3d> {
|
||||
* @return The new pose of the robot.
|
||||
*/
|
||||
public Pose3d exp(Twist3d twist) {
|
||||
final var Omega = rotationVectorToMatrix(VecBuilder.fill(twist.rx, twist.ry, twist.rz));
|
||||
final var OmegaSq = Omega.times(Omega);
|
||||
// Implementation from Section 3.2 of https://ethaneade.org/lie.pdf
|
||||
final var u = VecBuilder.fill(twist.dx, twist.dy, twist.dz);
|
||||
final var rvec = VecBuilder.fill(twist.rx, twist.ry, twist.rz);
|
||||
final var omega = rotationVectorToMatrix(rvec);
|
||||
final var omegaSq = omega.pow(2);
|
||||
double theta = rvec.norm();
|
||||
double thetaSq = theta * theta;
|
||||
|
||||
double thetaSq = twist.rx * twist.rx + twist.ry * twist.ry + twist.rz * twist.rz;
|
||||
|
||||
// Get left Jacobian of SO3. See first line in right column of
|
||||
// http://asrl.utias.utoronto.ca/~tdb/bib/barfoot_ser17_identities.pdf
|
||||
Matrix<N3, N3> J;
|
||||
if (thetaSq < 1E-9 * 1E-9) {
|
||||
// J = I + 0.5ω
|
||||
J = Matrix.eye(Nat.N3()).plus(Omega.times(0.5));
|
||||
double A;
|
||||
double B;
|
||||
double C;
|
||||
if (theta < 1E-9) {
|
||||
// Taylor Expansions around θ = 0
|
||||
// A = 1/1! - θ²/3! + θ⁴/5!
|
||||
// B = 1/2! - θ²/4! + θ⁴/6!
|
||||
// C = 1/3! - θ²/5! + θ⁴/7!
|
||||
A = 1 - thetaSq / 6 + thetaSq * thetaSq / 120;
|
||||
B = 1 / 2.0 - thetaSq / 24 + thetaSq * thetaSq / 720;
|
||||
C = 1 / 6.0 - thetaSq / 120 + thetaSq * thetaSq / 5040;
|
||||
} else {
|
||||
double theta = Math.sqrt(thetaSq);
|
||||
// J = I + (1 − cos(θ))/θ² ω + (θ − sin(θ))/θ³ ω²
|
||||
J =
|
||||
Matrix.eye(Nat.N3())
|
||||
.plus(Omega.times((1.0 - Math.cos(theta)) / thetaSq))
|
||||
.plus(OmegaSq.times((theta - Math.sin(theta)) / (thetaSq * theta)));
|
||||
// A = sin(θ)/θ
|
||||
// B = (1 - cos(θ)) / θ²
|
||||
// C = (1 - A) / θ²
|
||||
A = Math.sin(theta) / theta;
|
||||
B = (1 - Math.cos(theta)) / thetaSq;
|
||||
C = (1 - A) / thetaSq;
|
||||
}
|
||||
|
||||
// Get translation component
|
||||
final var translation =
|
||||
J.times(new MatBuilder<>(Nat.N3(), Nat.N1()).fill(twist.dx, twist.dy, twist.dz));
|
||||
|
||||
Matrix<N3, N3> R = Matrix.eye(Nat.N3()).plus(omega.times(A)).plus(omegaSq.times(B));
|
||||
Matrix<N3, N3> V = Matrix.eye(Nat.N3()).plus(omega.times(B)).plus(omegaSq.times(C));
|
||||
Matrix<N3, N1> translation_component = V.times(u);
|
||||
final var transform =
|
||||
new Transform3d(
|
||||
new Translation3d(translation.get(0, 0), translation.get(1, 0), translation.get(2, 0)),
|
||||
new Rotation3d(twist.rx, twist.ry, twist.rz));
|
||||
new Translation3d(
|
||||
translation_component.get(0, 0),
|
||||
translation_component.get(1, 0),
|
||||
translation_component.get(2, 0)),
|
||||
new Rotation3d(R));
|
||||
|
||||
return this.plus(transform);
|
||||
}
|
||||
@@ -240,50 +251,43 @@ public class Pose3d implements Interpolatable<Pose3d> {
|
||||
* @return The twist that maps this to end.
|
||||
*/
|
||||
public Twist3d log(Pose3d end) {
|
||||
// Implementation from Section 3.2 of https://ethaneade.org/lie.pdf
|
||||
final var transform = end.relativeTo(this);
|
||||
|
||||
final var rotVec = transform.getRotation().getQuaternion().toRotationVector();
|
||||
final var rvec = transform.getRotation().getQuaternion().toRotationVector();
|
||||
|
||||
final var Omega = rotationVectorToMatrix(rotVec);
|
||||
final var OmegaSq = Omega.times(Omega);
|
||||
final var omega = rotationVectorToMatrix(rvec);
|
||||
final var theta = transform.getRotation().getAngle();
|
||||
final var thetaSq = theta * theta;
|
||||
|
||||
double thetaSq =
|
||||
rotVec.get(0, 0) * rotVec.get(0, 0)
|
||||
+ rotVec.get(1, 0) * rotVec.get(1, 0)
|
||||
+ rotVec.get(2, 0) * rotVec.get(2, 0);
|
||||
|
||||
// Get left Jacobian inverse of SO3. See fourth line in right column of
|
||||
// http://asrl.utias.utoronto.ca/~tdb/bib/barfoot_ser17_identities.pdf
|
||||
Matrix<N3, N3> Jinv;
|
||||
if (thetaSq < 1E-9 * 1E-9) {
|
||||
// J⁻¹ = I − 0.5ω + 1/12 ω²
|
||||
Jinv = Matrix.eye(Nat.N3()).minus(Omega.times(0.5)).plus(OmegaSq.times(1.0 / 12.0));
|
||||
double C;
|
||||
if (theta < 1E-9) {
|
||||
// Taylor Expansions around θ = 0
|
||||
// A = 1/1! - θ²/3! + θ⁴/5!
|
||||
// B = 1/2! - θ²/4! + θ⁴/6!
|
||||
// C = 1/6 * (1/2 + θ²/5! + θ⁴/7!)
|
||||
C = 1 / 6.0 - thetaSq / 120 + thetaSq * thetaSq / 5040;
|
||||
} else {
|
||||
double theta = Math.sqrt(thetaSq);
|
||||
double halfTheta = 0.5 * theta;
|
||||
|
||||
// J⁻¹ = I − 0.5ω + (1 − 0.5θ cos(θ/2) / sin(θ/2))/θ² ω²
|
||||
Jinv =
|
||||
Matrix.eye(Nat.N3())
|
||||
.minus(Omega.times(0.5))
|
||||
.plus(
|
||||
OmegaSq.times(
|
||||
(1.0 - 0.5 * theta * Math.cos(halfTheta) / Math.sin(halfTheta)) / thetaSq));
|
||||
// A = sin(θ)/θ
|
||||
// B = (1 - cos(θ)) / θ²
|
||||
// C = (1 - A/(2*B)) / θ²
|
||||
double A = Math.sin(theta) / theta;
|
||||
double B = (1 - Math.cos(theta)) / thetaSq;
|
||||
C = (1 - A / (2 * B)) / thetaSq;
|
||||
}
|
||||
|
||||
// Get dtranslation component
|
||||
final var dtranslation =
|
||||
Jinv.times(
|
||||
new MatBuilder<>(Nat.N3(), Nat.N1())
|
||||
.fill(transform.getX(), transform.getY(), transform.getZ()));
|
||||
final var V_inv = Matrix.eye(Nat.N3()).minus(omega.times(0.5)).plus(omega.pow(2).times(C));
|
||||
|
||||
final var twist_translation =
|
||||
V_inv.times(VecBuilder.fill(transform.getX(), transform.getY(), transform.getZ()));
|
||||
|
||||
return new Twist3d(
|
||||
dtranslation.get(0, 0),
|
||||
dtranslation.get(1, 0),
|
||||
dtranslation.get(2, 0),
|
||||
rotVec.get(0, 0),
|
||||
rotVec.get(1, 0),
|
||||
rotVec.get(2, 0));
|
||||
twist_translation.get(0, 0),
|
||||
twist_translation.get(1, 0),
|
||||
twist_translation.get(2, 0),
|
||||
rvec.get(0, 0),
|
||||
rvec.get(1, 0),
|
||||
rvec.get(2, 0));
|
||||
}
|
||||
|
||||
/**
|
||||
|
||||
@@ -71,73 +71,86 @@ Pose3d Pose3d::RelativeTo(const Pose3d& other) const {
|
||||
}
|
||||
|
||||
Pose3d Pose3d::Exp(const Twist3d& twist) const {
|
||||
Matrixd<3, 3> Omega = RotationVectorToMatrix(
|
||||
Vectord<3>{twist.rx.value(), twist.ry.value(), twist.rz.value()});
|
||||
Matrixd<3, 3> OmegaSq = Omega * Omega;
|
||||
// Implementation from Section 3.2 of https://ethaneade.org/lie.pdf
|
||||
auto u = Vectord<3>{twist.dx.value(), twist.dy.value(), twist.dz.value()};
|
||||
auto rvec = Vectord<3>{twist.rx.value(), twist.ry.value(), twist.rz.value()};
|
||||
auto omega = RotationVectorToMatrix(rvec);
|
||||
auto omegaSq = omega * omega;
|
||||
auto theta = rvec.norm();
|
||||
auto thetaSq = theta * theta;
|
||||
|
||||
double thetaSq =
|
||||
(twist.rx * twist.rx + twist.ry * twist.ry + twist.rz * twist.rz).value();
|
||||
|
||||
// Get left Jacobian of SO3. See first line in right column of
|
||||
// http://asrl.utias.utoronto.ca/~tdb/bib/barfoot_ser17_identities.pdf
|
||||
Matrixd<3, 3> J;
|
||||
if (thetaSq < 1E-9 * 1E-9) {
|
||||
// J = I + 0.5ω
|
||||
J = Matrixd<3, 3>::Identity() + 0.5 * Omega;
|
||||
double A;
|
||||
double B;
|
||||
double C;
|
||||
if (theta < 1E-9) {
|
||||
// Taylor Expansions around θ = 0
|
||||
// A = 1/1! - θ²/3! + θ⁴/5!
|
||||
// B = 1/2! - θ²/4! + θ⁴/6!
|
||||
// C = 1/3! - θ²/5! + θ⁴/7!
|
||||
A = 1 - thetaSq / 6 + thetaSq * thetaSq / 120;
|
||||
B = 1 / 2.0 - thetaSq / 24 + thetaSq * thetaSq / 720;
|
||||
C = 1 / 6.0 - thetaSq / 120 + thetaSq * thetaSq / 5040;
|
||||
} else {
|
||||
double theta = std::sqrt(thetaSq);
|
||||
// J = I + (1 − std::cos(θ))/θ² ω + (θ − std::sin(θ))/θ³ ω²
|
||||
J = Matrixd<3, 3>::Identity() + (1.0 - std::cos(theta)) / thetaSq * Omega +
|
||||
(theta - std::sin(theta)) / (thetaSq * theta) * OmegaSq;
|
||||
// A = std::sin(θ)/θ
|
||||
// B = (1 - std::cos(θ)) / θ²
|
||||
// C = (1 - A) / θ²
|
||||
A = std::sin(theta) / theta;
|
||||
B = (1 - std::cos(theta)) / thetaSq;
|
||||
C = (1 - A) / thetaSq;
|
||||
}
|
||||
|
||||
// Get translation component
|
||||
Vectord<3> translation =
|
||||
J * Vectord<3>{twist.dx.value(), twist.dy.value(), twist.dz.value()};
|
||||
auto R = Matrixd<3, 3>::Identity() + A * omega + B * omegaSq;
|
||||
auto V = Matrixd<3, 3>::Identity() + B * omega + C * omegaSq;
|
||||
|
||||
const Transform3d transform{Translation3d{units::meter_t{translation(0)},
|
||||
units::meter_t{translation(1)},
|
||||
units::meter_t{translation(2)}},
|
||||
Rotation3d{twist.rx, twist.ry, twist.rz}};
|
||||
auto translation_component = V * u;
|
||||
const Transform3d transform{
|
||||
Translation3d{units::meter_t{translation_component(0)},
|
||||
units::meter_t{translation_component(1)},
|
||||
units::meter_t{translation_component(2)}},
|
||||
Rotation3d{R}};
|
||||
|
||||
return *this + transform;
|
||||
}
|
||||
|
||||
Twist3d Pose3d::Log(const Pose3d& end) const {
|
||||
// Implementation from Section 3.2 of https://ethaneade.org/lie.pdf
|
||||
const auto transform = end.RelativeTo(*this);
|
||||
|
||||
Vectord<3> rotVec = transform.Rotation().GetQuaternion().ToRotationVector();
|
||||
auto u = Vectord<3>{transform.X().value(), transform.Y().value(),
|
||||
transform.Z().value()};
|
||||
auto rvec = transform.Rotation().GetQuaternion().ToRotationVector();
|
||||
|
||||
Matrixd<3, 3> Omega = RotationVectorToMatrix(rotVec);
|
||||
Matrixd<3, 3> OmegaSq = Omega * Omega;
|
||||
auto omega = RotationVectorToMatrix(rvec);
|
||||
auto omegaSq = omega * omega;
|
||||
auto theta = rvec.norm();
|
||||
auto thetaSq = theta * theta;
|
||||
|
||||
double thetaSq = rotVec.squaredNorm();
|
||||
|
||||
// Get left Jacobian inverse of SO3. See fourth line in right column of
|
||||
// http://asrl.utias.utoronto.ca/~tdb/bib/barfoot_ser17_identities.pdf
|
||||
Matrixd<3, 3> Jinv;
|
||||
if (thetaSq < 1E-9 * 1E-9) {
|
||||
// J⁻¹ = I − 0.5ω + 1/12 ω²
|
||||
Jinv = Matrixd<3, 3>::Identity() - 0.5 * Omega + 1.0 / 12.0 * OmegaSq;
|
||||
double C;
|
||||
if (theta < 1E-9) {
|
||||
// Taylor Expansions around θ = 0
|
||||
// A = 1/1! - θ²/3! + θ⁴/5!
|
||||
// B = 1/2! - θ²/4! + θ⁴/6!
|
||||
// C = 1/6 * (1/2 + θ²/5! + θ⁴/7!)
|
||||
C = 1 / 6.0 - thetaSq / 120 + thetaSq * thetaSq / 5040;
|
||||
} else {
|
||||
double theta = std::sqrt(thetaSq);
|
||||
double halfTheta = 0.5 * theta;
|
||||
|
||||
// J⁻¹ = I − 0.5ω + (1 − 0.5θ std::cos(θ/2) / std::sin(θ/2))/θ² ω²
|
||||
Jinv = Matrixd<3, 3>::Identity() - 0.5 * Omega +
|
||||
(1.0 - 0.5 * theta * std::cos(halfTheta) / std::sin(halfTheta)) /
|
||||
thetaSq * OmegaSq;
|
||||
// A = std::sin(θ)/θ
|
||||
// B = (1 - std::cos(θ)) / θ²
|
||||
// C = (1 - A/(2*B)) / θ²
|
||||
double A = std::sin(theta) / theta;
|
||||
double B = (1 - std::cos(theta)) / thetaSq;
|
||||
C = (1 - A / (2 * B)) / thetaSq;
|
||||
}
|
||||
|
||||
// Get dtranslation component
|
||||
Vectord<3> dtranslation =
|
||||
Jinv * Vectord<3>{transform.X().value(), transform.Y().value(),
|
||||
transform.Z().value()};
|
||||
auto V_inv = Matrixd<3, 3>::Identity() - 0.5 * omega + C * omegaSq;
|
||||
|
||||
return Twist3d{
|
||||
units::meter_t{dtranslation(0)}, units::meter_t{dtranslation(1)},
|
||||
units::meter_t{dtranslation(2)}, units::radian_t{rotVec(0)},
|
||||
units::radian_t{rotVec(1)}, units::radian_t{rotVec(2)}};
|
||||
auto translation_component = V_inv * u;
|
||||
|
||||
return Twist3d{units::meter_t{translation_component(0)},
|
||||
units::meter_t{translation_component(1)},
|
||||
units::meter_t{translation_component(2)},
|
||||
units::radian_t{rvec(0)},
|
||||
units::radian_t{rvec(1)},
|
||||
units::radian_t{rvec(2)}};
|
||||
}
|
||||
|
||||
Pose2d Pose3d::ToPose2d() const {
|
||||
|
||||
@@ -10,6 +10,7 @@ import static org.junit.jupiter.api.Assertions.assertNotEquals;
|
||||
|
||||
import edu.wpi.first.math.VecBuilder;
|
||||
import edu.wpi.first.math.util.Units;
|
||||
import java.util.Arrays;
|
||||
import org.junit.jupiter.api.Test;
|
||||
|
||||
class Pose3dTest {
|
||||
@@ -49,10 +50,13 @@ class Pose3dTest {
|
||||
Units.degreesToRadians(-45.0),
|
||||
Units.degreesToRadians(0.0)));
|
||||
|
||||
// This sequence of rotations should diverge from the origin and eventually return to it. When
|
||||
// each rotation occurs, it should be performed intrinsicly, i.e. 'from the viewpoint' of and
|
||||
// This sequence of rotations should diverge from the origin and eventually
|
||||
// return to it. When
|
||||
// each rotation occurs, it should be performed intrinsicly, i.e. 'from the
|
||||
// viewpoint' of and
|
||||
// with
|
||||
// the axes of the pose that is being transformed, just like how the translation is done 'from
|
||||
// the axes of the pose that is being transformed, just like how the translation
|
||||
// is done 'from
|
||||
// the
|
||||
// viewpoint' of the pose that is being transformed.
|
||||
var finalPose =
|
||||
@@ -153,4 +157,77 @@ class Pose3dTest {
|
||||
|
||||
assertEquals(expected, pose.toPose2d());
|
||||
}
|
||||
|
||||
@Test
|
||||
void testComplexTwists() {
|
||||
var initial_poses =
|
||||
Arrays.asList(
|
||||
new Pose3d(
|
||||
new Translation3d(0.698303, -0.959096, 0.271076),
|
||||
new Rotation3d(new Quaternion(0.86403, -0.076866, 0.147234, 0.475254))),
|
||||
new Pose3d(
|
||||
new Translation3d(0.634892, -0.765209, 0.117543),
|
||||
new Rotation3d(new Quaternion(0.84987, -0.070829, 0.162097, 0.496415))),
|
||||
new Pose3d(
|
||||
new Translation3d(0.584827, -0.590303, -0.02557),
|
||||
new Rotation3d(new Quaternion(0.832743, -0.041991, 0.202188, 0.513708))),
|
||||
new Pose3d(
|
||||
new Translation3d(0.505038, -0.451479, -0.112835),
|
||||
new Rotation3d(new Quaternion(0.816515, -0.002673, 0.226182, 0.531166))),
|
||||
new Pose3d(
|
||||
new Translation3d(0.428178, -0.329692, -0.189707),
|
||||
new Rotation3d(new Quaternion(0.807886, 0.029298, 0.257788, 0.529157))));
|
||||
|
||||
var final_poses =
|
||||
Arrays.asList(
|
||||
new Pose3d(
|
||||
new Translation3d(-0.230448, -0.511957, 0.198406),
|
||||
new Rotation3d(new Quaternion(0.753984, 0.347016, 0.409105, 0.379106))),
|
||||
new Pose3d(
|
||||
new Translation3d(-0.088932, -0.343253, 0.095018),
|
||||
new Rotation3d(new Quaternion(0.638738, 0.413016, 0.536281, 0.365833))),
|
||||
new Pose3d(
|
||||
new Translation3d(-0.107908, -0.317552, 0.133946),
|
||||
new Rotation3d(new Quaternion(0.653444, 0.417069, 0.465505, 0.427046))),
|
||||
new Pose3d(
|
||||
new Translation3d(-0.123383, -0.156411, -0.047435),
|
||||
new Rotation3d(new Quaternion(0.652983, 0.40644, 0.431566, 0.47135))),
|
||||
new Pose3d(
|
||||
new Translation3d(-0.084654, -0.019305, -0.030022),
|
||||
new Rotation3d(new Quaternion(0.620243, 0.429104, 0.479384, 0.44873))));
|
||||
|
||||
final var eps = 1E-5;
|
||||
for (int i = 0; i < initial_poses.size(); i++) {
|
||||
var start = initial_poses.get(i);
|
||||
var end = final_poses.get(i);
|
||||
|
||||
var twist = start.log(end);
|
||||
var start_exp = start.exp(twist);
|
||||
|
||||
assertAll(
|
||||
() -> assertEquals(start_exp.getX(), end.getX(), eps),
|
||||
() -> assertEquals(start_exp.getY(), end.getY(), eps),
|
||||
() -> assertEquals(start_exp.getZ(), end.getZ(), eps),
|
||||
() ->
|
||||
assertEquals(
|
||||
start_exp.getRotation().getQuaternion().getW(),
|
||||
end.getRotation().getQuaternion().getW(),
|
||||
eps),
|
||||
() ->
|
||||
assertEquals(
|
||||
start_exp.getRotation().getQuaternion().getX(),
|
||||
end.getRotation().getQuaternion().getX(),
|
||||
eps),
|
||||
() ->
|
||||
assertEquals(
|
||||
start_exp.getRotation().getQuaternion().getY(),
|
||||
end.getRotation().getQuaternion().getY(),
|
||||
eps),
|
||||
() ->
|
||||
assertEquals(
|
||||
start_exp.getRotation().getQuaternion().getZ(),
|
||||
end.getRotation().getQuaternion().getZ(),
|
||||
eps));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -4,6 +4,8 @@
|
||||
|
||||
#include <cmath>
|
||||
|
||||
#include <wpi/array.h>
|
||||
|
||||
#include "frc/geometry/Pose3d.h"
|
||||
#include "gtest/gtest.h"
|
||||
|
||||
@@ -95,3 +97,53 @@ TEST(Pose3dTest, ToPose2d) {
|
||||
|
||||
EXPECT_EQ(expected, pose.ToPose2d());
|
||||
}
|
||||
|
||||
TEST(Pose3dTest, ComplexTwists) {
|
||||
wpi::array<Pose3d, 5> initial_poses{
|
||||
Pose3d{0.698303_m, -0.959096_m, 0.271076_m,
|
||||
Rotation3d{Quaternion{0.86403, -0.076866, 0.147234, 0.475254}}},
|
||||
Pose3d{0.634892_m, -0.765209_m, 0.117543_m,
|
||||
Rotation3d{Quaternion{0.84987, -0.070829, 0.162097, 0.496415}}},
|
||||
Pose3d{0.584827_m, -0.590303_m, -0.02557_m,
|
||||
Rotation3d{Quaternion{0.832743, -0.041991, 0.202188, 0.513708}}},
|
||||
Pose3d{0.505038_m, -0.451479_m, -0.112835_m,
|
||||
Rotation3d{Quaternion{0.816515, -0.002673, 0.226182, 0.531166}}},
|
||||
Pose3d{0.428178_m, -0.329692_m, -0.189707_m,
|
||||
Rotation3d{Quaternion{0.807886, 0.029298, 0.257788, 0.529157}}},
|
||||
};
|
||||
|
||||
wpi::array<Pose3d, 5> final_poses{
|
||||
Pose3d{-0.230448_m, -0.511957_m, 0.198406_m,
|
||||
Rotation3d{Quaternion{0.753984, 0.347016, 0.409105, 0.379106}}},
|
||||
Pose3d{-0.088932_m, -0.343253_m, 0.095018_m,
|
||||
Rotation3d{Quaternion{0.638738, 0.413016, 0.536281, 0.365833}}},
|
||||
Pose3d{-0.107908_m, -0.317552_m, 0.133946_m,
|
||||
Rotation3d{Quaternion{0.653444, 0.417069, 0.465505, 0.427046}}},
|
||||
Pose3d{-0.123383_m, -0.156411_m, -0.047435_m,
|
||||
Rotation3d{Quaternion{0.652983, 0.40644, 0.431566, 0.47135}}},
|
||||
Pose3d{-0.084654_m, -0.019305_m, -0.030022_m,
|
||||
Rotation3d{Quaternion{0.620243, 0.429104, 0.479384, 0.44873}}},
|
||||
};
|
||||
|
||||
for (size_t i = 0; i < initial_poses.size(); i++) {
|
||||
auto start = initial_poses[i];
|
||||
auto end = final_poses[i];
|
||||
|
||||
auto twist = start.Log(end);
|
||||
auto start_exp = start.Exp(twist);
|
||||
|
||||
auto eps = 1E-5;
|
||||
|
||||
EXPECT_NEAR(start_exp.X().value(), end.X().value(), eps);
|
||||
EXPECT_NEAR(start_exp.Y().value(), end.Y().value(), eps);
|
||||
EXPECT_NEAR(start_exp.Z().value(), end.Z().value(), eps);
|
||||
EXPECT_NEAR(start_exp.Rotation().GetQuaternion().W(),
|
||||
end.Rotation().GetQuaternion().W(), eps);
|
||||
EXPECT_NEAR(start_exp.Rotation().GetQuaternion().X(),
|
||||
end.Rotation().GetQuaternion().X(), eps);
|
||||
EXPECT_NEAR(start_exp.Rotation().GetQuaternion().Y(),
|
||||
end.Rotation().GetQuaternion().Y(), eps);
|
||||
EXPECT_NEAR(start_exp.Rotation().GetQuaternion().Z(),
|
||||
end.Rotation().GetQuaternion().Z(), eps);
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user