testMultiProjectionFactor.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 
19 #include <gtsam/slam/BetweenFactor.h>
20 #include <gtsam/slam/ProjectionFactor.h>
21 #include <gtsam_unstable/slam/MultiProjectionFactor.h>
22 #include <gtsam/nonlinear/ISAM2.h>
23 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
24 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
25 #include <gtsam/nonlinear/LinearContainerFactor.h>
26 #include <gtsam/inference/Ordering.h>
27 #include <gtsam/nonlinear/Values.h>
28 #include <gtsam/inference/Symbol.h>
29 #include <gtsam/inference/Key.h>
30 #include <gtsam/inference/JunctionTree.h>
31 #include <gtsam/geometry/Pose3.h>
32 #include <gtsam/geometry/Point3.h>
33 #include <gtsam/geometry/Point2.h>
34 #include <gtsam/geometry/Cal3DS2.h>
35 #include <gtsam/geometry/Cal3_S2.h>
36 #include <CppUnitLite/TestHarness.h>
37 
38 
39 using namespace std;
40 using namespace gtsam;
41 
42 // make a realistic calibration matrix
43 static double fov = 60; // degrees
44 static int w=640,h=480;
45 static Cal3_S2::shared_ptr K(new Cal3_S2(fov,w,h));
46 
47 // Create a noise model for the pixel error
48 static SharedNoiseModel model(noiseModel::Unit::Create(2));
49 
50 // Convenience for named keys
51 //using symbol_shorthand::X;
52 //using symbol_shorthand::L;
53 
54 //typedef GenericProjectionFactor<Pose3, Point3> TestProjectionFactor;
55 
56 
58 TEST( MultiProjectionFactor, create ){
59  Values theta;
60  NonlinearFactorGraph graph;
61 
62  Symbol x1('X', 1);
63  Symbol x2('X', 2);
64  Symbol x3('X', 3);
65 
66  Symbol l1('l', 1);
67  Vector n_measPixel(6); // Pixel measurements from 3 cameras observing landmark 1
68  n_measPixel << 10, 10, 10, 10, 10, 10;
69  const SharedDiagonal noiseProjection = noiseModel::Isotropic::Sigma(2, 1);
70 
71  KeySet views;
72  views.insert(x1);
73  views.insert(x2);
74  views.insert(x3);
75 
76  graph.emplace_shared<MultiProjectionFactor<Pose3, Point3>>(
77  n_measPixel, noiseProjection, views, l1, K);
78 }
79 
80 
81 
83 //TEST( ProjectionFactor, nonStandard ) {
84 // GenericProjectionFactor<Pose3, Point3, Cal3DS2> f;
85 //}
86 //
88 //TEST( ProjectionFactor, Constructor) {
89 // Key poseKey(X(1));
90 // Key pointKey(L(1));
91 //
92 // Point2 measurement(323.0, 240.0);
93 //
94 // TestProjectionFactor factor(measurement, model, poseKey, pointKey, K);
95 //}
96 //
98 //TEST( ProjectionFactor, ConstructorWithTransform) {
99 // Key poseKey(X(1));
100 // Key pointKey(L(1));
101 //
102 // Point2 measurement(323.0, 240.0);
103 // Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0));
104 //
105 // TestProjectionFactor factor(measurement, model, poseKey, pointKey, K, body_P_sensor);
106 //}
107 //
109 //TEST( ProjectionFactor, Equals ) {
110 // // Create two identical factors and make sure they're equal
111 // Point2 measurement(323.0, 240.0);
112 //
113 // TestProjectionFactor factor1(measurement, model, X(1), L(1), K);
114 // TestProjectionFactor factor2(measurement, model, X(1), L(1), K);
115 //
116 // CHECK(assert_equal(factor1, factor2));
117 //}
118 //
120 //TEST( ProjectionFactor, EqualsWithTransform ) {
121 // // Create two identical factors and make sure they're equal
122 // Point2 measurement(323.0, 240.0);
123 // Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0));
124 //
125 // TestProjectionFactor factor1(measurement, model, X(1), L(1), K, body_P_sensor);
126 // TestProjectionFactor factor2(measurement, model, X(1), L(1), K, body_P_sensor);
127 //
128 // CHECK(assert_equal(factor1, factor2));
129 //}
130 //
132 //TEST( ProjectionFactor, Error ) {
133 // // Create the factor with a measurement that is 3 pixels off in x
134 // Key poseKey(X(1));
135 // Key pointKey(L(1));
136 // Point2 measurement(323.0, 240.0);
137 // TestProjectionFactor factor(measurement, model, poseKey, pointKey, K);
138 //
139 // // Set the linearization point
140 // Pose3 pose(Rot3(), Point3(0,0,-6));
141 // Point3 point(0.0, 0.0, 0.0);
142 //
143 // // Use the factor to calculate the error
144 // Vector actualError(factor.evaluateError(pose, point));
145 //
146 // // The expected error is (-3.0, 0.0) pixels / UnitCovariance
147 // Vector expectedError = Vector2(-3.0, 0.0);
148 //
149 // // Verify we get the expected error
150 // CHECK(assert_equal(expectedError, actualError, 1e-9));
151 //}
152 //
154 //TEST( ProjectionFactor, ErrorWithTransform ) {
155 // // Create the factor with a measurement that is 3 pixels off in x
156 // Key poseKey(X(1));
157 // Key pointKey(L(1));
158 // Point2 measurement(323.0, 240.0);
159 // Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0));
160 // TestProjectionFactor factor(measurement, model, poseKey, pointKey, K, body_P_sensor);
161 //
162 // // Set the linearization point. The vehicle pose has been selected to put the camera at (-6, 0, 0)
163 // Pose3 pose(Rot3(), Point3(-6.25, 0.10 , -1.0));
164 // Point3 point(0.0, 0.0, 0.0);
165 //
166 // // Use the factor to calculate the error
167 // Vector actualError(factor.evaluateError(pose, point));
168 //
169 // // The expected error is (-3.0, 0.0) pixels / UnitCovariance
170 // Vector expectedError = Vector2(-3.0, 0.0);
171 //
172 // // Verify we get the expected error
173 // CHECK(assert_equal(expectedError, actualError, 1e-9));
174 //}
175 //
177 //TEST( ProjectionFactor, Jacobian ) {
178 // // Create the factor with a measurement that is 3 pixels off in x
179 // Key poseKey(X(1));
180 // Key pointKey(L(1));
181 // Point2 measurement(323.0, 240.0);
182 // TestProjectionFactor factor(measurement, model, poseKey, pointKey, K);
183 //
184 // // Set the linearization point
185 // Pose3 pose(Rot3(), Point3(0,0,-6));
186 // Point3 point(0.0, 0.0, 0.0);
187 //
188 // // Use the factor to calculate the Jacobians
189 // Matrix H1Actual, H2Actual;
190 // factor.evaluateError(pose, point, H1Actual, H2Actual);
191 //
192 // // The expected Jacobians
193 // Matrix H1Expected = (Matrix(2, 6) << 0., -554.256, 0., -92.376, 0., 0., 554.256, 0., 0., 0., -92.376, 0.).finished();
194 // Matrix H2Expected = (Matrix(2, 3) << 92.376, 0., 0., 0., 92.376, 0.).finished();
195 //
196 // // Verify the Jacobians are correct
197 // CHECK(assert_equal(H1Expected, H1Actual, 1e-3));
198 // CHECK(assert_equal(H2Expected, H2Actual, 1e-3));
199 //}
200 //
202 //TEST( ProjectionFactor, JacobianWithTransform ) {
203 // // Create the factor with a measurement that is 3 pixels off in x
204 // Key poseKey(X(1));
205 // Key pointKey(L(1));
206 // Point2 measurement(323.0, 240.0);
207 // Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0));
208 // TestProjectionFactor factor(measurement, model, poseKey, pointKey, K, body_P_sensor);
209 //
210 // // Set the linearization point. The vehicle pose has been selected to put the camera at (-6, 0, 0)
211 // Pose3 pose(Rot3(), Point3(-6.25, 0.10 , -1.0));
212 // Point3 point(0.0, 0.0, 0.0);
213 //
214 // // Use the factor to calculate the Jacobians
215 // Matrix H1Actual, H2Actual;
216 // factor.evaluateError(pose, point, H1Actual, H2Actual);
217 //
218 // // The expected Jacobians
219 // Matrix H1Expected = (Matrix(2, 6) << -92.376, 0., 577.350, 0., 92.376, 0., -9.2376, -577.350, 0., 0., 0., 92.376).finished();
220 // Matrix H2Expected = (Matrix(2, 3) << 0., -92.376, 0., 0., 0., -92.376).finished();
221 //
222 // // Verify the Jacobians are correct
223 // CHECK(assert_equal(H1Expected, H1Actual, 1e-3));
224 // CHECK(assert_equal(H2Expected, H2Actual, 1e-3));
225 //}
226 
227 /* ************************************************************************* */
228 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
229 /* ************************************************************************* */
230 
gtsam::FactorGraph::emplace_shared
IsDerived< DERIVEDFACTOR > emplace_shared(Args &&... args)
Emplace a shared pointer to factor of given type.
Definition: FactorGraph.h:196
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
Values.h
A non-templated config holding any types of Manifold-group elements.
LinearContainerFactor.h
Wrap Jacobian and Hessian linear factors to allow simple injection into a nonlinear graph...
NonlinearFactorGraph.h
Factor Graph consisting of non-linear factors.
w
static int w
Definition: testMultiProjectionFactor.cpp:44
std
Definition: BFloat16.h:88
Cal3DS2.h
Calibration of a camera with radial distortion, calculations in base class Cal3DS2_Base.
x2
Pose3 x2(Rot3::Ypr(0.0, 0.0, 0.0), l2)
graph
NonlinearFactorGraph graph
Definition: doc/Code/OdometryExample.cpp:2
fov
static double fov
Definition: testMultiProjectionFactor.cpp:43
K
static Cal3_S2::shared_ptr K(new Cal3_S2(fov, w, h))
h
static int h
Definition: testMultiProjectionFactor.cpp:44
gtsam::Cal3_S2::shared_ptr
std::shared_ptr< Cal3_S2 > shared_ptr
Definition: Cal3_S2.h:39
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
ISAM2.h
Incremental update functionality (ISAM2) for BayesTree, with fluid relinearization.
LevenbergMarquardtOptimizer.h
A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme.
gtsam::Cal3_S2
The most common 5DOF 3D->2D calibration.
Definition: Cal3_S2.h:34
ProjectionFactor.h
Reprojection of a LANDMARK to a 2D point.
x3
Pose3 x3(Rot3::Ypr(M_PI/4.0, 0.0, 0.0), l2)
l1
gtsam::Key l1
Definition: testLinearContainerFactor.cpp:24
gtsam::SharedDiagonal
noiseModel::Diagonal::shared_ptr SharedDiagonal
Definition: NoiseModel.h:743
gtsam
traits
Definition: chartTesting.h:28
JunctionTree.h
The junction tree.
create
ADT create(const Signature &signature)
Definition: testAlgebraicDecisionTree.cpp:130
model
static SharedNoiseModel model(noiseModel::Unit::Create(2))
x1
Pose3 x1
Definition: testPose3.cpp:663
Point3.h
3D Point
Point2.h
2D Point
Pose3.h
3D Pose
TEST
TEST(MultiProjectionFactor, create)
Definition: testMultiProjectionFactor.cpp:58
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:741
Cal3_S2.h
The most common 5DOF 3D->2D calibration.

gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:38:46