30 using namespace gtsam;
36 static
Point3 P(0.2, 0.7, -2.0);
49 Matrix R = (
Matrix(3, 3) << 0, 1, 0, 1, 0, 0, 0, 0, -1).finished();
65 Matrix R = (
Matrix(3, 3) << 0, 1, 0, 1, 0, 0, 0, 0, -1).finished();
83 Rot3 R(0, 1, 0, 1, 0, 0, 0, 0, -1);
100 if (
t < 1
e-5)
return Rot3();
101 Matrix3
R = I_3x3 +
sin(
t) *
J + (1.0 -
cos(
t)) * (
J *
J);
109 double angle = 3.14 / 4.0;
112 -0.706825, 0, 0.707388);
113 Rot3 actual = Rot3::AxisAngle(axis, angle);
115 Rot3 actual2 = Rot3::AxisAngle(axis, angle-2*
M_PI);
119 Rot3 actual3 = Rot3::AxisAngle(axis, angle);
127 Rot3 R1(-0.999957, 0.00922903, 0.00203116, 0.00926964, 0.999739, 0.0208927, -0.0018374, 0.0209105, -0.999781);
130 const auto [actualAxis, actualAngle] =
R1.axisAngle();
132 double expectedAngle = 3.1396582;
149 double angle = 3.14 / 4.0;
152 -0.706825, 0, 0.707388);
153 Rot3 actual = Rot3::AxisAngle(axis, angle);
155 Rot3 actual2 = Rot3::Rodrigues(angle*axis);
163 Rot3 R1 = Rot3::AxisAngle(
w /
w.norm(),
w.norm());
172 double angle =
M_PI/2.0;
173 Rot3 actual = Rot3::AxisAngle(axis, angle);
196 static const double PI =
std::acos(-1.0);
200 #define CHECK_OMEGA(X, Y, Z) \
201 w = (Vector(3) << (X), (Y), (Z)).finished(); \
202 R = Rot3::Rodrigues(w); \
203 EXPECT(assert_equal(w, Rot3::Logmap(R), 1e-12));
209 double norm =
sqrt(1.0 + 16.0 + 4.0);
210 double x = 1.0 / norm,
y = 4.0 / norm,
z = 2.0 / norm;
234 w = (
Vector(3) <<
x * PI,
y * PI,
z * PI).finished();
235 R = Rot3::Rodrigues(
w);
242 #define CHECK_OMEGA_ZERO(X, Y, Z) \
243 w = (Vector(3) << (X), (Y), (Z)).finished(); \
244 R = Rot3::Rodrigues(w); \
245 EXPECT(assert_equal((Vector)Z_3x1, Rot3::Logmap(R)));
254 Rot3 Rlund(-0.98582676, -0.03958746, -0.16303092,
255 -0.03997006, -0.88835923, 0.45740671,
256 -0.16293753, 0.45743998, 0.87418537);
261 #if defined(GTSAM_USE_QUATERNIONS)
264 (
Vector)Rot3::Logmap(Rlund), 1
e-8));
268 (
Vector)Rot3::Logmap(Rlund), 1
e-8));
275 Vector3 d12 = Vector3::Constant(0.1);
282 Vector d12 = Vector3::Constant(0.1);
293 Vector d21 = t2.localCoordinates(t1);
300 Rot3 gR1 = Rot3::Rodrigues(0.1, 0.4, 0.2);
301 Rot3 gR2 = Rot3::Rodrigues(0.3, 0.1, 0.7);
330 template <
typename Derived>
357 Matrix actualDrotate1a, actualDrotate1b, actualDrotate2;
358 R.
rotate(
P, actualDrotate1a, actualDrotate2);
386 Rot3 R1 = Rot3::Rodrigues(0.1, 0.2, 0.3);
387 Rot3 R2 = Rot3::Rodrigues(0.2, 0.3, 0.5);
390 Matrix actualH1, actualH2;
406 Rot3 R = Rot3::Rodrigues(0.1, 0.2, 0.3);
426 0.5, -
sqrt(3.0)/2.0, 0.0,
427 sqrt(3.0)/2.0, 0.5, 0.0,
428 0.0, 0.0, 1.0).finished();
431 Rot3 R = Rot3::Rodrigues(0.1, 0.4, 0.2);
436 Rot3 R1 = Rot3::Rodrigues(0.1, 0.2, 0.3);
437 Rot3 R2 = Rot3::Rodrigues(0.2, 0.3, 0.5);
440 Matrix actualH1, actualH2;
454 double t = 0.1, st =
sin(
t), ct =
cos(
t);
462 Rot3 expected1(1, 0, 0, 0, ct, -st, 0, st, ct);
468 Rot3 expected2(ct, 0, st, 0, 1, 0, -st, 0, ct);
474 Rot3 expected3(ct, -st, 0, st, ct, 0, 0, 0, 1);
478 Rot3 expected = Rot3::Rz(0.3) * Rot3::Ry(0.2) * Rot3::Rx(0.1);
497 Rot3 expected = Rot3::Yaw(0.1) * Rot3::Pitch(0.2) * Rot3::Roll(0.3);
507 const auto [actualK, actual] =
RQ(
R.
matrix());
526 Matrix K = (
Matrix(3, 3) << 500.0, 0.0, 320.0, 0.0, 500.0, 240.0, 0.0, 0.0, 1.0).finished();
528 const auto [actualK2, actual2] =
RQ(
A);
536 double theta =
w.norm();
537 double theta2 = theta*theta;
541 -
w(1),
w(0), 0.0 ).finished();
543 Matrix Rmat = I_3x3 + (1.0-theta2/6.0 + theta2*theta2/120.0
544 - theta2*theta2*theta2/5040.0)*
W + (0.5 - theta2/24.0 + theta2*theta2/720.0)*W2 ;
558 Vector actualw = Rot3::Logmap(
R);
565 Quaternion q1(0.710997408193224, 0.360544029310185, 0.594459869568306, 0.105395217842782);
566 Rot3 R1(0.271018623057411, 0.278786459830371, 0.921318086098018,
567 0.578529366719085, 0.717799701969298, -0.387385285854279,
568 -0.769319620053772, 0.637998195662053, 0.033250932803219);
570 Quaternion q2(0.263360579192421, 0.571813128030932, 0.494678363680335,
572 Rot3 R2(-0.207341903877828, 0.250149415542075, 0.945745528564780,
573 0.881304914479026, -0.371869043667957, 0.291573424846290,
574 0.424630407073532, 0.893945571198514, -0.143353873763946);
603 eigenQuaternion.w() = 1.0;
604 eigenQuaternion.x() = 2.0;
605 eigenQuaternion.y() = 3.0;
606 eigenQuaternion.z() = 4.0;
612 Rot3 R(eigenQuaternion);
637 std::ostringstream
os;
639 string expected =
"[\n\t1, 0, 0;\n\t0, 1, 0;\n\t0, 0, 1\n]";
647 Rot3 R1 = Rot3::Rz(1),
R2 = Rot3::Rz(2),
R3 = Rot3::Rz(1.5);
664 EXPECT(check_group_invariants(
id,
id));
665 EXPECT(check_group_invariants(
id,
T1));
666 EXPECT(check_group_invariants(
T2,
id));
670 EXPECT(check_manifold_invariants(
id,
id));
671 EXPECT(check_manifold_invariants(
id,
T1));
672 EXPECT(check_manifold_invariants(
T2,
id));
700 M << 0.79067393, 0.6051136, -0.0930814,
701 0.4155925, -0.64214347, -0.64324489,
702 -0.44948549, 0.47046326, -0.75917576;
705 expected << 0.790687, 0.605096, -0.0931312,
706 0.415746, -0.642355, -0.643844,
707 -0.449411, 0.47036, -0.759468;
709 auto actual = Rot3::ClosestTo(3*
M);
719 #define CHECK_AXIS_ANGLE(expectedAxis, expectedAngle, rotation) \
720 std::tie(actualAxis, actualAngle) = rotation.axisAngle(); \
721 EXPECT(assert_equal(expectedAxis, actualAxis, 1e-9)); \
722 EXPECT_DOUBLES_EQUAL(expectedAngle, actualAngle, 1e-9); \
723 EXPECT(assert_equal(rotation, Rot3::AxisAngle(expectedAxis, expectedAngle)))
743 const double theta270 =
M_PI +
M_PI / 2;
750 const double theta_270 = -(
M_PI +
M_PI / 2);
756 const double theta195 = 195 *
M_PI / 180;
757 const double theta165 = 165 *
M_PI / 180;
774 return Rot3::RzRyRx(
a,
b,
c);
778 const auto x = 0.1,
y = 0.4,
z = 0.2;
784 Rot3::RzRyRx(
x,
y,
z, act_x, act_y, act_z);
795 const auto xyz =
Vector3{-0.3, 0.1, 0.7};
799 Rot3::RzRyRx(xyz, act);
806 return Rot3::Ypr(
y,
p, r);
810 const auto y = 0.7,
p = -0.3, r = 0.1;
816 Rot3::Ypr(
y,
p, r, act_y, act_p, act_r);
825 const auto RQ_ypr =
RQ(
R);
826 return RQ_ypr.second;
830 using VecAndErr = std::pair<Vector3, double>;
831 std::vector<VecAndErr> test_xyz;
833 test_xyz.push_back(VecAndErr{{0, 0, 0},
error});
834 test_xyz.push_back(VecAndErr{{0, 0.5, -0.5},
error});
835 test_xyz.push_back(VecAndErr{{0.3, 0, 0.2},
error});
836 test_xyz.push_back(VecAndErr{{-0.6, 1.3, 0}, 1
e-8});
837 test_xyz.push_back(VecAndErr{{1.0, 0.7, 0.8},
error});
838 test_xyz.push_back(VecAndErr{{3.0, 0.7, -0.6},
error});
839 test_xyz.push_back(VecAndErr{{
M_PI / 2, 0, 0},
error});
840 test_xyz.push_back(VecAndErr{{0, 0,
M_PI / 2},
error});
843 test_xyz.push_back(VecAndErr{{0,
M_PI / 2 - 1
e-1, 0}, 1
e-7});
844 test_xyz.push_back(VecAndErr{{0, 3 *
M_PI / 2 + 1
e-1, 0}, 1
e-7});
845 test_xyz.push_back(VecAndErr{{0,
M_PI / 2 - 1.1e-2, 0}, 1
e-4});
846 test_xyz.push_back(VecAndErr{{0, 3 *
M_PI / 2 + 1.1e-2, 0}, 1
e-4});
848 for (
auto const& vec_err : test_xyz) {
849 auto const& xyz = vec_err.first;
851 const auto R = Rot3::RzRyRx(xyz).
matrix();
856 const auto err = vec_err.second;
865 const auto aa =
Vector3{-0.6, 0.3, 0.2};
878 const auto aa =
Vector3{0.1, -0.3, -0.2};
891 const auto aa =
Vector3{1.2, 0.3, -0.9};
904 const auto aa =
Vector3{0.8, -0.8, 0.8};
917 const auto aa =
Vector3{0.01, 0.1, 0.0};
930 const auto aa =
Vector3{0.0, 0.1, 0.6};
948 for (
size_t i = 2;
i < 360; ++
i) {
953 actual = R_w2.
matrix().determinant();
963 M << 1, 2, 3, 4, 5, 6, 7, 8, 9;
969 const Matrix3
expected = numericalDerivative11<Rot3, Vector3>(
g,
Z_3x1);
974 const Matrix3 expected2 = numericalDerivative11<Rot3, Vector3>(
g,
delta);
975 EXPECT(assert_equal<Matrix3>(expected2, SO3::ExpmapDerivative(
M*
delta) *
M, 1
e-5));
983 M << 1, 2, 3, 4, 5, 6, 7, 8, 9;
990 const Matrix3
expected = numericalDerivative11<Rot3, Vector3>(
g,
Z_3x1);
995 const Matrix3 expected2 = numericalDerivative11<Rot3, Vector3>(
g,
delta);
996 EXPECT(assert_equal<Matrix3>(expected2, SO3::ExpmapDerivative(
M*
delta) *
M, 1
e-5));