testPoseRTV.cpp
Go to the documentation of this file.
1 
6 #include <CppUnitLite/TestHarness.h>
7 #include <gtsam/base/Testable.h>
8 #include <gtsam/base/TestableAssertions.h>
9 #include <gtsam/base/numericalDerivative.h>
10 
11 #include <gtsam_unstable/dynamics/PoseRTV.h>
12 
13 using namespace gtsam;
14 
15 GTSAM_CONCEPT_TESTABLE_INST(PoseRTV)
16 GTSAM_CONCEPT_LIE_INST(PoseRTV)
17 
18 static const Rot3 rot = Rot3::RzRyRx(0.1, 0.2, 0.3);
19 static const Point3 pt(1.0, 2.0, 3.0);
20 static const Velocity3 vel(0.4, 0.5, 0.6);
21 static const Vector3 kZero3 = Vector3::Zero();
22 
23 /* ************************************************************************* */
24 TEST( testPoseRTV, constructors ) {
25  PoseRTV state1(pt, rot, vel);
26  EXPECT(assert_equal(pt, state1.t()));
27  EXPECT(assert_equal(rot, state1.R()));
28  EXPECT(assert_equal(vel, state1.v()));
29  EXPECT(assert_equal(Pose3(rot, pt), state1.pose()));
30 
31  PoseRTV state2;
32  EXPECT(assert_equal(Point3(0,0,0), state2.t()));
33  EXPECT(assert_equal(Rot3(), state2.R()));
34  EXPECT(assert_equal(kZero3, state2.v()));
35  EXPECT(assert_equal(Pose3(), state2.pose()));
36 
37  PoseRTV state3(Pose3(rot, pt), vel);
38  EXPECT(assert_equal(pt, state3.t()));
39  EXPECT(assert_equal(rot, state3.R()));
40  EXPECT(assert_equal(vel, state3.v()));
41  EXPECT(assert_equal(Pose3(rot, pt), state3.pose()));
42 
43  PoseRTV state4(Pose3(rot, pt));
44  EXPECT(assert_equal(pt, state4.t()));
45  EXPECT(assert_equal(rot, state4.R()));
46  EXPECT(assert_equal(kZero3, state4.v()));
47  EXPECT(assert_equal(Pose3(rot, pt), state4.pose()));
48 
49  Vector vec_init = (Vector(9) << 0.1, 0.2, 0.3, 1.0, 2.0, 3.0, 0.4, 0.5, 0.6).finished();
50  PoseRTV state5(vec_init);
51  EXPECT(assert_equal(pt, state5.t()));
52  EXPECT(assert_equal(rot, state5.R()));
53  EXPECT(assert_equal(vel, state5.v()));
54  EXPECT(assert_equal(vec_init, state5.vector()));
55 }
56 
57 /* ************************************************************************* */
58 TEST( testPoseRTV, dim ) {
59  PoseRTV state1(pt, rot, vel);
60  EXPECT_LONGS_EQUAL(9, state1.dim());
61  EXPECT_LONGS_EQUAL(9, PoseRTV::Dim());
62 }
63 
64 /* ************************************************************************* */
65 TEST( testPoseRTV, equals ) {
66  PoseRTV state1, state2(pt, rot, vel), state3(state2), state4(Pose3(rot, pt));
67  EXPECT(assert_equal(state1, state1));
68  EXPECT(assert_equal(state2, state3));
69  EXPECT(assert_equal(state3, state2));
70  EXPECT(assert_inequal(state1, state2));
71  EXPECT(assert_inequal(state2, state1));
72  EXPECT(assert_inequal(state2, state4));
73 }
74 
75 /* ************************************************************************* */
76 TEST( testPoseRTV, Lie ) {
77  // origin and zero deltas
78  PoseRTV identity;
79  EXPECT(assert_equal(identity, (PoseRTV)identity.retract(Z_9x1)));
80  EXPECT(assert_equal((Vector) Z_9x1, identity.localCoordinates(identity)));
81 
82  PoseRTV state1(pt, rot, vel);
83  EXPECT(assert_equal(state1, (PoseRTV)state1.retract(Z_9x1)));
84  EXPECT(assert_equal((Vector) Z_9x1, state1.localCoordinates(state1)));
85 
86  Vector delta(9);
87  delta << 0.1, 0.1, 0.1, 0.2, 0.3, 0.4,-0.1,-0.2,-0.3;
88  Pose3 pose2 = Pose3(rot, pt).retract(delta.head<6>());
89  Velocity3 vel2 = vel + Velocity3(-0.1, -0.2, -0.3);
90  PoseRTV state2(pose2.translation(), pose2.rotation(), vel2);
91  EXPECT(assert_equal(state2, (PoseRTV)state1.retract(delta)));
92  EXPECT(assert_equal(delta, state1.localCoordinates(state2)));
93 
94  // roundtrip from state2 to state3 and back
95  PoseRTV state3 = state2.retract(delta);
96  EXPECT(assert_equal(delta, state2.localCoordinates(state3)));
97 
98  // roundtrip from state3 to state4 and back, with expmap.
99  PoseRTV state4 = state3.expmap(delta);
100  EXPECT(assert_equal(delta, state3.logmap(state4)));
101 
102  // For the expmap/logmap (not necessarily retract/local) -delta goes other way
103  EXPECT(assert_equal(state3, (PoseRTV)state4.expmap(-delta)));
104  EXPECT(assert_equal(delta, -state4.logmap(state3)));
105 }
106 
107 /* ************************************************************************* */
108 TEST( testPoseRTV, dynamics_identities ) {
109  // general dynamics should produce the same measurements as the imuPrediction function
110  PoseRTV x0, x1, x2, x3, x4;
111 
112  const double dt = 0.1;
113  Vector accel = Vector3(0.2, 0.0, 0.0), gyro = Vector3(0.0, 0.0, 0.2);
114  Vector imu01 = Z_6x1, imu12 = Z_6x1, imu23 = Z_6x1, imu34 = Z_6x1;
115 
116  x1 = x0.generalDynamics(accel, gyro, dt);
117  x2 = x1.generalDynamics(accel, gyro, dt);
118  x3 = x2.generalDynamics(accel, gyro, dt);
119  x4 = x3.generalDynamics(accel, gyro, dt);
120 
121 // EXPECT(assert_equal(imu01, x0.imuPrediction(x1, dt).first));
122 // EXPECT(assert_equal(imu12, x1.imuPrediction(x2, dt).first));
123 // EXPECT(assert_equal(imu23, x2.imuPrediction(x3, dt).first));
124 // EXPECT(assert_equal(imu34, x3.imuPrediction(x4, dt).first));
125 //
126 // EXPECT(assert_equal(x1.translation(), x0.imuPrediction(x1, dt).second));
127 // EXPECT(assert_equal(x2.translation(), x1.imuPrediction(x2, dt).second));
128 // EXPECT(assert_equal(x3.translation(), x2.imuPrediction(x3, dt).second));
129 // EXPECT(assert_equal(x4.translation(), x3.imuPrediction(x4, dt).second));
130 }
131 
132 
133 /* ************************************************************************* */
134 PoseRTV compose_proxy(const PoseRTV& A, const PoseRTV& B) { return A.compose(B); }
135 TEST( testPoseRTV, compose ) {
136  PoseRTV state1(pt, rot, vel), state2 = state1;
137 
138  Matrix actH1, actH2;
139  state1.compose(state2, actH1, actH2);
140  Matrix numericH1 = numericalDerivative21(compose_proxy, state1, state2);
141  Matrix numericH2 = numericalDerivative22(compose_proxy, state1, state2);
142  EXPECT(assert_equal(numericH1, actH1));
143  EXPECT(assert_equal(numericH2, actH2));
144 }
145 
146 /* ************************************************************************* */
147 PoseRTV between_proxy(const PoseRTV& A, const PoseRTV& B) { return A.between(B); }
148 TEST( testPoseRTV, between ) {
149  PoseRTV state1(pt, rot, vel), state2 = state1;
150 
151  Matrix actH1, actH2;
152  state1.between(state2, actH1, actH2);
153  Matrix numericH1 = numericalDerivative21(between_proxy, state1, state2);
154  Matrix numericH2 = numericalDerivative22(between_proxy, state1, state2);
155  EXPECT(assert_equal(numericH1, actH1));
156  EXPECT(assert_equal(numericH2, actH2));
157 }
158 
159 /* ************************************************************************* */
160 PoseRTV inverse_proxy(const PoseRTV& A) { return A.inverse(); }
161 TEST( testPoseRTV, inverse ) {
162  PoseRTV state1(pt, rot, vel);
163 
164  Matrix actH1;
165  state1.inverse(actH1);
166  Matrix numericH1 = numericalDerivative11(inverse_proxy, state1);
167  EXPECT(assert_equal(numericH1, actH1));
168 }
169 
170 /* ************************************************************************* */
171 double range_proxy(const PoseRTV& A, const PoseRTV& B) { return A.range(B); }
172 TEST( testPoseRTV, range ) {
173  Point3 tA(1.0, 2.0, 3.0), tB(3.0, 2.0, 3.0);
174  PoseRTV rtvA(tA), rtvB(tB);
175  EXPECT_DOUBLES_EQUAL(0.0, rtvA.range(rtvA), 1e-9);
176  EXPECT_DOUBLES_EQUAL(2.0, rtvA.range(rtvB), 1e-9);
177  EXPECT_DOUBLES_EQUAL(2.0, rtvB.range(rtvA), 1e-9);
178 
179  Matrix actH1, actH2;
180  rtvA.range(rtvB, actH1, actH2);
181  Matrix numericH1 = numericalDerivative21(range_proxy, rtvA, rtvB);
182  Matrix numericH2 = numericalDerivative22(range_proxy, rtvA, rtvB);
183  EXPECT(assert_equal(numericH1, actH1));
184  EXPECT(assert_equal(numericH2, actH2));
185 }
186 
187 /* ************************************************************************* */
188 PoseRTV transformed_from_proxy(const PoseRTV& a, const Pose3& trans) {
189  return a.transformed_from(trans);
190 }
191 TEST( testPoseRTV, transformed_from_1 ) {
192  Rot3 R = Rot3::Rodrigues(0.1, 0.2, 0.3);
193  Point3 T(1.0, 2.0, 3.0);
194  Velocity3 V(2.0, 3.0, 4.0);
195  PoseRTV start(R, T, V);
196  Pose3 transform(Rot3::Yaw(M_PI_2), Point3(1.0, 2.0, 3.0));
197 
198  Matrix actDTrans, actDGlobal;
199  PoseRTV actual = start.transformed_from(transform, actDGlobal, actDTrans);
200  PoseRTV expected(transform.compose(start.pose()), transform.rotation().matrix() * V);
201  EXPECT(assert_equal(expected, actual));
202 
203  Matrix numDGlobal = numericalDerivative21(transformed_from_proxy, start, transform, 1e-5); // At 1e-8, fails
204  Matrix numDTrans = numericalDerivative22(transformed_from_proxy, start, transform, 1e-8); // Sensitive to step size
205  EXPECT(assert_equal(numDGlobal, actDGlobal));
206  EXPECT(assert_equal(numDTrans, actDTrans, 1e-5)); // FIXME: still needs analytic derivative
207 }
208 
209 /* ************************************************************************* */
210 TEST( testPoseRTV, transformed_from_2 ) {
211  Rot3 R;
212  Point3 T(1.0, 2.0, 3.0);
213  Velocity3 V(2.0, 3.0, 4.0);
214  PoseRTV start(R, T, V);
215  Pose3 transform(Rot3::Yaw(M_PI_2), Point3(1.0, 2.0, 3.0));
216 
217  Matrix actDTrans, actDGlobal;
218  PoseRTV actual = start.transformed_from(transform, actDGlobal, actDTrans);
219  PoseRTV expected(transform.compose(start.pose()), transform.rotation().matrix() * V);
220  EXPECT(assert_equal(expected, actual));
221 
222  Matrix numDGlobal = numericalDerivative21(transformed_from_proxy, start, transform, 1e-5); // At 1e-8, fails
223  Matrix numDTrans = numericalDerivative22(transformed_from_proxy, start, transform, 1e-8); // Sensitive to step size
224  EXPECT(assert_equal(numDGlobal, actDGlobal));
225  EXPECT(assert_equal(numDTrans, actDTrans, 1e-5)); // FIXME: still needs analytic derivative
226 }
227 
228 /* ************************************************************************* */
229 TEST(testPoseRTV, RRTMbn) {
230  EXPECT(assert_equal(I_3x3, PoseRTV::RRTMbn(kZero3)));
231  EXPECT(assert_equal(I_3x3, PoseRTV::RRTMbn(Rot3())));
232  EXPECT(assert_equal(PoseRTV::RRTMbn(Vector3(0.3, 0.2, 0.1)), PoseRTV::RRTMbn(Rot3::Ypr(0.1, 0.2, 0.3))));
233 }
234 
235 /* ************************************************************************* */
236 TEST(testPoseRTV, RRTMnb) {
237  EXPECT(assert_equal(I_3x3, PoseRTV::RRTMnb(kZero3)));
238  EXPECT(assert_equal(I_3x3, PoseRTV::RRTMnb(Rot3())));
239  EXPECT(assert_equal(PoseRTV::RRTMnb(Vector3(0.3, 0.2, 0.1)), PoseRTV::RRTMnb(Rot3::Ypr(0.1, 0.2, 0.3))));
240 }
241 
242 /* ************************************************************************* */
243 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
244 /* ************************************************************************* */
245 
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
gtsam::ProductLieGroup::localCoordinates
TangentVector localCoordinates(const ProductLieGroup &g, ChartJacobian H1={}, ChartJacobian H2={}) const
Definition: ProductLieGroup.h:84
B
Matrix< SCALARB, Dynamic, Dynamic, opt_B > B
Definition: bench_gemm.cpp:49
gtsam::PoseRTV::transformed_from
PoseRTV transformed_from(const Pose3 &trans, ChartJacobian Dglobal={}, OptionalJacobian< 9, 6 > Dtrans={}) const
Definition: PoseRTV.cpp:182
inverse
const EIGEN_DEVICE_FUNC InverseReturnType inverse() const
Definition: ArrayCwiseUnaryOps.h:411
gtsam::between
Expression< T > between(const Expression< T > &t1, const Expression< T > &t2)
Definition: nonlinear/expressions.h:17
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
GTSAM_CONCEPT_TESTABLE_INST
#define GTSAM_CONCEPT_TESTABLE_INST(T)
Definition: Testable.h:176
EXPECT_LONGS_EQUAL
#define EXPECT_LONGS_EQUAL(expected, actual)
Definition: Test.h:154
Testable.h
Concept check for values that can be used in unit tests.
EXPECT
#define EXPECT(condition)
Definition: Test.h:150
TestHarness.h
gtsam::PoseRTV::RRTMbn
static Matrix RRTMbn(const Vector3 &euler)
Definition: PoseRTV.cpp:208
gtsam::Velocity3
Vector3 Velocity3
Velocity is currently typedef'd to Vector3.
Definition: NavState.h:28
x4
static string x4("x4")
gtsam::numericalDerivative11
internal::FixedSizeMatrix< Y, X >::type numericalDerivative11(std::function< Y(const X &)> h, const X &x, double delta=1e-5)
New-style numerical derivatives using manifold_traits.
Definition: numericalDerivative.h:110
kZero3
static const Vector3 kZero3
Definition: testPoseRTV.cpp:21
dt
const double dt
Definition: testVelocityConstraint.cpp:15
gtsam::Rot3::Yaw
static Rot3 Yaw(double t)
Positive yaw is to right (as in aircraft heading). See ypr.
Definition: Rot3.h:174
gtsam::PoseRTV::vector
Vector vector() const
Definition: PoseRTV.cpp:37
gtsam::numericalDerivative22
internal::FixedSizeMatrix< Y, X2 >::type numericalDerivative22(std::function< Y(const X1 &, const X2 &)> h, const X1 &x1, const X2 &x2, double delta=1e-5)
Definition: numericalDerivative.h:195
vel
static const Velocity3 vel(0.4, 0.5, 0.6)
pt
static const Point3 pt(1.0, 2.0, 3.0)
T
Eigen::Triplet< double > T
Definition: Tutorial_sparse_example.cpp:6
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
simple::pose2
static Pose3 pose2
Definition: testInitializePose3.cpp:58
gtsam::Vector3
Eigen::Vector3d Vector3
Definition: Vector.h:43
gtsam::NavState::v
const Vector3 & v() const
Return velocity as Vector3. Computation-free.
Definition: NavState.h:107
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
gtsam::Rot3::Ypr
static Rot3 Ypr(double y, double p, double r, OptionalJacobian< 3, 1 > Hy={}, OptionalJacobian< 3, 1 > Hp={}, OptionalJacobian< 3, 1 > Hr={})
Definition: Rot3.h:196
x3
Pose3 x3(Rot3::Ypr(M_PI/4.0, 0.0, 0.0), l2)
TestableAssertions.h
Provides additional testing facilities for common data structures.
A
Matrix< SCALARA, Dynamic, Dynamic, opt_A > A
Definition: bench_gemm.cpp:48
transformed_from_proxy
PoseRTV transformed_from_proxy(const PoseRTV &a, const Pose3 &trans)
Definition: testPoseRTV.cpp:188
gtsam::compose
Expression< T > compose(const Expression< T > &t1, const Expression< T > &t2)
Definition: nonlinear/expressions.h:23
gtsam::range
Double_ range(const Point2_ &p, const Point2_ &q)
Definition: slam/expressions.h:30
gtsam::PoseRTV::t
const Point3 & t() const
Definition: PoseRTV.h:58
numericalDerivative.h
Some functions to compute numerical derivatives.
compose_proxy
PoseRTV compose_proxy(const PoseRTV &A, const PoseRTV &B)
Definition: testPoseRTV.cpp:134
gtsam.examples.PlanarManipulatorExample.delta
def delta(g0, g1)
Definition: PlanarManipulatorExample.py:45
x1
Pose3 x1
Definition: testPose3.cpp:663
gtsam::Rot3
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
gtsam::Pose3
Definition: Pose3.h:37
gtsam::Rot3::Rodrigues
static Rot3 Rodrigues(const Vector3 &w)
Definition: Rot3.h:240
GTSAM_CONCEPT_LIE_INST
#define GTSAM_CONCEPT_LIE_INST(T)
Definition: Lie.h:372
gtsam::PoseRTV::v
const Velocity3 & v() const
Definition: PoseRTV.h:57
main
int main()
Definition: testPoseRTV.cpp:243
cholesky::expected
Matrix expected
Definition: testMatrix.cpp:971
gtsam::Pose3::translation
const Point3 & translation(OptionalJacobian< 3, 6 > Hself={}) const
get translation
Definition: Pose3.cpp:308
PoseRTV.h
Pose3 with translational velocity.
x0
static Symbol x0('x', 0)
gtsam::NavState::t
Vector3 t() const
Return position as Vector3.
Definition: NavState.h:103
gtsam::NavState::R
Matrix3 R() const
Return rotation matrix. Induces computation in quaternion mode.
Definition: NavState.h:95
transform
EIGEN_DONT_INLINE void transform(const Transformation &t, Data &data)
Definition: geometry.cpp:25
Eigen::Triplet< double >
common::state2
static const NavState state2(x2, v2)
gtsam::ProductLieGroup::expmap
ProductLieGroup expmap(const TangentVector &v) const
Definition: ProductLieGroup.h:167
range_proxy
double range_proxy(const PoseRTV &A, const PoseRTV &B)
Definition: testPoseRTV.cpp:171
gtsam::ProductLieGroup< Pose3, Velocity3 >::Dim
static size_t Dim()
Definition: ProductLieGroup.h:71
EXPECT_DOUBLES_EQUAL
#define EXPECT_DOUBLES_EQUAL(expected, actual, threshold)
Definition: Test.h:161
gtsam::equals
Definition: Testable.h:112
TestResult
Definition: TestResult.h:26
gtsam::PoseRTV
Definition: PoseRTV.h:23
M_PI_2
#define M_PI_2
Definition: mconf.h:118
rot
static const Rot3 rot
Definition: testPoseRTV.cpp:18
gtsam::Pose3::rotation
const Rot3 & rotation(OptionalJacobian< 3, 6 > Hself={}) const
get rotation
Definition: Pose3.cpp:315
gtsam::ProductLieGroup::retract
ProductLieGroup retract(const TangentVector &v, ChartJacobian H1={}, ChartJacobian H2={}) const
Definition: ProductLieGroup.h:77
a
ArrayXXi a
Definition: Array_initializer_list_23_cxx11.cpp:1
gtsam
traits
Definition: chartTesting.h:28
gtsam::ProductLieGroup::logmap
TangentVector logmap(const ProductLieGroup &g) const
Definition: ProductLieGroup.h:170
gtsam::TEST
TEST(SmartFactorBase, Pinhole)
Definition: testSmartFactorBase.cpp:38
gtsam::trans
Matrix trans(const Matrix &A)
Definition: base/Matrix.h:241
gtsam::numericalDerivative21
internal::FixedSizeMatrix< Y, X1 >::type numericalDerivative21(const std::function< Y(const X1 &, const X2 &)> &h, const X1 &x1, const X2 &x2, double delta=1e-5)
Definition: numericalDerivative.h:166
gtsam::PoseRTV::R
const Rot3 & R() const
Definition: PoseRTV.h:59
inverse_proxy
PoseRTV inverse_proxy(const PoseRTV &A)
Definition: testPoseRTV.cpp:160
between_proxy
PoseRTV between_proxy(const PoseRTV &A, const PoseRTV &B)
Definition: testPoseRTV.cpp:147
gtsam::PoseRTV::RRTMnb
static Matrix RRTMnb(const Vector3 &euler)
Definition: PoseRTV.cpp:225
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:40
gtsam::NavState::retract
NavState retract(const Vector9 &v, OptionalJacobian< 9, 9 > H1={}, OptionalJacobian< 9, 9 > H2={}) const
retract with optional derivatives
Definition: NavState.cpp:107
gtsam::Point3
Vector3 Point3
Definition: Point3.h:38
gtsam::LieGroup::retract
Class retract(const TangentVector &v) const
retract as required by manifold concept: applies v at *this
Definition: Lie.h:131
V
MatrixXcd V
Definition: EigenSolver_EigenSolver_MatrixType.cpp:15
gtsam::assert_inequal
bool assert_inequal(const Matrix &A, const Matrix &B, double tol)
Definition: Matrix.cpp:60
gtsam::PoseRTV::pose
const Pose3 & pose() const
Definition: PoseRTV.h:56
gtsam::NavState::localCoordinates
Vector9 localCoordinates(const NavState &g, OptionalJacobian< 9, 9 > H1={}, OptionalJacobian< 9, 9 > H2={}) const
localCoordinates with optional derivatives
Definition: NavState.cpp:134
common::state1
static const NavState state1(x1, v1)
gtsam::NavState::pose
const Pose3 pose() const
Definition: NavState.h:86
gtsam::PoseRTV::range
double range(const PoseRTV &other, OptionalJacobian< 1, 9 > H1={}, OptionalJacobian< 1, 9 > H2={}) const
Definition: PoseRTV.cpp:169
x2
Pose3 x2(Rot3::Ypr(0.0, 0.0, 0.0), l2)
R
Rot2 R(Rot2::fromAngle(0.1))

gtsam
Author(s):
autogenerated on Tue Jun 25 2024 03:06:28