testPartialPriorFactor.cpp
Go to the documentation of this file.
1 /* ----------------------------------------------------------------------------
2 
3  * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4  * Atlanta, Georgia 30332-0415
5  * All Rights Reserved
6  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7 
8  * See LICENSE for the license information
9 
10  * -------------------------------------------------------------------------- */
11 
13 #include <gtsam/inference/Symbol.h>
14 #include <gtsam/geometry/Pose2.h>
15 #include <gtsam/geometry/Pose3.h>
18 
20 
21 using namespace std::placeholders;
22 using namespace std;
23 using namespace gtsam;
24 
25 namespace NM = gtsam::noiseModel;
26 
27 // Pose3 tangent representation is [ Rx Ry Rz Tx Ty Tz ].
28 static const int kIndexRx = 0;
29 static const int kIndexRy = 1;
30 static const int kIndexRz = 2;
31 static const int kIndexTx = 3;
32 static const int kIndexTy = 4;
33 static const int kIndexTz = 5;
34 
37 typedef std::vector<size_t> Indices;
38 
40 namespace gtsam {
41 template<>
42 struct traits<TestPartialPriorFactor2> : public Testable<TestPartialPriorFactor2> {};
43 
44 template<>
45 struct traits<TestPartialPriorFactor3> : public Testable<TestPartialPriorFactor3> {};
46 }
47 
48 /* ************************************************************************* */
49 TEST(PartialPriorFactor, Constructors2) {
50  Key poseKey(1);
51  Pose2 measurement(-13.1, 3.14, -0.73);
52 
53  // Prior on x component of translation.
55  CHECK(assert_equal(1, factor1.prior().rows()));
56  CHECK(assert_equal(measurement.x(), factor1.prior()(0)));
57  CHECK(assert_container_equality<Indices>({ 0 }, factor1.indices()));
58 
59  // Prior on full translation vector.
60  const Indices t_indices = { 0, 1 };
61  TestPartialPriorFactor2 factor2(poseKey, t_indices, measurement.translation(), NM::Isotropic::Sigma(2, 0.25));
62  CHECK(assert_equal(2, factor2.prior().rows()));
63  CHECK(assert_equal(measurement.translation(), factor2.prior()));
64  CHECK(assert_container_equality<Indices>(t_indices, factor2.indices()));
65 
66  // Prior on theta.
68  CHECK(assert_equal(1, factor3.prior().rows()));
69  CHECK(assert_equal(measurement.theta(), factor3.prior()(0)));
70  CHECK(assert_container_equality<Indices>({ 2 }, factor3.indices()));
71 }
72 
73 /* ************************************************************************* */
74 TEST(PartialPriorFactor, JacobianPartialTranslation2) {
75  Key poseKey(1);
76  Pose2 measurement(-13.1, 3.14, -0.73);
77 
78  // Prior on x component of translation.
80 
81  Pose2 pose = measurement; // Zero-error linearization point.
82 
83  // Calculate numerical derivatives.
84  Matrix expectedH1 = numericalDerivative11<Vector, Pose2>(
85  [&factor](const Pose2& p) { return factor.evaluateError(p); }, pose);
86 
87  // Use the factor to calculate the derivative.
88  Matrix actualH1;
89  factor.evaluateError(pose, actualH1);
90 
91  // Verify we get the expected error.
92  CHECK(assert_equal(expectedH1, actualH1, 1e-5));
93 }
94 
95 /* ************************************************************************* */
96 TEST(PartialPriorFactor, JacobianFullTranslation2) {
97  Key poseKey(1);
98  Pose2 measurement(-6.0, 3.5, 0.123);
99 
100  // Prior on x component of translation.
101  TestPartialPriorFactor2 factor(poseKey, {0, 1}, measurement.translation(),
102  NM::Isotropic::Sigma(2, 0.25));
103 
104  Pose2 pose = measurement; // Zero-error linearization point.
105 
106  // Calculate numerical derivatives.
107  Matrix expectedH1 = numericalDerivative11<Vector, Pose2>(
108  [&factor](const Pose2& p) { return factor.evaluateError(p); }, pose);
109 
110  // Use the factor to calculate the derivative.
111  Matrix actualH1;
112  factor.evaluateError(pose, actualH1);
113 
114  // Verify we get the expected error.
115  CHECK(assert_equal(expectedH1, actualH1, 1e-5));
116 }
117 
118 /* ************************************************************************* */
119 TEST(PartialPriorFactor, JacobianTheta) {
120  Key poseKey(1);
121  Pose2 measurement(-1.0, 0.4, -2.5);
122 
123  // Prior on x component of translation.
124  TestPartialPriorFactor2 factor(poseKey, 2, measurement.theta(), NM::Isotropic::Sigma(1, 0.25));
125 
126  Pose2 pose = measurement; // Zero-error linearization point.
127 
128  // Calculate numerical derivatives.
129  Matrix expectedH1 = numericalDerivative11<Vector, Pose2>(
130  [&factor](const Pose2& p) { return factor.evaluateError(p); }, pose);
131 
132  // Use the factor to calculate the derivative.
133  Matrix actualH1;
134  factor.evaluateError(pose, actualH1);
135 
136  // Verify we get the expected error.
137  CHECK(assert_equal(expectedH1, actualH1, 1e-5));
138 }
139 
140 /* ************************************************************************* */
141 TEST(PartialPriorFactor, Constructors3) {
142  Key poseKey(1);
143  Pose3 measurement(Rot3::RzRyRx(-0.17, 0.567, M_PI), Point3(10.0, -2.3, 3.14));
144 
145  // Single component of translation.
147  NM::Isotropic::Sigma(1, 0.25));
148  CHECK(assert_equal(1, factor1.prior().rows()));
149  CHECK(assert_equal(measurement.y(), factor1.prior()(0)));
150  CHECK(assert_container_equality<Indices>({ kIndexTy }, factor1.indices()));
151 
152  // Full translation vector.
153  const Indices t_indices = { kIndexTx, kIndexTy, kIndexTz };
154  TestPartialPriorFactor3 factor2(poseKey, t_indices, measurement.translation(),
155  NM::Isotropic::Sigma(3, 0.25));
156  CHECK(assert_equal(3, factor2.prior().rows()));
157  CHECK(assert_equal(measurement.translation(), factor2.prior()));
158  CHECK(assert_container_equality<Indices>(t_indices, factor2.indices()));
159 
160  // Full tangent vector of rotation.
161  const Indices r_indices = { kIndexRx, kIndexRy, kIndexRz };
162  TestPartialPriorFactor3 factor3(poseKey, r_indices, Rot3::Logmap(measurement.rotation()),
163  NM::Isotropic::Sigma(3, 0.1));
164  CHECK(assert_equal(3, factor3.prior().rows()));
165  CHECK(assert_equal(Rot3::Logmap(measurement.rotation()), factor3.prior()));
166  CHECK(assert_container_equality<Indices>(r_indices, factor3.indices()));
167 }
168 
169 /* ************************************************************************* */
170 TEST(PartialPriorFactor, JacobianAtIdentity3) {
171  Key poseKey(1);
172  Pose3 measurement = Pose3::Identity();
174 
175  TestPartialPriorFactor3 factor(poseKey, kIndexTy, measurement.translation().y(), model);
176 
177  Pose3 pose = measurement; // Zero-error linearization point.
178 
179  // Calculate numerical derivatives.
180  Matrix expectedH1 = numericalDerivative11<Vector, Pose3>(
181  [&factor](const Pose3& p) { return factor.evaluateError(p); }, pose);
182 
183  // Use the factor to calculate the derivative.
184  Matrix actualH1;
185  factor.evaluateError(pose, actualH1);
186 
187  // Verify we get the expected error.
188  CHECK(assert_equal(expectedH1, actualH1, 1e-5));
189 }
190 
191 /* ************************************************************************* */
192 TEST(PartialPriorFactor, JacobianPartialTranslation3) {
193  Key poseKey(1);
194  Pose3 measurement(Rot3::RzRyRx(0.15, -0.30, 0.45), Point3(-5.0, 8.0, -11.0));
196 
197  TestPartialPriorFactor3 factor(poseKey, kIndexTy, measurement.translation().y(), model);
198 
199  Pose3 pose = measurement; // Zero-error linearization point.
200 
201  // Calculate numerical derivatives.
202  Matrix expectedH1 = numericalDerivative11<Vector, Pose3>(
203  [&factor](const Pose3& p) { return factor.evaluateError(p); }, pose);
204 
205  // Use the factor to calculate the derivative.
206  Matrix actualH1;
207  factor.evaluateError(pose, actualH1);
208 
209  // Verify we get the expected error.
210  CHECK(assert_equal(expectedH1, actualH1, 1e-5));
211 }
212 
213 /* ************************************************************************* */
214 TEST(PartialPriorFactor, JacobianFullTranslation3) {
215  Key poseKey(1);
216  Pose3 measurement(Rot3::RzRyRx(0.15, -0.30, 0.45), Point3(-5.0, 8.0, -11.0));
218 
219  std::vector<size_t> translationIndices = { kIndexTx, kIndexTy, kIndexTz };
220  TestPartialPriorFactor3 factor(poseKey, translationIndices, measurement.translation(), model);
221 
222  // Create a linearization point at the zero-error point
223  Pose3 pose = measurement; // Zero-error linearization point.
224 
225  // Calculate numerical derivatives.
226  Matrix expectedH1 = numericalDerivative11<Vector, Pose3>(
227  [&factor](const Pose3& p) { return factor.evaluateError(p); }, pose);
228 
229  // Use the factor to calculate the derivative.
230  Matrix actualH1;
231  factor.evaluateError(pose, actualH1);
232 
233  // Verify we get the expected error.
234  CHECK(assert_equal(expectedH1, actualH1, 1e-5));
235 }
236 
237 /* ************************************************************************* */
238 TEST(PartialPriorFactor, JacobianTxTz3) {
239  Key poseKey(1);
240  Pose3 measurement(Rot3::RzRyRx(-0.17, 0.567, M_PI), Point3(10.0, -2.3, 3.14));
242 
243  std::vector<size_t> translationIndices = { kIndexTx, kIndexTz };
244  TestPartialPriorFactor3 factor(poseKey, translationIndices,
245  (Vector(2) << measurement.x(), measurement.z()).finished(), model);
246 
247  Pose3 pose = measurement; // Zero-error linearization point.
248 
249  // Calculate numerical derivatives.
250  Matrix expectedH1 = numericalDerivative11<Vector, Pose3>(
251  [&factor](const Pose3& p) { return factor.evaluateError(p); }, pose);
252 
253  // Use the factor to calculate the derivative.
254  Matrix actualH1;
255  factor.evaluateError(pose, actualH1);
256 
257  // Verify we get the expected error.
258  CHECK(assert_equal(expectedH1, actualH1, 1e-5));
259 }
260 
261 /* ************************************************************************* */
262 TEST(PartialPriorFactor, JacobianFullRotation3) {
263  Key poseKey(1);
264  Pose3 measurement(Rot3::RzRyRx(0.15, -0.30, 0.45), Point3(-5.0, 8.0, -11.0));
266 
267  std::vector<size_t> rotationIndices = { kIndexRx, kIndexRy, kIndexRz };
268  TestPartialPriorFactor3 factor(poseKey, rotationIndices, Rot3::Logmap(measurement.rotation()), model);
269 
270  Pose3 pose = measurement; // Zero-error linearization point.
271 
272  // Calculate numerical derivatives.
273  Matrix expectedH1 = numericalDerivative11<Vector, Pose3>(
274  [&factor](const Pose3& p) { return factor.evaluateError(p); }, pose);
275 
276  // Use the factor to calculate the derivative.
277  Matrix actualH1;
278  factor.evaluateError(pose, actualH1);
279 
280  // Verify we get the expected error.
281  CHECK(assert_equal(expectedH1, actualH1, 1e-5));
282 }
283 
284 /* ************************************************************************* */
285 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
286 /* ************************************************************************* */
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
PartialPriorFactor.h
A simple prior factor that allows for setting a prior only on a part of linear parameters.
Pose2.h
2D Pose
pose
static const Pose3 pose(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5))
simple_graph::factor2
auto factor2
Definition: testJacobianFactor.cpp:203
kIndexRz
static const int kIndexRz
Definition: testPartialPriorFactor.cpp:30
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
TestHarness.h
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
TestPartialPriorFactor2
PartialPriorFactor< Pose2 > TestPartialPriorFactor2
Definition: testPartialPriorFactor.cpp:35
poseKey
const gtsam::Key poseKey
Definition: testPoseRotationPrior.cpp:29
TestableAssertions.h
Provides additional testing facilities for common data structures.
TestPartialPriorFactor3
PartialPriorFactor< Pose3 > TestPartialPriorFactor3
Definition: testPartialPriorFactor.cpp:36
numericalDerivative.h
Some functions to compute numerical derivatives.
kIndexTz
static const int kIndexTz
Definition: testPartialPriorFactor.cpp:33
kIndexTy
static const int kIndexTy
Definition: testPartialPriorFactor.cpp:32
gtsam::Pose3
Definition: Pose3.h:37
Symbol.h
kIndexRy
static const int kIndexRy
Definition: testPartialPriorFactor.cpp:29
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:762
model
noiseModel::Diagonal::shared_ptr model
Definition: doc/Code/Pose2SLAMExample.cpp:7
TestResult
Definition: TestResult.h:26
simple_graph::factor3
auto factor3
Definition: testJacobianFactor.cpp:210
main
int main()
Definition: testPartialPriorFactor.cpp:285
gtsam
traits
Definition: SFMdata.h:40
gtsam::Testable
Definition: Testable.h:152
gtsam::traits
Definition: Group.h:36
gtsam::PartialPriorFactor::evaluateError
Vector evaluateError(const T &p, OptionalMatrixType H) const override
Definition: PartialPriorFactor.h:112
CHECK
#define CHECK(condition)
Definition: Test.h:108
std
Definition: BFloat16.h:88
gtsam::noiseModel
All noise models live in the noiseModel namespace.
Definition: LossFunctions.cpp:28
p
float * p
Definition: Tutorial_Map_using.cpp:9
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:40
gtsam::Point3
Vector3 Point3
Definition: Point3.h:38
gtsam::noiseModel::Isotropic::Sigma
static shared_ptr Sigma(size_t dim, double sigma, bool smart=true)
Definition: NoiseModel.cpp:624
M_PI
#define M_PI
Definition: mconf.h:117
Indices
std::vector< size_t > Indices
Definition: testPartialPriorFactor.cpp:37
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97
kIndexTx
static const int kIndexTx
Definition: testPartialPriorFactor.cpp:31
ceres::Vector
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
Definition: gtsam/3rdparty/ceres/eigen.h:38
simple_graph::factor1
auto factor1
Definition: testJacobianFactor.cpp:196
measurement
static Point2 measurement(323.0, 240.0)
Pose3.h
3D Pose
gtsam::PartialPriorFactor
Definition: PartialPriorFactor.h:38
gtsam::Pose2
Definition: Pose2.h:39
kIndexRx
static const int kIndexRx
Definition: testPartialPriorFactor.cpp:28
TEST
TEST(PartialPriorFactor, Constructors2)
Definition: testPartialPriorFactor.cpp:49


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:40:53