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 
22 #include <gtsam/nonlinear/Values.h>
23 #include <gtsam/inference/Symbol.h>
26 #include <gtsam/geometry/Pose3.h>
27 #include <gtsam/geometry/Point3.h>
28 
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 std::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
46 
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 
71 
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 
83 
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 {
187 
189 
190  StereoPoint2 measurement(320, 320.0-50, 240);
191  // arguments: measurement, sigma, cam#, measurement #, K, baseline (m)
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 /* ************************************************************************* */
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
TestStereoFactor
GenericStereoFactor< Pose3, Point3 > TestStereoFactor
Definition: testStereoFactor.cpp:47
gtsam::NonlinearOptimizer::optimize
virtual const Values & optimize()
Definition: NonlinearOptimizer.h:98
pose
static const Pose3 pose(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5))
gtsam::GaussNewtonOptimizer
Definition: GaussNewtonOptimizer.h:38
simple_graph::factor2
auto factor2
Definition: testJacobianFactor.cpp:203
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
TestHarness.h
camera1
static Pose3 camera1(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 6.25))
gtsam::StereoFactor
Definition: testSmartFactorBase.cpp:80
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
different_sigmas::values
HybridValues values
Definition: testHybridBayesNet.cpp:245
X
#define X
Definition: icosphere.cpp:20
Point3.h
3D Point
gtsam::Vector3
Eigen::Vector3d Vector3
Definition: Vector.h:43
gtsam::GenericStereoFactor
Definition: StereoFactor.h:32
body_P_sensor
static Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0))
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
gtsam::NonlinearEquality
Definition: NonlinearEquality.h:43
p
static Point3 p(0, 0, 5)
gtsam::NonlinearOptimizer::error
double error() const
return error in current optimizer state
Definition: NonlinearOptimizer.cpp:49
GaussNewtonOptimizer.h
gtsam::GenericStereoFactor::evaluateError
Vector evaluateError(const Pose3 &pose, const Point3 &point, OptionalMatrixType H1, OptionalMatrixType H2) const override
Definition: StereoFactor.h:126
gtsam_unstable.tests.test_ProjectionFactorRollingShutter.point
point
Definition: test_ProjectionFactorRollingShutter.py:25
ceres::Matrix
Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor > Matrix
Definition: gtsam/3rdparty/ceres/eigen.h:42
main
int main()
Definition: testStereoFactor.cpp:217
model
static SharedNoiseModel model(noiseModel::Unit::Create(3))
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
DOUBLES_EQUAL
#define DOUBLES_EQUAL(expected, actual, threshold)
Definition: Test.h:141
StereoFactor.h
A non-linear factor for stereo measurements.
gtsam::NonlinearFactorGraph
Definition: NonlinearFactorGraph.h:55
TEST
TEST(StereoFactor, Constructor)
Definition: testStereoFactor.cpp:50
L
MatrixXd L
Definition: LLT_example.cpp:6
Symbol.h
gtsam::HybridValues::insert
void insert(Key j, const Vector &value)
Definition: HybridValues.cpp:85
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:762
Cal3_S2Stereo.h
The most common 5DOF 3D->2D calibration + Stereo baseline.
NonlinearEquality.h
K
static std::shared_ptr< Cal3_S2Stereo > K(new Cal3_S2Stereo(625, 625, 0, 320, 240, 0.5))
TestResult
Definition: TestResult.h:26
M_PI_2
#define M_PI_2
Definition: mconf.h:118
gtsam
traits
Definition: SFMdata.h:40
gtsam::Cal3_S2Stereo
The most common 5DOF 3D->2D calibration, stereo version.
Definition: Cal3_S2Stereo.h:30
NonlinearFactorGraph.h
Factor Graph consisting of non-linear factors.
gtsam::Values
Definition: Values.h:65
CHECK
#define CHECK(condition)
Definition: Test.h:108
gtsam::StereoPoint2
Definition: StereoPoint2.h:34
l1
gtsam::Key l1
Definition: testLinearContainerFactor.cpp:24
std
Definition: BFloat16.h:88
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:40
gtsam::GaussNewtonOptimizer::iterate
GaussianFactorGraph::shared_ptr iterate() override
Definition: GaussNewtonOptimizer.cpp:44
gtsam::Point3
Vector3 Point3
Definition: Point3.h:38
graph
NonlinearFactorGraph graph
Definition: doc/Code/OdometryExample.cpp:2
simple_graph::factor1
auto factor1
Definition: testJacobianFactor.cpp:196
measurement
static Point2 measurement(323.0, 240.0)
Values.h
A non-templated config holding any types of Manifold-group elements.
Pose3.h
3D Pose
gtsam::FactorGraph::emplace_shared
IsDerived< DERIVEDFACTOR > emplace_shared(Args &&... args)
Emplace a shared pointer to factor of given type.
Definition: FactorGraph.h:153
StereoCamera.h
A Stereo Camera based on two Simple Cameras.


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:41:48