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
12/**
13 * @file testGeneralSFMFactorB.cpp
14 * @author Frank Dellaert
15 * @brief test general SFM class, with nonlinear optimization and BAL files
16 */
17
18#include <gtsam/sfm/SfmData.h>
19#include <gtsam/slam/dataset.h>
20#include <gtsam/slam/GeneralSFMFactor.h>
21#include <gtsam/geometry/Point3.h>
22#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
23#include <gtsam/nonlinear/NonlinearFactorGraph.h>
24#include <gtsam/nonlinear/NonlinearOptimizer.h>
25#include <gtsam/nonlinear/Values.h>
26#include <gtsam/inference/Symbol.h>
27#include <gtsam/linear/NoiseModel.h>
28
29#include <CppUnitLite/Failure.h>
30#include <CppUnitLite/Test.h>
31#include <CppUnitLite/TestRegistry.h>
32#include <CppUnitLite/TestResult.h>
33#include <stddef.h>
34#include <stdexcept>
35#include <string>
36
37using namespace std;
38using namespace gtsam;
39
40typedef GeneralSFMFactor<PinholeCamera<Cal3Bundler>, Point3> sfmFactor;
41using symbol_shorthand::P;
42
43/* ************************************************************************* */
44TEST(PinholeCamera, BAL) {
45 string filename = findExampleDataFile(name: "dubrovnik-3-7-pre");
46 SfmData db = SfmData::FromBalFile(filename);
47
48 SharedNoiseModel unit2 = noiseModel::Unit::Create(dim: 2);
49 NonlinearFactorGraph graph;
50
51 for (size_t j = 0; j < db.numberTracks(); j++) {
52 for (const SfmMeasurement& m: db.tracks[j].measurements)
53 graph.emplace_shared<sfmFactor>(args: m.second, args&: unit2, args: m.first, args: P(j));
54 }
55
56 Values initial = initialCamerasAndPointsEstimate(db);
57
58 LevenbergMarquardtOptimizer lm(graph, initial);
59
60 Values actual = lm.optimize();
61 double actualError = graph.error(values: actual);
62 EXPECT_DOUBLES_EQUAL(0.0199833, actualError, 1e-5);
63}
64
65/* ************************************************************************* */
66int main() {
67 TestResult tr;
68 return TestRegistry::runAllTests(result&: tr);
69}
70/* ************************************************************************* */
71