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 
22 #include <gtsam/nonlinear/ISAM2.h>
27 #include <gtsam/nonlinear/Values.h>
28 #include <gtsam/inference/Symbol.h>
29 #include <gtsam/inference/Key.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>
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 
59  Values theta;
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  MultiProjectionFactor<Pose3, Point3> mpFactor(n_measPixel, noiseProjection, views, l1, K);
77  graph += mpFactor;
78 
79 
80 }
81 
82 
83 
85 //TEST( ProjectionFactor, nonStandard ) {
86 // GenericProjectionFactor<Pose3, Point3, Cal3DS2> f;
87 //}
88 //
90 //TEST( ProjectionFactor, Constructor) {
91 // Key poseKey(X(1));
92 // Key pointKey(L(1));
93 //
94 // Point2 measurement(323.0, 240.0);
95 //
96 // TestProjectionFactor factor(measurement, model, poseKey, pointKey, K);
97 //}
98 //
100 //TEST( ProjectionFactor, ConstructorWithTransform) {
101 // Key poseKey(X(1));
102 // Key pointKey(L(1));
103 //
104 // Point2 measurement(323.0, 240.0);
105 // Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0));
106 //
107 // TestProjectionFactor factor(measurement, model, poseKey, pointKey, K, body_P_sensor);
108 //}
109 //
111 //TEST( ProjectionFactor, Equals ) {
112 // // Create two identical factors and make sure they're equal
113 // Point2 measurement(323.0, 240.0);
114 //
115 // TestProjectionFactor factor1(measurement, model, X(1), L(1), K);
116 // TestProjectionFactor factor2(measurement, model, X(1), L(1), K);
117 //
118 // CHECK(assert_equal(factor1, factor2));
119 //}
120 //
122 //TEST( ProjectionFactor, EqualsWithTransform ) {
123 // // Create two identical factors and make sure they're equal
124 // Point2 measurement(323.0, 240.0);
125 // Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0));
126 //
127 // TestProjectionFactor factor1(measurement, model, X(1), L(1), K, body_P_sensor);
128 // TestProjectionFactor factor2(measurement, model, X(1), L(1), K, body_P_sensor);
129 //
130 // CHECK(assert_equal(factor1, factor2));
131 //}
132 //
134 //TEST( ProjectionFactor, Error ) {
135 // // Create the factor with a measurement that is 3 pixels off in x
136 // Key poseKey(X(1));
137 // Key pointKey(L(1));
138 // Point2 measurement(323.0, 240.0);
139 // TestProjectionFactor factor(measurement, model, poseKey, pointKey, K);
140 //
141 // // Set the linearization point
142 // Pose3 pose(Rot3(), Point3(0,0,-6));
143 // Point3 point(0.0, 0.0, 0.0);
144 //
145 // // Use the factor to calculate the error
146 // Vector actualError(factor.evaluateError(pose, point));
147 //
148 // // The expected error is (-3.0, 0.0) pixels / UnitCovariance
149 // Vector expectedError = Vector2(-3.0, 0.0);
150 //
151 // // Verify we get the expected error
152 // CHECK(assert_equal(expectedError, actualError, 1e-9));
153 //}
154 //
156 //TEST( ProjectionFactor, ErrorWithTransform ) {
157 // // Create the factor with a measurement that is 3 pixels off in x
158 // Key poseKey(X(1));
159 // Key pointKey(L(1));
160 // Point2 measurement(323.0, 240.0);
161 // Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0));
162 // TestProjectionFactor factor(measurement, model, poseKey, pointKey, K, body_P_sensor);
163 //
164 // // Set the linearization point. The vehicle pose has been selected to put the camera at (-6, 0, 0)
165 // Pose3 pose(Rot3(), Point3(-6.25, 0.10 , -1.0));
166 // Point3 point(0.0, 0.0, 0.0);
167 //
168 // // Use the factor to calculate the error
169 // Vector actualError(factor.evaluateError(pose, point));
170 //
171 // // The expected error is (-3.0, 0.0) pixels / UnitCovariance
172 // Vector expectedError = Vector2(-3.0, 0.0);
173 //
174 // // Verify we get the expected error
175 // CHECK(assert_equal(expectedError, actualError, 1e-9));
176 //}
177 //
179 //TEST( ProjectionFactor, Jacobian ) {
180 // // Create the factor with a measurement that is 3 pixels off in x
181 // Key poseKey(X(1));
182 // Key pointKey(L(1));
183 // Point2 measurement(323.0, 240.0);
184 // TestProjectionFactor factor(measurement, model, poseKey, pointKey, K);
185 //
186 // // Set the linearization point
187 // Pose3 pose(Rot3(), Point3(0,0,-6));
188 // Point3 point(0.0, 0.0, 0.0);
189 //
190 // // Use the factor to calculate the Jacobians
191 // Matrix H1Actual, H2Actual;
192 // factor.evaluateError(pose, point, H1Actual, H2Actual);
193 //
194 // // The expected Jacobians
195 // Matrix H1Expected = (Matrix(2, 6) << 0., -554.256, 0., -92.376, 0., 0., 554.256, 0., 0., 0., -92.376, 0.).finished();
196 // Matrix H2Expected = (Matrix(2, 3) << 92.376, 0., 0., 0., 92.376, 0.).finished();
197 //
198 // // Verify the Jacobians are correct
199 // CHECK(assert_equal(H1Expected, H1Actual, 1e-3));
200 // CHECK(assert_equal(H2Expected, H2Actual, 1e-3));
201 //}
202 //
204 //TEST( ProjectionFactor, JacobianWithTransform ) {
205 // // Create the factor with a measurement that is 3 pixels off in x
206 // Key poseKey(X(1));
207 // Key pointKey(L(1));
208 // Point2 measurement(323.0, 240.0);
209 // Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0));
210 // TestProjectionFactor factor(measurement, model, poseKey, pointKey, K, body_P_sensor);
211 //
212 // // Set the linearization point. The vehicle pose has been selected to put the camera at (-6, 0, 0)
213 // Pose3 pose(Rot3(), Point3(-6.25, 0.10 , -1.0));
214 // Point3 point(0.0, 0.0, 0.0);
215 //
216 // // Use the factor to calculate the Jacobians
217 // Matrix H1Actual, H2Actual;
218 // factor.evaluateError(pose, point, H1Actual, H2Actual);
219 //
220 // // The expected Jacobians
221 // Matrix H1Expected = (Matrix(2, 6) << -92.376, 0., 577.350, 0., 92.376, 0., -9.2376, -577.350, 0., 0., 0., 92.376).finished();
222 // Matrix H2Expected = (Matrix(2, 3) << 0., -92.376, 0., 0., 0., -92.376).finished();
223 //
224 // // Verify the Jacobians are correct
225 // CHECK(assert_equal(H1Expected, H1Actual, 1e-3));
226 // CHECK(assert_equal(H2Expected, H2Actual, 1e-3));
227 //}
228 
229 /* ************************************************************************* */
230 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
231 /* ************************************************************************* */
232 
static int runAllTests(TestResult &result)
A non-templated config holding any types of Manifold-group elements.
Wrap Jacobian and Hessian linear factors to allow simple injection into a nonlinear graph...
Factor Graph consisting of non-linear factors.
static int w
Definition: Half.h:150
Calibration of a camera with radial distortion, calculations in base class Cal3DS2_Base.
Pose3 x2(Rot3::Ypr(0.0, 0.0, 0.0), l2)
Rot2 theta
NonlinearFactorGraph graph
static double fov
static Cal3_S2::shared_ptr K(new Cal3_S2(fov, w, h))
static int h
Eigen::VectorXd Vector
Definition: Vector.h:38
Incremental update functionality (ISAM2) for BayesTree, with fluid relinearization.
A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme.
Basic bearing factor from 2D measurement.
Pose3 x3(Rot3::Ypr(M_PI/4.0, 0.0, 0.0), l2)
gtsam::Key l1
noiseModel::Diagonal::shared_ptr SharedDiagonal
Definition: NoiseModel.h:736
traits
Definition: chartTesting.h:28
The junction tree.
ADT create(const Signature &signature)
static SharedNoiseModel model(noiseModel::Unit::Create(2))
Pose3 x1
Definition: testPose3.cpp:588
3D Point
2D Point
3D Pose
TEST(MultiProjectionFactor, create)
boost::shared_ptr< Cal3_S2 > shared_ptr
Definition: Cal3_S2.h:39
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:734
The most common 5DOF 3D->2D calibration.


gtsam
Author(s):
autogenerated on Sat May 8 2021 02:48:08