testGeneralSFMFactor.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 
20 #include <gtsam/sam/RangeFactor.h>
21 #include <gtsam/geometry/Cal3_S2.h>
22 #include <gtsam/geometry/Rot2.h>
28 #include <gtsam/inference/Symbol.h>
29 #include <gtsam/base/Testable.h>
30 
31 #include <memory>
33 
34 #include <iostream>
35 #include <vector>
36 
37 using namespace std;
38 using namespace gtsam;
39 
40 // Convenience for named keys
43 
48 
49 class Graph: public NonlinearFactorGraph {
50 public:
51  void addMeasurement(int i, int j, const Point2& z,
52  const SharedNoiseModel& model) {
53  emplace_shared<Projection>(z, model, X(i), L(j));
54  }
55 
56  void addCameraConstraint(int j, const GeneralCamera& p) {
57  std::shared_ptr<CameraConstraint> factor(new CameraConstraint(X(j), p));
58  push_back(factor);
59  }
60 
61  void addPoint3Constraint(int j, const Point3& p) {
62  std::shared_ptr<Point3Constraint> factor(new Point3Constraint(L(j), p));
63  push_back(factor);
64  }
65 
66 };
67 
68 static double getGaussian() {
69  double S, V1, V2;
70  // Use Box-Muller method to create gauss noise from uniform noise
71  do {
72  double U1 = rand() / (double) (RAND_MAX);
73  double U2 = rand() / (double) (RAND_MAX);
74  V1 = 2 * U1 - 1; /* V1=[-1,1] */
75  V2 = 2 * U2 - 1; /* V2=[-1,1] */
76  S = V1 * V1 + V2 * V2;
77  } while (S >= 1);
78  return sqrt(-2.f * (double) log(S) / S) * V1;
79 }
80 
81 static const SharedNoiseModel sigma1(noiseModel::Unit::Create(2));
82 static const double baseline = 5.;
83 
84 /* ************************************************************************* */
85 static vector<Point3> genPoint3() {
86  const double z = 5;
87  vector<Point3> landmarks;
88  landmarks.push_back(Point3(-1., -1., z));
89  landmarks.push_back(Point3(-1., 1., z));
90  landmarks.push_back(Point3(1., 1., z));
91  landmarks.push_back(Point3(1., -1., z));
92  landmarks.push_back(Point3(-1.5, -1.5, 1.5 * z));
93  landmarks.push_back(Point3(-1.5, 1.5, 1.5 * z));
94  landmarks.push_back(Point3(1.5, 1.5, 1.5 * z));
95  landmarks.push_back(Point3(1.5, -1.5, 1.5 * z));
96  landmarks.push_back(Point3(-2., -2., 2 * z));
97  landmarks.push_back(Point3(-2., 2., 2 * z));
98  landmarks.push_back(Point3(2., 2., 2 * z));
99  landmarks.push_back(Point3(2., -2., 2 * z));
100  return landmarks;
101 }
102 
103 static vector<GeneralCamera> genCameraDefaultCalibration() {
104  vector<GeneralCamera> X;
105  X.push_back(GeneralCamera(Pose3(Rot3(), Point3(-baseline / 2., 0., 0.))));
106  X.push_back(GeneralCamera(Pose3(Rot3(), Point3(baseline / 2., 0., 0.))));
107  return X;
108 }
109 
110 static vector<GeneralCamera> genCameraVariableCalibration() {
111  const Cal3_S2 K(640, 480, 0.1, 320, 240);
112  vector<GeneralCamera> X;
113  X.push_back(GeneralCamera(Pose3(Rot3(), Point3(-baseline / 2., 0., 0.)), K));
114  X.push_back(GeneralCamera(Pose3(Rot3(), Point3(baseline / 2., 0., 0.)), K));
115  return X;
116 }
117 
118 static std::shared_ptr<Ordering> getOrdering(
119  const vector<GeneralCamera>& cameras, const vector<Point3>& landmarks) {
120  std::shared_ptr<Ordering> ordering(new Ordering);
121  for (size_t i = 0; i < landmarks.size(); ++i)
122  ordering->push_back(L(i));
123  for (size_t i = 0; i < cameras.size(); ++i)
124  ordering->push_back(X(i));
125  return ordering;
126 }
127 
128 /* ************************************************************************* */
130  // Create two identical factors and make sure they're equal
131  Point2 z(323., 240.);
132  const Symbol cameraFrameNumber('x', 1), landmarkNumber('l', 1);
133  const SharedNoiseModel sigma(noiseModel::Unit::Create(1));
134  std::shared_ptr<Projection> factor1(
135  new Projection(z, sigma, cameraFrameNumber, landmarkNumber));
136 
137  std::shared_ptr<Projection> factor2(
138  new Projection(z, sigma, cameraFrameNumber, landmarkNumber));
139 
141 }
142 
143 /* ************************************************************************* */
145  Point2 z(3., 0.);
146  const SharedNoiseModel sigma(noiseModel::Unit::Create(2));
147  Projection factor(z, sigma, X(1), L(1));
148  // For the following configuration, the factor predicts 320,240
149  Values values;
150  Rot3 R;
151  Point3 t1(0, 0, -6);
152  Pose3 x1(R, t1);
154  Point3 l1(0,0,0);
155  values.insert(L(1), l1);
156  EXPECT(
157  assert_equal(((Vector ) Vector2(-3., 0.)),
158  factor.unwhitenedError(values)));
159 }
160 
161 /* ************************************************************************* */
162 TEST( GeneralSFMFactor, optimize_defaultK ) {
163 
164  vector<Point3> landmarks = genPoint3();
165  vector<GeneralCamera> cameras = genCameraDefaultCalibration();
166 
167  // add measurement with noise
168  Graph graph;
169  for (size_t j = 0; j < cameras.size(); ++j) {
170  for (size_t i = 0; i < landmarks.size(); ++i) {
171  Point2 pt = cameras[j].project(landmarks[i]);
172  graph.addMeasurement(j, i, pt, sigma1);
173  }
174  }
175 
176  const size_t nMeasurements = cameras.size() * landmarks.size();
177 
178  // add initial
179  const double noise = baseline * 0.1;
180  Values values;
181  for (size_t i = 0; i < cameras.size(); ++i)
182  values.insert(X(i), cameras[i]);
183 
184  for (size_t i = 0; i < landmarks.size(); ++i) {
185  Point3 pt(landmarks[i].x() + noise * getGaussian(),
186  landmarks[i].y() + noise * getGaussian(),
187  landmarks[i].z() + noise * getGaussian());
188  values.insert(L(i), pt);
189  }
190 
191  graph.addCameraConstraint(0, cameras[0]);
192 
193  // Create an ordering of the variables
194  Ordering ordering = *getOrdering(cameras, landmarks);
196  Values final = optimizer.optimize();
197  EXPECT(optimizer.error() < 0.5 * 1e-5 * nMeasurements);
198 }
199 
200 /* ************************************************************************* */
201 TEST( GeneralSFMFactor, optimize_varK_SingleMeasurementError ) {
202  vector<Point3> landmarks = genPoint3();
203  vector<GeneralCamera> cameras = genCameraVariableCalibration();
204  // add measurement with noise
205  Graph graph;
206  for (size_t j = 0; j < cameras.size(); ++j) {
207  for (size_t i = 0; i < landmarks.size(); ++i) {
208  Point2 pt = cameras[j].project(landmarks[i]);
209  graph.addMeasurement(j, i, pt, sigma1);
210  }
211  }
212 
213  const size_t nMeasurements = cameras.size() * landmarks.size();
214 
215  // add initial
216  const double noise = baseline * 0.1;
217  Values values;
218  for (size_t i = 0; i < cameras.size(); ++i)
219  values.insert(X(i), cameras[i]);
220 
221  // add noise only to the first landmark
222  for (size_t i = 0; i < landmarks.size(); ++i) {
223  if (i == 0) {
224  Point3 pt(landmarks[i].x() + noise * getGaussian(),
225  landmarks[i].y() + noise * getGaussian(),
226  landmarks[i].z() + noise * getGaussian());
227  values.insert(L(i), pt);
228  } else {
229  values.insert(L(i), landmarks[i]);
230  }
231  }
232 
233  graph.addCameraConstraint(0, cameras[0]);
234  const double reproj_error = 1e-5;
235 
236  Ordering ordering = *getOrdering(cameras, landmarks);
238  Values final = optimizer.optimize();
239  EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements);
240 }
241 
242 /* ************************************************************************* */
243 TEST( GeneralSFMFactor, optimize_varK_FixCameras ) {
244 
245  vector<Point3> landmarks = genPoint3();
246  vector<GeneralCamera> cameras = genCameraVariableCalibration();
247 
248  // add measurement with noise
249  const double noise = baseline * 0.1;
250  Graph graph;
251  for (size_t i = 0; i < cameras.size(); ++i) {
252  for (size_t j = 0; j < landmarks.size(); ++j) {
253  Point2 z = cameras[i].project(landmarks[j]);
254  graph.addMeasurement(i, j, z, sigma1);
255  }
256  }
257 
258  const size_t nMeasurements = landmarks.size() * cameras.size();
259 
260  Values values;
261  for (size_t i = 0; i < cameras.size(); ++i)
262  values.insert(X(i), cameras[i]);
263 
264  for (size_t j = 0; j < landmarks.size(); ++j) {
265  Point3 pt(landmarks[j].x() + noise * getGaussian(),
266  landmarks[j].y() + noise * getGaussian(),
267  landmarks[j].z() + noise * getGaussian());
268  values.insert(L(j), pt);
269  }
270 
271  for (size_t i = 0; i < cameras.size(); ++i)
272  graph.addCameraConstraint(i, cameras[i]);
273 
274  const double reproj_error = 1e-5;
275 
276  Ordering ordering = *getOrdering(cameras, landmarks);
278  Values final = optimizer.optimize();
279  EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements);
280 }
281 
282 /* ************************************************************************* */
283 TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) {
284 
285  vector<Point3> landmarks = genPoint3();
286  vector<GeneralCamera> cameras = genCameraVariableCalibration();
287 
288  // add measurement with noise
289  Graph graph;
290  for (size_t j = 0; j < cameras.size(); ++j) {
291  for (size_t i = 0; i < landmarks.size(); ++i) {
292  Point2 pt = cameras[j].project(landmarks[i]);
293  graph.addMeasurement(j, i, pt, sigma1);
294  }
295  }
296 
297  const size_t nMeasurements = landmarks.size() * cameras.size();
298 
299  Values values;
300  for (size_t i = 0; i < cameras.size(); ++i) {
301  const double rot_noise = 1e-5, trans_noise = 1e-3, focal_noise = 1,
302  skew_noise = 1e-5;
303  if (i == 0) {
304  values.insert(X(i), cameras[i]);
305  } else {
306 
307  Vector delta = (Vector(11) << rot_noise, rot_noise, rot_noise, // rotation
308  trans_noise, trans_noise, trans_noise, // translation
309  focal_noise, focal_noise, // f_x, f_y
310  skew_noise, // s
311  trans_noise, trans_noise // ux, uy
312  ).finished();
313  values.insert(X(i), cameras[i].retract(delta));
314  }
315  }
316 
317  for (size_t i = 0; i < landmarks.size(); ++i) {
318  values.insert(L(i), landmarks[i]);
319  }
320 
321  // fix X0 and all landmarks, allow only the cameras[1] to move
322  graph.addCameraConstraint(0, cameras[0]);
323  for (size_t i = 0; i < landmarks.size(); ++i)
324  graph.addPoint3Constraint(i, landmarks[i]);
325 
326  const double reproj_error = 1e-5;
327 
328  Ordering ordering = *getOrdering(cameras, landmarks);
330  Values final = optimizer.optimize();
331  EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements);
332 }
333 
334 /* ************************************************************************* */
335 TEST( GeneralSFMFactor, optimize_varK_BA ) {
336  vector<Point3> landmarks = genPoint3();
337  vector<GeneralCamera> cameras = genCameraVariableCalibration();
338 
339  // add measurement with noise
340  Graph graph;
341  for (size_t j = 0; j < cameras.size(); ++j) {
342  for (size_t i = 0; i < landmarks.size(); ++i) {
343  Point2 pt = cameras[j].project(landmarks[i]);
344  graph.addMeasurement(j, i, pt, sigma1);
345  }
346  }
347 
348  const size_t nMeasurements = cameras.size() * landmarks.size();
349 
350  // add initial
351  const double noise = baseline * 0.1;
352  Values values;
353  for (size_t i = 0; i < cameras.size(); ++i)
354  values.insert(X(i), cameras[i]);
355 
356  // add noise only to the first landmark
357  for (size_t i = 0; i < landmarks.size(); ++i) {
358  Point3 pt(landmarks[i].x() + noise * getGaussian(),
359  landmarks[i].y() + noise * getGaussian(),
360  landmarks[i].z() + noise * getGaussian());
361  values.insert(L(i), pt);
362  }
363 
364  // Constrain position of system with the first camera constrained to the origin
365  graph.addCameraConstraint(0, cameras[0]);
366 
367  // Constrain the scale of the problem with a soft range factor of 1m between the cameras
370  noiseModel::Isotropic::Sigma(1, 10.));
371 
372  const double reproj_error = 1e-5;
373 
374  Ordering ordering = *getOrdering(cameras, landmarks);
376  Values final = optimizer.optimize();
377  EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements);
378 }
379 
380 /* ************************************************************************* */
381 TEST(GeneralSFMFactor, GeneralCameraPoseRange) {
382  // Tests range factor between a GeneralCamera and a Pose3
383  Graph graph;
384  graph.addCameraConstraint(0, GeneralCamera());
387  noiseModel::Isotropic::Sigma(1, 1.));
388  graph.addPrior(X(1), Pose3(Rot3(), Point3(1., 0., 0.)),
389  noiseModel::Isotropic::Sigma(6, 1.));
390 
391  Values init;
392  init.insert(X(0), GeneralCamera());
393  init.insert(X(1), Pose3(Rot3(), Point3(1., 1., 1.)));
394 
395  // The optimal value between the 2m range factor and 1m prior is 1.5m
397  expected.insert(X(0), GeneralCamera());
398  expected.insert(X(1), Pose3(Rot3(), Point3(1.5, 0., 0.)));
399 
401  params.absoluteErrorTol = 1e-9;
402  params.relativeErrorTol = 1e-9;
404 
405  EXPECT(assert_equal(expected, actual, 1e-4));
406 }
407 
408 /* ************************************************************************* */
409 TEST(GeneralSFMFactor, CalibratedCameraPoseRange) {
410  // Tests range factor between a CalibratedCamera and a Pose3
413  noiseModel::Isotropic::Sigma(6, 1.));
416  noiseModel::Isotropic::Sigma(1, 1.));
417  graph.addPrior(X(1), Pose3(Rot3(), Point3(1., 0., 0.)),
418  noiseModel::Isotropic::Sigma(6, 1.));
419 
420  Values init;
421  init.insert(X(0), CalibratedCamera());
422  init.insert(X(1), Pose3(Rot3(), Point3(1., 1., 1.)));
423 
425  expected.insert(X(0),
426  CalibratedCamera(Pose3(Rot3(), Point3(-0.333333333333, 0, 0))));
427  expected.insert(X(1), Pose3(Rot3(), Point3(1.333333333333, 0, 0)));
428 
430  params.absoluteErrorTol = 1e-9;
431  params.relativeErrorTol = 1e-9;
433 
434  EXPECT(assert_equal(expected, actual, 1e-4));
435 }
436 
437 /* ************************************************************************* */
438 // Frank created these tests after switching to a custom LinearizedFactor
440  Point2 measurement(3., -1.);
441 
442  // Create Values
443  Values values;
444  Rot3 R;
445  Point3 t1(0, 0, -6);
446  Pose3 x1(R, t1);
448  Point3 l1(0,0,0);
449  values.insert(L(1), l1);
450 
451  vector<SharedNoiseModel> models;
452  {
453  // Create various noise-models to test all cases
454  using namespace noiseModel;
455  Rot2 R = Rot2::fromAngle(0.3);
456  Matrix2 cov = R.matrix() * R.matrix().transpose();
457  models = {SharedNoiseModel(), Unit::Create(2), Isotropic::Sigma(2, 0.5),
458  Constrained::All(2), Gaussian::Covariance(cov)};
459  }
460 
461  // Now loop over all these noise models
462  for(SharedNoiseModel model: models) {
463  Projection factor(measurement, model, X(1), L(1));
464 
465  // Test linearize
467  factor.NoiseModelFactor::linearize(values);
469  EXPECT(assert_equal(*expected, *actual, 1e-9));
470 
471  // Test methods that rely on updateHessian
472  if (model && !model->isConstrained()) {
473  // Construct HessianFactor from single JacobianFactor
474  HessianFactor expectedHessian(*expected), actualHessian(*actual);
475  EXPECT(assert_equal(expectedHessian, actualHessian, 1e-9));
476 
477  // Convert back
478  JacobianFactor actualJacobian(actualHessian);
479  // Note we do not expect the actualJacobian to match *expected
480  // Just that they have the same information on the variable.
481  EXPECT(
482  assert_equal(expected->augmentedInformation(),
483  actualJacobian.augmentedInformation(), 1e-9));
484 
485  // Construct from GaussianFactorGraph
486  GaussianFactorGraph gfg1 {expected}, gfg2 {actual};
487  HessianFactor hessian1(gfg1), hessian2(gfg2);
488  EXPECT(assert_equal(hessian1, hessian2, 1e-9));
489  }
490  }
491 }
492 
493 /* ************************************************************************* */
494 // Do a thorough test of BinaryJacobianFactor
495 TEST( GeneralSFMFactor, BinaryJacobianFactor2 ) {
496 
497  vector<Point3> landmarks = genPoint3();
498  vector<GeneralCamera> cameras = genCameraVariableCalibration();
499 
500  Values values;
501  for (size_t i = 0; i < cameras.size(); ++i)
502  values.insert(X(i), cameras[i]);
503  for (size_t j = 0; j < landmarks.size(); ++j)
504  values.insert(L(j), landmarks[j]);
505 
506  for (size_t i = 0; i < cameras.size(); ++i) {
507  for (size_t j = 0; j < landmarks.size(); ++j) {
508  Point2 z = cameras[i].project(landmarks[j]);
509  Projection::shared_ptr nonlinear = //
510  std::make_shared<Projection>(z, sigma1, X(i), L(j));
511  GaussianFactor::shared_ptr factor = nonlinear->linearize(values);
512  HessianFactor hessian(*factor);
513  JacobianFactor jacobian(hessian);
514  EXPECT(
515  assert_equal(factor->augmentedInformation(),
516  jacobian.augmentedInformation(), 1e-9));
517  }
518  }
519 }
520 
521 /* ************************************************************************* */
522 int main() {
523  TestResult tr;
524  return TestRegistry::runAllTests(tr);
525 }
526 /* ************************************************************************* */
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
gtsam::GeneralSFMFactor::linearize
std::shared_ptr< GaussianFactor > linearize(const Values &values) const override
Linearize using fixed-size matrices.
Definition: GeneralSFMFactor.h:140
gtsam::NonlinearOptimizer::optimize
virtual const Values & optimize()
Definition: NonlinearOptimizer.h:98
main
int main()
Definition: testGeneralSFMFactor.cpp:522
gtsam::RangeFactor
Definition: sam/RangeFactor.h:35
fixture::cameras
std::array< PinholeCamera< Cal3_S2 >, 3 > cameras
Definition: testTransferFactor.cpp:59
simple_graph::factor2
auto factor2
Definition: testJacobianFactor.cpp:203
gtsam::HessianFactor
A Gaussian factor using the canonical parameters (information form)
Definition: HessianFactor.h:100
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
CameraConstraint
NonlinearEquality< GeneralCamera > CameraConstraint
Definition: testGeneralSFMFactor.cpp:46
Testable.h
Concept check for values that can be used in unit tests.
EXPECT
#define EXPECT(condition)
Definition: Test.h:150
gtsam::Vector2
Eigen::Vector2d Vector2
Definition: Vector.h:42
Cal3_S2.h
The most common 5DOF 3D->2D calibration.
TestHarness.h
Projection
GeneralSFMFactor< GeneralCamera, Point3 > Projection
Definition: testGeneralSFMFactor.cpp:45
LevenbergMarquardtOptimizer.h
A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme.
getGaussian
static double getGaussian()
Definition: testGeneralSFMFactor.cpp:68
x
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy x
Definition: gnuplot_common_settings.hh:12
RangeFactor.h
Serializable factor induced by a range measurement.
gtsam::JacobianFactor
Definition: JacobianFactor.h:91
pt
static const Point3 pt(1.0, 2.0, 3.0)
different_sigmas::values
HybridValues values
Definition: testHybridBayesNet.cpp:245
log
const EIGEN_DEVICE_FUNC LogReturnType log() const
Definition: ArrayCwiseUnaryOps.h:128
GeneralSFMFactor.h
a general SFM factor with an unknown calibration
X
#define X
Definition: icosphere.cpp:20
gtsam::Rot2::matrix
Matrix2 matrix() const
Definition: Rot2.cpp:85
sigma1
static const SharedNoiseModel sigma1(noiseModel::Unit::Create(2))
vanilla::params
static const SmartProjectionParams params
Definition: smartFactorScenarios.h:69
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
gtsam::NonlinearEquality
Definition: NonlinearEquality.h:43
gtsam::BinaryJacobianFactor
Definition: BinaryJacobianFactor.h:33
gtsam::NoiseModelFactorN::unwhitenedError
Vector unwhitenedError(const Values &x, OptionalMatrixVecType H=nullptr) const override
Definition: NonlinearFactor.h:606
Rot2.h
2D rotation
sampling::sigma
static const double sigma
Definition: testGaussianBayesNet.cpp:170
gtsam::NonlinearOptimizer::error
double error() const
return error in current optimizer state
Definition: NonlinearOptimizer.cpp:49
Graph::addPoint3Constraint
void addPoint3Constraint(int j, const Point3 &p)
Definition: testGeneralSFMFactor.cpp:61
gtsam::GaussianFactorGraph
Definition: GaussianFactorGraph.h:73
genCameraDefaultCalibration
static vector< GeneralCamera > genCameraDefaultCalibration()
Definition: testGeneralSFMFactor.cpp:103
gtsam.examples.PlanarManipulatorExample.delta
def delta(g0, g1)
Definition: PlanarManipulatorExample.py:45
baseline
static const double baseline
Definition: testGeneralSFMFactor.cpp:82
j
std::ptrdiff_t j
Definition: tut_arithmetic_redux_minmax.cpp:2
x1
Pose3 x1
Definition: testPose3.cpp:663
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
gtsam::PinholeCamera
Definition: PinholeCamera.h:33
gtsam::NonlinearFactorGraph
Definition: NonlinearFactorGraph.h:55
gtsam::GaussianFactor::shared_ptr
std::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition: GaussianFactor.h:42
cholesky::expected
Matrix expected
Definition: testMatrix.cpp:971
gtsam::NonlinearFactorGraph::addPrior
void addPrior(Key key, const T &prior, const SharedNoiseModel &model=nullptr)
Definition: NonlinearFactorGraph.h:199
PinholeCamera.h
Base class for all pinhole cameras.
gtsam::LevenbergMarquardtOptimizer
Definition: LevenbergMarquardtOptimizer.h:35
L
MatrixXd L
Definition: LLT_example.cpp:6
Symbol.h
pybind_wrapper_test_script.z
z
Definition: pybind_wrapper_test_script.py:61
gtsam::HybridValues::insert
void insert(Key j, const Vector &value)
Definition: HybridValues.cpp:85
VectorValues.h
Factor Graph Values.
gtsam::Point2
Vector2 Point2
Definition: Point2.h:32
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:762
NonlinearEquality.h
genPoint3
static vector< Point3 > genPoint3()
Definition: testGeneralSFMFactor.cpp:85
init
detail::initimpl::constructor< Args... > init()
Binds an existing constructor taking arguments Args...
Definition: pybind11.h:2006
gtsam::CalibratedCamera
Definition: CalibratedCamera.h:251
model
noiseModel::Diagonal::shared_ptr model
Definition: doc/Code/Pose2SLAMExample.cpp:7
ordering
static enum @1096 ordering
gtsam::equals
Definition: Testable.h:112
TestResult
Definition: TestResult.h:26
y
Scalar * y
Definition: level1_cplx_impl.h:124
tree::f
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
Definition: testExpression.cpp:218
gtsam::GeneralSFMFactor::shared_ptr
std::shared_ptr< This > shared_ptr
Definition: GeneralSFMFactor.h:82
GeneralCamera
PinholeCamera< Cal3_S2 > GeneralCamera
Definition: testGeneralSFMFactor.cpp:44
gtsam::FactorGraph::size
size_t size() const
Definition: FactorGraph.h:297
gtsam::Rot2
Definition: Rot2.h:35
gtsam
traits
Definition: SFMdata.h:40
error
static double error
Definition: testRot3.cpp:37
K
#define K
Definition: igam.h:8
NonlinearFactorGraph.h
Factor Graph consisting of non-linear factors.
gtsam::Values
Definition: Values.h:65
l1
gtsam::Key l1
Definition: testLinearContainerFactor.cpp:24
std
Definition: BFloat16.h:88
p
float * p
Definition: Tutorial_Map_using.cpp:9
gtsam::GeneralSFMFactor
Definition: GeneralSFMFactor.h:59
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:40
getOrdering
static std::shared_ptr< Ordering > getOrdering(const vector< GeneralCamera > &cameras, const vector< Point3 > &landmarks)
Definition: testGeneralSFMFactor.cpp:118
gtsam::LevenbergMarquardtParams
Definition: LevenbergMarquardtParams.h:35
gtsam::Cal3_S2
The most common 5DOF 3D->2D calibration.
Definition: Cal3_S2.h:34
genCameraVariableCalibration
static vector< GeneralCamera > genCameraVariableCalibration()
Definition: testGeneralSFMFactor.cpp:110
gtsam::Point3
Vector3 Point3
Definition: Point3.h:38
Graph
Definition: testGeneralSFMFactor.cpp:49
Graph::addMeasurement
void addMeasurement(int i, int j, const Point2 &z, const SharedNoiseModel &model)
Definition: testGeneralSFMFactor.cpp:51
graph
NonlinearFactorGraph graph
Definition: doc/Code/OdometryExample.cpp:2
Graph::addCameraConstraint
void addCameraConstraint(int j, const GeneralCamera &p)
Definition: testGeneralSFMFactor.cpp:56
init
Definition: TutorialInplaceLU.cpp:2
gtsam::Ordering
Definition: inference/Ordering.h:33
Point3Constraint
NonlinearEquality< Point3 > Point3Constraint
Definition: testGeneralSFMFactor.cpp:47
ceres::Vector
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
Definition: gtsam/3rdparty/ceres/eigen.h:38
ceres::sqrt
Jet< T, N > sqrt(const Jet< T, N > &f)
Definition: jet.h:418
i
int i
Definition: BiCGSTAB_step_by_step.cpp:9
gtsam::JacobianFactor::augmentedInformation
Matrix augmentedInformation() const override
Definition: JacobianFactor.cpp:494
R
Rot2 R(Rot2::fromAngle(0.1))
simple_graph::factor1
auto factor1
Definition: testJacobianFactor.cpp:196
measurement
static Point2 measurement(323.0, 240.0)
TEST
TEST(GeneralSFMFactor, equals)
Definition: testGeneralSFMFactor.cpp:129
S
DiscreteKey S(1, 2)
gtsam::FactorGraph::emplace_shared
IsDerived< DERIVEDFACTOR > emplace_shared(Args &&... args)
Emplace a shared pointer to factor of given type.
Definition: FactorGraph.h:153
gtsam::Symbol
Definition: inference/Symbol.h:37


gtsam
Author(s):
autogenerated on Sat Nov 16 2024 04:07:31