testGeneralSFMFactor_Cal3Bundler.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>
23 #include <gtsam/inference/Symbol.h>
26 #include <gtsam/geometry/Point3.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 /* ************************************************************************* */
50 class Graph: public NonlinearFactorGraph {
51 public:
52  void addMeasurement(const int& i, const int& j, const Point2& z,
53  const SharedNoiseModel& model) {
54  emplace_shared<Projection>(z, model, X(i), L(j));
55  }
56 
57  void addCameraConstraint(int j, const GeneralCamera& p) {
58  std::shared_ptr<CameraConstraint> factor(new CameraConstraint(X(j), p));
59  push_back(factor);
60  }
61 
62  void addPoint3Constraint(int j, const Point3& p) {
63  std::shared_ptr<Point3Constraint> factor(new Point3Constraint(L(j), p));
64  push_back(factor);
65  }
66 
67 };
68 
69 static double getGaussian() {
70  double S, V1, V2;
71  // Use Box-Muller method to create gauss noise from uniform noise
72  do {
73  double U1 = rand() / (double) (RAND_MAX);
74  double U2 = rand() / (double) (RAND_MAX);
75  V1 = 2 * U1 - 1; /* V1=[-1,1] */
76  V2 = 2 * U2 - 1; /* V2=[-1,1] */
77  S = V1 * V1 + V2 * V2;
78  } while (S >= 1);
79  return sqrt(-2.f * (double) log(S) / S) * V1;
80 }
81 
82 static const SharedNoiseModel sigma1(noiseModel::Unit::Create(2));
83 
84 /* ************************************************************************* */
85 TEST( GeneralSFMFactor_Cal3Bundler, equals ) {
86  // Create two identical factors and make sure they're equal
87  Point2 z(323., 240.);
88  const Symbol cameraFrameNumber('x', 1), landmarkNumber('l', 1);
89  const SharedNoiseModel sigma(noiseModel::Unit::Create(1));
90  std::shared_ptr<Projection> factor1(
91  new Projection(z, sigma, cameraFrameNumber, landmarkNumber));
92 
93  std::shared_ptr<Projection> factor2(
94  new Projection(z, sigma, cameraFrameNumber, landmarkNumber));
95 
96  EXPECT(assert_equal(*factor1, *factor2));
97 }
98 
99 /* ************************************************************************* */
100 TEST( GeneralSFMFactor_Cal3Bundler, error ) {
101  Point2 z(3., 0.);
102  const SharedNoiseModel sigma(noiseModel::Unit::Create(1));
103  std::shared_ptr<Projection> factor(new Projection(z, sigma, X(1), L(1)));
104  // For the following configuration, the factor predicts 320,240
105  Values values;
106  Rot3 R;
107  Point3 t1(0, 0, -6);
108  Pose3 x1(R, t1);
109  values.insert(X(1), GeneralCamera(x1));
110  Point3 l1(0,0,0);
111  values.insert(L(1), l1);
112  EXPECT(assert_equal(Vector2(-3., 0.), factor->unwhitenedError(values)));
113 }
114 
115 static const double baseline = 5.;
116 
117 /* ************************************************************************* */
118 static vector<Point3> genPoint3() {
119  const double z = 5;
120  vector<Point3> landmarks;
121  landmarks.push_back(Point3(-1., -1., z));
122  landmarks.push_back(Point3(-1., 1., z));
123  landmarks.push_back(Point3(1., 1., z));
124  landmarks.push_back(Point3(1., -1., z));
125  landmarks.push_back(Point3(-1.5, -1.5, 1.5 * z));
126  landmarks.push_back(Point3(-1.5, 1.5, 1.5 * z));
127  landmarks.push_back(Point3(1.5, 1.5, 1.5 * z));
128  landmarks.push_back(Point3(1.5, -1.5, 1.5 * z));
129  landmarks.push_back(Point3(-2., -2., 2 * z));
130  landmarks.push_back(Point3(-2., 2., 2 * z));
131  landmarks.push_back(Point3(2., 2., 2 * z));
132  landmarks.push_back(Point3(2., -2., 2 * z));
133  return landmarks;
134 }
135 
136 static vector<GeneralCamera> genCameraDefaultCalibration() {
137  vector<GeneralCamera> cameras;
138  cameras.push_back(
139  GeneralCamera(Pose3(Rot3(), Point3(-baseline / 2., 0., 0.))));
140  cameras.push_back(
141  GeneralCamera(Pose3(Rot3(), Point3(baseline / 2., 0., 0.))));
142  return cameras;
143 }
144 
145 static vector<GeneralCamera> genCameraVariableCalibration() {
146  const Cal3Bundler K(500, 1e-3, 1e-3);
147  vector<GeneralCamera> cameras;
148  cameras.push_back(
149  GeneralCamera(Pose3(Rot3(), Point3(-baseline / 2., 0., 0.)), K));
150  cameras.push_back(
151  GeneralCamera(Pose3(Rot3(), Point3(baseline / 2., 0., 0.)), K));
152  return cameras;
153 }
154 
155 static std::shared_ptr<Ordering> getOrdering(
156  const std::vector<GeneralCamera>& cameras,
157  const std::vector<Point3>& landmarks) {
158  std::shared_ptr<Ordering> ordering(new Ordering);
159  for (size_t i = 0; i < landmarks.size(); ++i)
160  ordering->push_back(L(i));
161  for (size_t i = 0; i < cameras.size(); ++i)
162  ordering->push_back(X(i));
163  return ordering;
164 }
165 
166 /* ************************************************************************* */
167 TEST( GeneralSFMFactor_Cal3Bundler, optimize_defaultK ) {
168 
169  vector<Point3> landmarks = genPoint3();
170  vector<GeneralCamera> cameras = genCameraDefaultCalibration();
171 
172  // add measurement with noise
173  Graph graph;
174  for (size_t j = 0; j < cameras.size(); ++j) {
175  for (size_t i = 0; i < landmarks.size(); ++i) {
176  Point2 pt = cameras[j].project(landmarks[i]);
177  graph.addMeasurement(j, i, pt, sigma1);
178  }
179  }
180 
181  const size_t nMeasurements = cameras.size() * landmarks.size();
182 
183  // add initial
184  const double noise = baseline * 0.1;
185  Values values;
186  for (size_t i = 0; i < cameras.size(); ++i)
187  values.insert(X(i), cameras[i]);
188 
189  for (size_t i = 0; i < landmarks.size(); ++i) {
190  Point3 pt(landmarks[i].x() + noise * getGaussian(),
191  landmarks[i].y() + noise * getGaussian(),
192  landmarks[i].z() + noise * getGaussian());
193  values.insert(L(i), pt);
194  }
195 
196  graph.addCameraConstraint(0, cameras[0]);
197 
198  // Create an ordering of the variables
199  Ordering ordering = *getOrdering(cameras, landmarks);
200  LevenbergMarquardtOptimizer optimizer(graph, values, ordering);
201  Values final = optimizer.optimize();
202  EXPECT(optimizer.error() < 0.5 * 1e-5 * nMeasurements);
203 }
204 
205 /* ************************************************************************* */
206 TEST( GeneralSFMFactor_Cal3Bundler, optimize_varK_SingleMeasurementError ) {
207  vector<Point3> landmarks = genPoint3();
208  vector<GeneralCamera> cameras = genCameraVariableCalibration();
209  // add measurement with noise
210  Graph graph;
211  for (size_t j = 0; j < cameras.size(); ++j) {
212  for (size_t i = 0; i < landmarks.size(); ++i) {
213  Point2 pt = cameras[j].project(landmarks[i]);
214  graph.addMeasurement(j, i, pt, sigma1);
215  }
216  }
217 
218  const size_t nMeasurements = cameras.size() * landmarks.size();
219 
220  // add initial
221  const double noise = baseline * 0.1;
222  Values values;
223  for (size_t i = 0; i < cameras.size(); ++i)
224  values.insert(X(i), cameras[i]);
225 
226  // add noise only to the first landmark
227  for (size_t i = 0; i < landmarks.size(); ++i) {
228  if (i == 0) {
229  Point3 pt(landmarks[i].x() + noise * getGaussian(),
230  landmarks[i].y() + noise * getGaussian(),
231  landmarks[i].z() + noise * getGaussian());
232  values.insert(L(i), pt);
233  } else {
234  values.insert(L(i), landmarks[i]);
235  }
236  }
237 
238  graph.addCameraConstraint(0, cameras[0]);
239  const double reproj_error = 1e-5;
240 
241  Ordering ordering = *getOrdering(cameras, landmarks);
242  LevenbergMarquardtOptimizer optimizer(graph, values, ordering);
243  Values final = optimizer.optimize();
244  EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements);
245 }
246 
247 /* ************************************************************************* */
248 TEST( GeneralSFMFactor_Cal3Bundler, optimize_varK_FixCameras ) {
249 
250  vector<Point3> landmarks = genPoint3();
251  vector<GeneralCamera> cameras = genCameraVariableCalibration();
252 
253  // add measurement with noise
254  const double noise = baseline * 0.1;
255  Graph graph;
256  for (size_t j = 0; j < cameras.size(); ++j) {
257  for (size_t i = 0; i < landmarks.size(); ++i) {
258  Point2 pt = cameras[j].project(landmarks[i]);
259  graph.addMeasurement(j, i, pt, sigma1);
260  }
261  }
262 
263  const size_t nMeasurements = landmarks.size() * cameras.size();
264 
265  Values values;
266  for (size_t i = 0; i < cameras.size(); ++i)
267  values.insert(X(i), cameras[i]);
268 
269  for (size_t i = 0; i < landmarks.size(); ++i) {
270  Point3 pt(landmarks[i].x() + noise * getGaussian(),
271  landmarks[i].y() + noise * getGaussian(),
272  landmarks[i].z() + noise * getGaussian());
273  //Point3 pt(landmarks[i].x(), landmarks[i].y(), landmarks[i].z());
274  values.insert(L(i), pt);
275  }
276 
277  for (size_t i = 0; i < cameras.size(); ++i)
278  graph.addCameraConstraint(i, cameras[i]);
279 
280  const double reproj_error = 1e-5;
281 
282  Ordering ordering = *getOrdering(cameras, landmarks);
283  LevenbergMarquardtOptimizer optimizer(graph, values, ordering);
284  Values final = optimizer.optimize();
285  EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements);
286 }
287 
288 /* ************************************************************************* */
289 TEST( GeneralSFMFactor_Cal3Bundler, optimize_varK_FixLandmarks ) {
290 
291  vector<Point3> landmarks = genPoint3();
292  vector<GeneralCamera> cameras = genCameraVariableCalibration();
293 
294  // add measurement with noise
295  Graph graph;
296  for (size_t j = 0; j < cameras.size(); ++j) {
297  for (size_t i = 0; i < landmarks.size(); ++i) {
298  Point2 pt = cameras[j].project(landmarks[i]);
299  graph.addMeasurement(j, i, pt, sigma1);
300  }
301  }
302 
303  const size_t nMeasurements = landmarks.size() * cameras.size();
304 
305  Values values;
306  for (size_t i = 0; i < cameras.size(); ++i) {
307  const double rot_noise = 1e-5, trans_noise = 1e-3, focal_noise = 1,
308  distort_noise = 1e-3;
309  if (i == 0) {
310  values.insert(X(i), cameras[i]);
311  } else {
312 
313  Vector delta = (Vector(9) << rot_noise, rot_noise, rot_noise, // rotation
314  trans_noise, trans_noise, trans_noise, // translation
315  focal_noise, distort_noise, distort_noise // f, k1, k2
316  ).finished();
317  values.insert(X(i), cameras[i].retract(delta));
318  }
319  }
320 
321  for (size_t i = 0; i < landmarks.size(); ++i) {
322  values.insert(L(i), landmarks[i]);
323  }
324 
325  // fix X0 and all landmarks, allow only the cameras[1] to move
326  graph.addCameraConstraint(0, cameras[0]);
327  for (size_t i = 0; i < landmarks.size(); ++i)
328  graph.addPoint3Constraint(i, landmarks[i]);
329 
330  const double reproj_error = 1e-5;
331 
332  Ordering ordering = *getOrdering(cameras, landmarks);
333  LevenbergMarquardtOptimizer optimizer(graph, values, ordering);
334  Values final = optimizer.optimize();
335  EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements);
336 }
337 
338 /* ************************************************************************* */
339 TEST( GeneralSFMFactor_Cal3Bundler, optimize_varK_BA ) {
340  vector<Point3> landmarks = genPoint3();
341  vector<GeneralCamera> cameras = genCameraVariableCalibration();
342 
343  // add measurement with noise
344  Graph graph;
345  for (size_t j = 0; j < cameras.size(); ++j) {
346  for (size_t i = 0; i < landmarks.size(); ++i) {
347  Point2 pt = cameras[j].project(landmarks[i]);
348  graph.addMeasurement(j, i, pt, sigma1);
349  }
350  }
351 
352  const size_t nMeasurements = cameras.size() * landmarks.size();
353 
354  // add initial
355  const double noise = baseline * 0.1;
356  Values values;
357  for (size_t i = 0; i < cameras.size(); ++i)
358  values.insert(X(i), cameras[i]);
359 
360  // add noise only to the first landmark
361  for (size_t i = 0; i < landmarks.size(); ++i) {
362  Point3 pt(landmarks[i].x() + noise * getGaussian(),
363  landmarks[i].y() + noise * getGaussian(),
364  landmarks[i].z() + noise * getGaussian());
365  values.insert(L(i), pt);
366  }
367 
368  // Constrain position of system with the first camera constrained to the origin
369  graph.addCameraConstraint(0, cameras[0]);
370 
371  // Constrain the scale of the problem with a soft range factor of 1m between the cameras
372  graph.emplace_shared<
374  noiseModel::Isotropic::Sigma(1, 10.));
375 
376  const double reproj_error = 1e-5;
377 
378  Ordering ordering = *getOrdering(cameras, landmarks);
379  LevenbergMarquardtOptimizer optimizer(graph, values, ordering);
380  Values final = optimizer.optimize();
381  EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements);
382 }
383 
384 /* ************************************************************************* */
385 int main() {
386  TestResult tr;
387  return TestRegistry::runAllTests(tr);
388 }
389 /* ************************************************************************* */
TEST(GeneralSFMFactor_Cal3Bundler, equals)
static const SharedNoiseModel sigma1(noiseModel::Unit::Create(2))
Scalar * y
static vector< GeneralCamera > genCameraVariableCalibration()
virtual const Values & optimize()
Concept check for values that can be used in unit tests.
IsDerived< DERIVEDFACTOR > emplace_shared(Args &&... args)
Emplace a shared pointer to factor of given type.
Definition: FactorGraph.h:196
static int runAllTests(TestResult &result)
Factor Graph consisting of non-linear factors.
noiseModel::Diagonal::shared_ptr model
Vector2 Point2
Definition: Point2.h:32
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:40
Rot2 R(Rot2::fromAngle(0.1))
leaf::MyValues values
static Cal3_S2 K(500, 500, 0.1, 640/2, 480/2)
MatrixXd L
Definition: LLT_example.cpp:6
Definition: BFloat16.h:88
static double getGaussian()
static const Point3 pt(1.0, 2.0, 3.0)
DiscreteKey S(1, 2)
NonlinearFactorGraph graph
EIGEN_DEVICE_FUNC const LogReturnType log() const
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
static enum @1107 ordering
NonlinearEquality< Point3 > Point3Constraint
Base class for all pinhole cameras.
Factor Graph Values.
Eigen::VectorXd Vector
Definition: Vector.h:38
void addPoint3Constraint(int j, const Point3 &p)
#define EXPECT(condition)
Definition: Test.h:150
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme.
Array< double, 1, 3 > e(1./3., 0.5, 2.)
static vector< GeneralCamera > genCameraDefaultCalibration()
static vector< Point3 > genPoint3()
static const double baseline
gtsam::Key l1
traits
Definition: chartTesting.h:28
Calibration used by Bundler.
Definition: Cal3Bundler.h:32
Eigen::Vector2d Vector2
Definition: Vector.h:42
GeneralSFMFactor< GeneralCamera, Point3 > Projection
void addMeasurement(int i, int j, const Point2 &z, const SharedNoiseModel &model)
void addCameraConstraint(int j, const GeneralCamera &p)
Pose3 x1
Definition: testPose3.cpp:663
3D Point
float * p
static const double sigma
void insert(Key j, const Value &val)
Definition: Values.cpp:155
static double error
Definition: testRot3.cpp:37
Jet< T, N > sqrt(const Jet< T, N > &f)
Definition: jet.h:418
Vector3 Point3
Definition: Point3.h:38
#define X
Definition: icosphere.cpp:20
a general SFM factor with an unknown calibration
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
void addMeasurement(const int &i, const int &j, const Point2 &z, const SharedNoiseModel &model)
double error() const
return error in current optimizer state
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
Calibration used by Bundler.
std::ptrdiff_t j
NonlinearEquality< GeneralCamera > CameraConstraint
static std::shared_ptr< Ordering > getOrdering(const std::vector< GeneralCamera > &cameras, const std::vector< Point3 > &landmarks)
PinholeCamera< Cal3Bundler > GeneralCamera
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:741


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