72 lines
2.2 KiB
C++
72 lines
2.2 KiB
C++
/* ----------------------------------------------------------------------------
|
|
|
|
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
|
* Atlanta, Georgia 30332-0415
|
|
* All Rights Reserved
|
|
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
|
|
|
* See LICENSE for the license information
|
|
|
|
* -------------------------------------------------------------------------- */
|
|
|
|
/**
|
|
* @file testGeneralSFMFactorB.cpp
|
|
* @author Frank Dellaert
|
|
* @brief test general SFM class, with nonlinear optimization and BAL files
|
|
*/
|
|
|
|
#include <gtsam/slam/dataset.h>
|
|
#include <gtsam/slam/GeneralSFMFactor.h>
|
|
#include <gtsam/geometry/Point3.h>
|
|
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
|
|
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
|
#include <gtsam/nonlinear/NonlinearOptimizer.h>
|
|
#include <gtsam/nonlinear/Values.h>
|
|
#include <gtsam/inference/Symbol.h>
|
|
#include <gtsam/linear/NoiseModel.h>
|
|
|
|
#include <CppUnitLite/Failure.h>
|
|
#include <CppUnitLite/Test.h>
|
|
#include <CppUnitLite/TestRegistry.h>
|
|
#include <CppUnitLite/TestResult.h>
|
|
#include <stddef.h>
|
|
#include <stdexcept>
|
|
#include <string>
|
|
|
|
using namespace std;
|
|
using namespace gtsam;
|
|
|
|
typedef GeneralSFMFactor<PinholeCamera<Cal3Bundler>, Point3> sfmFactor;
|
|
using symbol_shorthand::P;
|
|
|
|
/* ************************************************************************* */
|
|
TEST(PinholeCamera, BAL) {
|
|
string filename = findExampleDataFile("dubrovnik-3-7-pre");
|
|
SfmData db;
|
|
bool success = readBAL(filename, db);
|
|
if (!success) throw runtime_error("Could not access file!");
|
|
|
|
SharedNoiseModel unit2 = noiseModel::Unit::Create(2);
|
|
NonlinearFactorGraph graph;
|
|
|
|
for (size_t j = 0; j < db.number_tracks(); j++) {
|
|
for (const SfmMeasurement& m: db.tracks[j].measurements)
|
|
graph.emplace_shared<sfmFactor>(m.second, unit2, m.first, P(j));
|
|
}
|
|
|
|
Values initial = initialCamerasAndPointsEstimate(db);
|
|
|
|
LevenbergMarquardtOptimizer lm(graph, initial);
|
|
|
|
Values actual = lm.optimize();
|
|
double actualError = graph.error(actual);
|
|
EXPECT_DOUBLES_EQUAL(0.0199833, actualError, 1e-5);
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
int main() {
|
|
TestResult tr;
|
|
return TestRegistry::runAllTests(tr);
|
|
}
|
|
/* ************************************************************************* */
|