testStereoFactor.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 
18 #include <gtsam/slam/StereoFactor.h>
19 #include <gtsam/nonlinear/NonlinearEquality.h>
20 #include <gtsam/nonlinear/GaussNewtonOptimizer.h>
21 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
22 #include <gtsam/nonlinear/Values.h>
23 #include <gtsam/inference/Symbol.h>
24 #include <gtsam/geometry/StereoCamera.h>
25 #include <gtsam/geometry/Cal3_S2Stereo.h>
26 #include <gtsam/geometry/Pose3.h>
27 #include <gtsam/geometry/Point3.h>
28 
29 #include <CppUnitLite/TestHarness.h>
30 
31 using namespace std;
32 using namespace gtsam;
33 
34 static Pose3 camera1(Rot3(Vector3(1, -1, -1).asDiagonal()),
35  Point3(0,0,6.25));
36 
37 static boost::shared_ptr<Cal3_S2Stereo> K(new Cal3_S2Stereo(625, 625, 0, 320, 240, 0.5));
38 
39 // point X Y Z in meters
40 static Point3 p(0, 0, 5);
41 static SharedNoiseModel model(noiseModel::Unit::Create(3));
42 
43 // Convenience for named keys
44 using symbol_shorthand::X;
45 using symbol_shorthand::L;
46 
47 typedef GenericStereoFactor<Pose3, Point3> TestStereoFactor;
48 
49 /* ************************************************************************* */
50 TEST( StereoFactor, Constructor) {
51  StereoPoint2 measurement(323, 318-50, 241);
52 
53  TestStereoFactor factor(measurement, model, X(1), L(1), K);
54 }
55 
56 /* ************************************************************************* */
57 TEST( StereoFactor, ConstructorWithTransform) {
58  StereoPoint2 measurement(323, 318-50, 241);
59  Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0));
60 
61  TestStereoFactor factor(measurement, model, X(1), L(1), K, body_P_sensor);
62 }
63 
64 /* ************************************************************************* */
65 TEST( StereoFactor, Equals ) {
66  // Create two identical factors and make sure they're equal
67  StereoPoint2 measurement(323, 318-50, 241);
68 
69  TestStereoFactor factor1(measurement, model, X(1), L(1), K);
70  TestStereoFactor factor2(measurement, model, X(1), L(1), K);
71 
72  CHECK(assert_equal(factor1, factor2));
73 }
74 
75 /* ************************************************************************* */
76 TEST( StereoFactor, EqualsWithTransform ) {
77  // Create two identical factors and make sure they're equal
78  StereoPoint2 measurement(323, 318-50, 241);
79  Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0));
80 
81  TestStereoFactor factor1(measurement, model, X(1), L(1), K, body_P_sensor);
82  TestStereoFactor factor2(measurement, model, X(1), L(1), K, body_P_sensor);
83 
84  CHECK(assert_equal(factor1, factor2));
85 }
86 
87 /* ************************************************************************* */
88 TEST( StereoFactor, Error ) {
89  // Create the factor with a measurement that is 3 pixels off in x
90  StereoPoint2 measurement(323, 318-50, 241);
91  TestStereoFactor factor(measurement, model, X(1), L(1), K);
92 
93  // Set the linearization point
94  Pose3 pose(Rot3(), Point3(0.0, 0.0, -6.25));
95  Point3 point(0.0, 0.0, 0.0);
96 
97  // Use the factor to calculate the error
98  Vector actualError(factor.evaluateError(pose, point));
99 
100  // The expected error is (-3.0, +2.0, -1.0) pixels / UnitCovariance
101  Vector expectedError = Vector3(-3.0, +2.0, -1.0);
102 
103  // Verify we get the expected error
104  CHECK(assert_equal(expectedError, actualError, 1e-9));
105 }
106 
107 /* ************************************************************************* */
108 TEST( StereoFactor, ErrorWithTransform ) {
109  // Create the factor with a measurement that is 3 pixels off in x
110  StereoPoint2 measurement(323, 318-50, 241);
111  Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0));
112  TestStereoFactor factor(measurement, model, X(1), L(1), K, body_P_sensor);
113 
114  // Set the linearization point. The vehicle pose has been selected to put the camera at (-6, 0, 0)
115  Pose3 pose(Rot3(), Point3(-6.50, 0.10 , -1.0));
116  Point3 point(0.0, 0.0, 0.0);
117 
118  // Use the factor to calculate the error
119  Vector actualError(factor.evaluateError(pose, point));
120 
121  // The expected error is (-3.0, +2.0, -1.0) pixels / UnitCovariance
122  Vector expectedError = Vector3(-3.0, +2.0, -1.0);
123 
124  // Verify we get the expected error
125  CHECK(assert_equal(expectedError, actualError, 1e-9));
126 }
127 
128 /* ************************************************************************* */
129 TEST( StereoFactor, Jacobian ) {
130  // Create the factor with a measurement that is 3 pixels off in x
131  StereoPoint2 measurement(323, 318-50, 241);
132  TestStereoFactor factor(measurement, model, X(1), L(1), K);
133 
134  // Set the linearization point
135  Pose3 pose(Rot3(), Point3(0.0, 0.0, -6.25));
136  Point3 point(0.0, 0.0, 0.0);
137 
138  // Use the factor to calculate the Jacobians
139  Matrix H1Actual, H2Actual;
140  factor.evaluateError(pose, point, H1Actual, H2Actual);
141 
142  // The expected Jacobians
143  Matrix H1Expected = (Matrix(3, 6) << 0.0, -625.0, 0.0, -100.0, 0.0, 0.0,
144  0.0, -625.0, 0.0, -100.0, 0.0, -8.0,
145  625.0, 0.0, 0.0, 0.0, -100.0, 0.0).finished();
146  Matrix H2Expected = (Matrix(3, 3) << 100.0, 0.0, 0.0,
147  100.0, 0.0, 8.0,
148  0.0, 100.0, 0.0).finished();
149 
150  // Verify the Jacobians are correct
151  CHECK(assert_equal(H1Expected, H1Actual, 1e-3));
152  CHECK(assert_equal(H2Expected, H2Actual, 1e-3));
153 }
154 
155 /* ************************************************************************* */
156 TEST( StereoFactor, JacobianWithTransform ) {
157  // Create the factor with a measurement that is 3 pixels off in x
158  StereoPoint2 measurement(323, 318-50, 241);
159  Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0));
160  TestStereoFactor factor(measurement, model, X(1), L(1), 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.50, 0.10 , -1.0));
164  Point3 point(0.0, 0.0, 0.0);
165 
166  // Use the factor to calculate the Jacobians
167  Matrix H1Actual, H2Actual;
168  factor.evaluateError(pose, point, H1Actual, H2Actual);
169 
170  // The expected Jacobians
171  Matrix H1Expected = (Matrix(3, 6) << -100.0, 0.0, 650.0, 0.0, 100.0, 0.0,
172  -100.0, -8.0, 649.2, -8.0, 100.0, 0.0,
173  -10.0, -650.0, 0.0, 0.0, 0.0, 100.0).finished();
174  Matrix H2Expected = (Matrix(3, 3) << 0.0, -100.0, 0.0,
175  8.0, -100.0, 0.0,
176  0.0, 0.0, -100.0).finished();
177 
178  // Verify the Jacobians are correct
179  CHECK(assert_equal(H1Expected, H1Actual, 1e-3));
180  CHECK(assert_equal(H2Expected, H2Actual, 1e-3));
181 }
182 
183 /* ************************************************************************* */
184 TEST( StereoFactor, singlePoint)
185 {
186  NonlinearFactorGraph graph;
187 
188  graph.emplace_shared<NonlinearEquality<Pose3> >(X(1), camera1);
189 
190  StereoPoint2 measurement(320, 320.0-50, 240);
191  // arguments: measurement, sigma, cam#, measurement #, K, baseline (m)
192  graph.emplace_shared<GenericStereoFactor<Pose3, Point3> >(measurement, model, X(1), L(1), K);
193 
194  // Create a configuration corresponding to the ground truth
195  Values values;
196  values.insert(X(1), camera1); // add camera at z=6.25m looking towards origin
197 
198  Point3 l1(0, 0, 0);
199  values.insert(L(1), l1); // add point at origin;
200 
201  GaussNewtonOptimizer optimizer(graph, values);
202 
203  // We expect the initial to be zero because config is the ground truth
204  DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9);
205 
206  // Iterate once, and the config should not have changed
207  optimizer.iterate();
208  DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9);
209 
210  // Complete solution
211  optimizer.optimize();
212 
213  DOUBLES_EQUAL(0.0, optimizer.error(), 1e-6);
214 }
215 
216 /* ************************************************************************* */
217  int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
218 /* ************************************************************************* */
CHECK
#define CHECK(condition)
Definition: Test.h:109
gtsam::NonlinearOptimizer::optimize
virtual const Values & optimize()
Definition: NonlinearOptimizer.h:98
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
gtsam::Vector3
Eigen::Vector3d Vector3
Definition: Vector.h:43
Values.h
A non-templated config holding any types of Manifold-group elements.
NonlinearFactorGraph.h
Factor Graph consisting of non-linear factors.
DOUBLES_EQUAL
#define DOUBLES_EQUAL(expected, actual, threshold)
Definition: Test.h:142
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:43
leaf::values
leaf::MyValues values
L
MatrixXd L
Definition: LLT_example.cpp:6
measurement
double measurement(10.0)
std
Definition: Half.h:150
graph
NonlinearFactorGraph graph
Definition: doc/Code/OdometryExample.cpp:2
example::point
Point3 point(10, 0,-5)
TestStereoFactor
GenericStereoFactor< Pose3, Point3 > TestStereoFactor
Definition: testStereoFactor.cpp:47
gtsam::FactorGraph::emplace_shared
IsDerived< DERIVEDFACTOR > emplace_shared(Args &&...args)
Emplace a shared pointer to factor of given type.
Definition: FactorGraph.h:172
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
model
static SharedNoiseModel model(noiseModel::Unit::Create(3))
gtsam::Cal3_S2Stereo
Definition: Cal3_S2Stereo.h:30
camera1
static Pose3 camera1(Rot3(Vector3(1,-1,-1).asDiagonal()), Point3(0, 0, 6.25))
pose
static const Pose3 pose(Rot3(Vector3(1,-1,-1).asDiagonal()), Point3(0, 0, 0.5))
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
gtsam::NonlinearOptimizer::error
double error() const
return error in current optimizer state
Definition: NonlinearOptimizer.cpp:52
l1
gtsam::Key l1
Definition: testLinearContainerFactor.cpp:25
gtsam
traits
Definition: chartTesting.h:28
main
int main()
Definition: testStereoFactor.cpp:217
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:42
Cal3_S2Stereo.h
The most common 5DOF 3D->2D calibration + Stereo baseline.
gtsam::GenericStereoFactor::evaluateError
Vector evaluateError(const Pose3 &pose, const Point3 &point, boost::optional< Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none) const override
Definition: StereoFactor.h:122
gtsam::GaussNewtonOptimizer::iterate
GaussianFactorGraph::shared_ptr iterate() override
Definition: GaussNewtonOptimizer.cpp:44
StereoCamera.h
A Stereo Camera based on two Simple Cameras.
Point3.h
3D Point
simple_graph::factor2
JacobianFactor factor2(keyX, A21, keyY, A22, b2, noiseModel::Isotropic::Sigma(2, sigma2))
K
static boost::shared_ptr< Cal3_S2Stereo > K(new Cal3_S2Stereo(625, 625, 0, 320, 240, 0.5))
ceres::Matrix
Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor > Matrix
Definition: gtsam/3rdparty/ceres/eigen.h:42
gtsam::Point3
Vector3 Point3
Definition: Point3.h:35
TEST
TEST(StereoFactor, Constructor)
Definition: testStereoFactor.cpp:50
X
#define X
Definition: icosphere.cpp:20
Pose3.h
3D Pose
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:734
p
static Point3 p(0, 0, 5)
simple_graph::factor1
JacobianFactor factor1(keyX, A11, b1, noiseModel::Isotropic::Sigma(2, sigma1))

gtsam
Author(s):
autogenerated on Sat May 8 2021 02:49:57