12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Made reading static named constructors

release/4.3a0
Frank Dellaert 2022-01-31 12:29:13 -05:00
parent 762e8097bb
commit 9dfe52d0b6
14 changed files with 189 additions and 104 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
examples/SFMExampleExpressions_bal.cpp
View File

@ -28,7 +28,8 @@
// Header order is close to far
#include <gtsam/inference/Symbol.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/slam/dataset.h> // for loading BAL datasets !
#include <gtsam/sfm/SfmData.h> // for loading BAL datasets !
#include <gtsam/slam/dataset.h>
#include <vector>
using namespace std;
@ -46,8 +47,7 @@ int main(int argc, char* argv[]) {
if (argc > 1) filename = string(argv[1]);
// Load the SfM data from file
SfmData mydata;
readBAL(filename, mydata);
SfmData mydata = SfmData::FromBalFile(filename);
cout << boost::format("read %1% tracks on %2% cameras\n") %
mydata.numberTracks() % mydata.numberCameras();

6
examples/SFMExample_bal.cpp
View File

@ -20,7 +20,8 @@
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/slam/GeneralSFMFactor.h>
#include <gtsam/slam/dataset.h> // for loading BAL datasets !
#include <gtsam/sfm/SfmData.h> // for loading BAL datasets !
#include <gtsam/slam/dataset.h>
#include <vector>
using namespace std;
@ -41,8 +42,7 @@ int main (int argc, char* argv[]) {
if (argc>1) filename = string(argv[1]);
// Load the SfM data from file
SfmData mydata;
readBAL(filename, mydata);
SfmData mydata = SfmData::FromBalFile(filename);
cout << boost::format("read %1% tracks on %2% cameras\n") % mydata.numberTracks() % mydata.numberCameras();
// Create a factor graph

6
examples/SFMExample_bal_COLAMD_METIS.cpp
View File

@ -22,7 +22,8 @@
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/slam/GeneralSFMFactor.h>
#include <gtsam/slam/dataset.h> // for loading BAL datasets !
#include <gtsam/sfm/SfmData.h> // for loading BAL datasets !
#include <gtsam/slam/dataset.h>
#include <gtsam/base/timing.h>
@ -45,8 +46,7 @@ int main(int argc, char* argv[]) {
if (argc > 1) filename = string(argv[1]);
// Load the SfM data from file
SfmData mydata;
readBAL(filename, mydata);
SfmData mydata = SfmData::FromBalFile(filename);
cout << boost::format("read %1% tracks on %2% cameras\n") %
mydata.numberTracks() % mydata.numberCameras();

111
gtsam/sfm/SfmData.cpp
View File

@ -18,6 +18,7 @@
#include <gtsam/inference/Symbol.h>
#include <gtsam/sfm/SfmData.h>
#include <gtsam/slam/GeneralSFMFactor.h>
#include <fstream>
#include <iostream>
@ -99,14 +100,16 @@ Pose3 gtsam2openGL(const Pose3 &PoseGTSAM) {
}
/* ************************************************************************** */
bool readBundler(const std::string &filename, SfmData &data) {
SfmData SfmData::FromBundlerFile(const std::string &filename) {
// Load the data file
std::ifstream is(filename.c_str(), std::ifstream::in);
if (!is) {
cout << "Error in readBundler: can not find the file!!" << endl;
return false;
throw std::runtime_error(
"Error in FromBundlerFile: can not find the file!!");
}
SfmData sfmData;
// Ignore the first line
char aux[500];
is.getline(aux, 500);
@ -133,9 +136,8 @@ bool readBundler(const std::string &filename, SfmData &data) {
// Check for all-zero R, in which case quit
if (r11 == 0 && r12 == 0 && r13 == 0) {
cout << "Error in readBundler: zero rotation matrix for pose " << i
<< endl;
return false;
throw std::runtime_error(
"Error in FromBundlerFile: zero rotation matrix");
}
// Get the translation vector
@ -144,11 +146,11 @@ bool readBundler(const std::string &filename, SfmData &data) {
Pose3 pose = openGL2gtsam(R, tx, ty, tz);
data.cameras.emplace_back(pose, K);
sfmData.cameras.emplace_back(pose, K);
}
// Get the information for the 3D points
data.tracks.reserve(nrPoints);
sfmData.tracks.reserve(nrPoints);
for (size_t j = 0; j < nrPoints; j++) {
SfmTrack track;
@ -178,34 +180,34 @@ bool readBundler(const std::string &filename, SfmData &data) {
track.siftIndices.emplace_back(cam_idx, point_idx);
}
data.tracks.push_back(track);
sfmData.tracks.push_back(track);
}
is.close();
return true;
return sfmData;
}
/* ************************************************************************** */
bool readBAL(const std::string &filename, SfmData &data) {
SfmData SfmData::FromBalFile(const std::string &filename) {
// Load the data file
std::ifstream is(filename.c_str(), std::ifstream::in);
if (!is) {
cout << "Error in readBAL: can not find the file!!" << endl;
return false;
throw std::runtime_error("Error in FromBalFile: can not find the file!!");
}
SfmData sfmData;
// Get the number of camera poses and 3D points
size_t nrPoses, nrPoints, nrObservations;
is >> nrPoses >> nrPoints >> nrObservations;
data.tracks.resize(nrPoints);
sfmData.tracks.resize(nrPoints);
// Get the information for the observations
for (size_t k = 0; k < nrObservations; k++) {
size_t i = 0, j = 0;
float u, v;
is >> i >> j >> u >> v;
data.tracks[j].measurements.emplace_back(i, Point2(u, -v));
sfmData.tracks[j].measurements.emplace_back(i, Point2(u, -v));
}
// Get the information for the camera poses
@ -226,7 +228,7 @@ bool readBAL(const std::string &filename, SfmData &data) {
is >> f >> k1 >> k2;
Cal3Bundler K(f, k1, k2);
data.cameras.emplace_back(pose, K);
sfmData.cameras.emplace_back(pose, K);
}
// Get the information for the 3D points
@ -234,26 +236,18 @@ bool readBAL(const std::string &filename, SfmData &data) {
// Get the 3D position
float x, y, z;
is >> x >> y >> z;
SfmTrack &track = data.tracks[j];
SfmTrack &track = sfmData.tracks[j];
track.p = Point3(x, y, z);
track.r = 0.4f;
track.g = 0.4f;
track.b = 0.4f;
}
is.close();
return true;
return sfmData;
}
/* ************************************************************************** */
SfmData readBal(const std::string &filename) {
SfmData data;
readBAL(filename, data);
return data;
}
/* ************************************************************************** */
bool writeBAL(const std::string &filename, SfmData &data) {
bool writeBAL(const std::string &filename, const SfmData &data) {
// Open the output file
std::ofstream os;
os.open(filename.c_str());
@ -329,9 +323,32 @@ bool writeBAL(const std::string &filename, SfmData &data) {
return true;
}
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
bool readBundler(const std::string &filename, SfmData &data) {
try {
data = SfmData::FromBundlerFile(filename);
return true;
} catch (const std::exception & /* e */) {
return false;
}
}
bool readBAL(const std::string &filename, SfmData &data) {
try {
data = SfmData::FromBalFile(filename);
return true;
} catch (const std::exception & /* e */) {
return false;
}
}
#endif
SfmData readBal(const std::string &filename) {
return SfmData::FromBalFile(filename);
}
/* ************************************************************************** */
bool writeBALfromValues(const std::string &filename, const SfmData &data,
Values &values) {
const Values &values) {
using Camera = PinholeCamera<Cal3Bundler>;
SfmData dataValues = data;
@ -387,6 +404,42 @@ bool writeBALfromValues(const std::string &filename, const SfmData &data,
return writeBAL(filename, dataValues);
}
/* ************************************************************************** */
NonlinearFactorGraph SfmData::generalSfmFactors(
const SharedNoiseModel &model) const {
using ProjectionFactor = GeneralSFMFactor<SfmCamera, Point3>;
NonlinearFactorGraph factors;
size_t j = 0;
for (const SfmTrack &track : tracks) {
for (const SfmMeasurement &m : track.measurements) {
size_t i = m.first;
Point2 uv = m.second;
factors.emplace_shared<ProjectionFactor>(
uv, model, X(i), P(j)); // note use of shorthand symbols X and P
}
j += 1;
}
return factors;
}
/* ************************************************************************** */
NonlinearFactorGraph SfmData::sfmFactorGraph(
const SharedNoiseModel &model, boost::optional<size_t> fixedCamera,
boost::optional<size_t> fixedPoint) const {
using ProjectionFactor = GeneralSFMFactor<SfmCamera, Point3>;
NonlinearFactorGraph graph = generalSfmFactors(model);
using noiseModel::Constrained;
if (fixedCamera) {
graph.addPrior(X(*fixedCamera), cameras[0], Constrained::All(9));
}
if (fixedPoint) {
graph.addPrior(P(*fixedPoint), tracks[0].p, Constrained::All(3));
}
return graph;
}
/* ************************************************************************** */
Values initialCamerasEstimate(const SfmData &db) {
Values initial;

73
gtsam/sfm/SfmData.h
View File

@ -20,6 +20,7 @@
#include <gtsam/geometry/Cal3Bundler.h>
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/sfm/SfmTrack.h>
@ -35,11 +36,31 @@ typedef PinholeCamera<Cal3Bundler> SfmCamera;
* @brief SfmData stores a bunch of SfmTracks
* @addtogroup sfm
*/
struct SfmData {
struct GTSAM_EXPORT SfmData {
std::vector<SfmCamera> cameras; ///< Set of cameras
std::vector<SfmTrack> tracks; ///< Sparse set of points
/// @name Create from file
/// @{
/**
* @brief Parses a bundler output file and return result as SfmData instance.
* @param filename The name of the bundler file
* @param data SfM structure where the data is stored
* @return true if the parsing was successful, false otherwise
*/
static SfmData FromBundlerFile(const std::string& filename);
/**
* @brief Parse a "Bundle Adjustment in the Large" (BAL) file and return
* result as SfmData instance.
* @param filename The name of the BAL file.
* @return SfM structure where the data is stored.
*/
static SfmData FromBalFile(const std::string& filename);
/// @}
/// @name Standard Interface
/// @{
@ -61,6 +82,29 @@ struct SfmData {
/// The camera pose at frame index `idx`
SfmCamera camera(size_t idx) const { return cameras[idx]; }
/**
* @brief Create projection factors using keys X(i) and P(j)
*
* @param model a noise model for projection errors
* @return NonlinearFactorGraph
*/
NonlinearFactorGraph generalSfmFactors(
const SharedNoiseModel& model = noiseModel::Isotropic::Sigma(2,
1.0)) const;
/**
* @brief Create factor graph with projection factors and gauge fix.
*
* @param model a noise model for projection errors
* @param fixedCamera which camera to fix, if any (use boost::none if none)
* @param fixedPoint which point to fix, if any (use boost::none if none)
* @return NonlinearFactorGraph
*/
NonlinearFactorGraph sfmFactorGraph(
const SharedNoiseModel& model = noiseModel::Isotropic::Sigma(2, 1.0),
boost::optional<size_t> fixedCamera = 0,
boost::optional<size_t> fixedPoint = 0) const;
/// @}
/// @name Testable
/// @{
@ -101,23 +145,12 @@ struct SfmData {
template <>
struct traits<SfmData> : public Testable<SfmData> {};
/**
* @brief This function parses a bundler output file and stores the data into a
* SfmData structure
* @param filename The name of the bundler file
* @param data SfM structure where the data is stored
* @return true if the parsing was successful, false otherwise
*/
GTSAM_EXPORT bool readBundler(const std::string& filename, SfmData& data);
/**
* @brief This function parses a "Bundle Adjustment in the Large" (BAL) file and
* stores the data into a SfmData structure
* @param filename The name of the BAL file
* @param data SfM structure where the data is stored
* @return true if the parsing was successful, false otherwise
*/
GTSAM_EXPORT bool readBAL(const std::string& filename, SfmData& data);
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
GTSAM_EXPORT bool GTSAM_DEPRECATED readBundler(const std::string& filename,
SfmData& data);
GTSAM_EXPORT bool GTSAM_DEPRECATED readBAL(const std::string& filename,
SfmData& data);
#endif
/**
* @brief This function parses a "Bundle Adjustment in the Large" (BAL) file and
@ -134,7 +167,7 @@ GTSAM_EXPORT SfmData readBal(const std::string& filename);
* @param data SfM structure where the data is stored
* @return true if the parsing was successful, false otherwise
*/
GTSAM_EXPORT bool writeBAL(const std::string& filename, SfmData& data);
GTSAM_EXPORT bool writeBAL(const std::string& filename, const SfmData& data);
/**
* @brief This function writes a "Bundle Adjustment in the Large" (BAL) file
@ -149,7 +182,7 @@ GTSAM_EXPORT bool writeBAL(const std::string& filename, SfmData& data);
* @return true if the parsing was successful, false otherwise
*/
GTSAM_EXPORT bool writeBALfromValues(const std::string& filename,
const SfmData& data, Values& values);
const SfmData& data, const Values& values);
/**
* @brief This function converts an openGL camera pose to an GTSAM camera pose

2
gtsam/sfm/SfmTrack.h
View File

@ -38,7 +38,7 @@ typedef std::pair<size_t, size_t> SiftIndex;
* @brief An SfmTrack stores SfM measurements grouped in a track
* @addtogroup sfm
*/
struct SfmTrack {
struct GTSAM_EXPORT SfmTrack {
Point3 p; ///< 3D position of the point
float r, g, b; ///< RGB color of the 3D point

3
gtsam/sfm/sfm.i
View File

@ -31,6 +31,9 @@ class SfmTrack {
#include <gtsam/sfm/SfmData.h>
class SfmData {
SfmData();
gtsam::SfmData FromBundlerFile(string filename);
gtsam::SfmData FromBalFile(string filename);
size_t numberCameras() const;
size_t numberTracks() const;
gtsam::PinholeCamera<gtsam::Cal3Bundler> camera(size_t idx) const;

52
gtsam/sfm/tests/testSfmData.cpp
View File

@ -18,6 +18,7 @@
#include <CppUnitLite/TestHarness.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/sfm/SfmData.h>
using namespace std;
@ -33,41 +34,50 @@ GTSAM_EXPORT std::string findExampleDataFile(const std::string& name);
/* ************************************************************************* */
TEST(dataSet, Balbianello) {
///< The structure where we will save the SfM data
// The structure where we will save the SfM data
const string filename = findExampleDataFile("Balbianello");
SfmData mydata;
CHECK(readBundler(filename, mydata));
SfmData sfmData = SfmData::FromBundlerFile(filename);
// Check number of things
EXPECT_LONGS_EQUAL(5, mydata.numberCameras());
EXPECT_LONGS_EQUAL(544, mydata.numberTracks());
const SfmTrack& track0 = mydata.tracks[0];
EXPECT_LONGS_EQUAL(5, sfmData.numberCameras());
EXPECT_LONGS_EQUAL(544, sfmData.numberTracks());
const SfmTrack& track0 = sfmData.tracks[0];
EXPECT_LONGS_EQUAL(3, track0.numberMeasurements());
// Check projection of a given point
EXPECT_LONGS_EQUAL(0, track0.measurements[0].first);
const SfmCamera& camera0 = mydata.cameras[0];
const SfmCamera& camera0 = sfmData.cameras[0];
Point2 expected = camera0.project(track0.p),
actual = track0.measurements[0].second;
EXPECT(assert_equal(expected, actual, 1));
// We share *one* noiseModel between all projection factors
auto model = noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v
// Convert to NonlinearFactorGraph
NonlinearFactorGraph graph = sfmData.sfmFactorGraph(model);
EXPECT_LONGS_EQUAL(1419, graph.size()); // regression
// Get initial estimate
Values values = initialCamerasAndPointsEstimate(sfmData);
EXPECT_LONGS_EQUAL(549, values.size()); // regression
}
/* ************************************************************************* */
TEST(dataSet, readBAL_Dubrovnik) {
///< The structure where we will save the SfM data
// The structure where we will save the SfM data
const string filename = findExampleDataFile("dubrovnik-3-7-pre");
SfmData mydata;
CHECK(readBAL(filename, mydata));
SfmData sfmData = SfmData::FromBalFile(filename);
// Check number of things
EXPECT_LONGS_EQUAL(3, mydata.numberCameras());
EXPECT_LONGS_EQUAL(7, mydata.numberTracks());
const SfmTrack& track0 = mydata.tracks[0];
EXPECT_LONGS_EQUAL(3, sfmData.numberCameras());
EXPECT_LONGS_EQUAL(7, sfmData.numberTracks());
const SfmTrack& track0 = sfmData.tracks[0];
EXPECT_LONGS_EQUAL(3, track0.numberMeasurements());
// Check projection of a given point
EXPECT_LONGS_EQUAL(0, track0.measurements[0].first);
const SfmCamera& camera0 = mydata.cameras[0];
const SfmCamera& camera0 = sfmData.cameras[0];
Point2 expected = camera0.project(track0.p),
actual = track0.measurements[0].second;
EXPECT(assert_equal(expected, actual, 12));
@ -105,18 +115,15 @@ TEST(dataSet, gtsam2openGL) {
/* ************************************************************************* */
TEST(dataSet, writeBAL_Dubrovnik) {
///< Read a file using the unit tested readBAL
const string filenameToRead = findExampleDataFile("dubrovnik-3-7-pre");
SfmData readData;
readBAL(filenameToRead, readData);
SfmData readData = SfmData::FromBalFile(filenameToRead);
// Write readData to file filenameToWrite
const string filenameToWrite = createRewrittenFileName(filenameToRead);
CHECK(writeBAL(filenameToWrite, readData));
// Read what we wrote
SfmData writtenData;
CHECK(readBAL(filenameToWrite, writtenData));
SfmData writtenData = SfmData::FromBalFile(filenameToWrite);
// Check that what we read is the same as what we wrote
EXPECT_LONGS_EQUAL(readData.numberCameras(), writtenData.numberCameras());
@ -154,10 +161,8 @@ TEST(dataSet, writeBAL_Dubrovnik) {
/* ************************************************************************* */
TEST(dataSet, writeBALfromValues_Dubrovnik) {
///< Read a file using the unit tested readBAL
const string filenameToRead = findExampleDataFile("dubrovnik-3-7-pre");
SfmData readData;
readBAL(filenameToRead, readData);
SfmData readData = SfmData::FromBalFile(filenameToRead);
Pose3 poseChange =
Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10), Point3(0.3, 0.1, 0.3));
@ -177,8 +182,7 @@ TEST(dataSet, writeBALfromValues_Dubrovnik) {
writeBALfromValues(filenameToWrite, readData, value);
// Read the file we wrote
SfmData writtenData;
readBAL(filenameToWrite, writtenData);
SfmData writtenData = SfmData::FromBalFile(filenameToWrite);
// Check that the reprojection errors are the same and the poses are correct
// Check number of things

9
gtsam/slam/tests/testEssentialMatrixFactor.cpp
View File

@ -15,6 +15,7 @@
#include <gtsam/nonlinear/expressionTesting.h>
#include <gtsam/nonlinear/factorTesting.h>
#include <gtsam/slam/EssentialMatrixFactor.h>
#include <gtsam/sfm/SfmData.h>
#include <gtsam/slam/dataset.h>
using namespace std::placeholders;
@ -34,8 +35,7 @@ gtsam::Rot3 cRb = gtsam::Rot3(bX, bZ, -bY).inverse();
namespace example1 {
const string filename = findExampleDataFile("18pointExample1.txt");
SfmData data;
bool readOK = readBAL(filename, data);
SfmData data = SfmData::FromBalFile(filename);
Rot3 c1Rc2 = data.cameras[1].pose().rotation();
Point3 c1Tc2 = data.cameras[1].pose().translation();
// TODO: maybe default value not good; assert with 0th
@ -53,8 +53,6 @@ Vector vB(size_t i) { return EssentialMatrix::Homogeneous(pB(i)); }
//*************************************************************************
TEST(EssentialMatrixFactor, testData) {
CHECK(readOK);
// Check E matrix
Matrix expected(3, 3);
expected << 0, 0, 0, 0, 0, -0.1, 0.1, 0, 0;
@ -538,8 +536,7 @@ TEST(EssentialMatrixFactor4, minimizationWithStrongCal3BundlerPrior) {
namespace example2 {
const string filename = findExampleDataFile("5pointExample2.txt");
SfmData data;
bool readOK = readBAL(filename, data);
SfmData data = SfmData::FromBalFile(filename);
Rot3 aRb = data.cameras[1].pose().rotation();
Point3 aTb = data.cameras[1].pose().translation();
EssentialMatrix trueE(aRb, Unit3(aTb));

7
gtsam/slam/tests/testSerializationDataset.cpp
View File

@ -16,6 +16,7 @@
* @date Jan 1, 2021
*/
#include <gtsam/sfm/SfmData.h>
#include <gtsam/slam/dataset.h>
#include <gtsam/base/serializationTestHelpers.h>
@ -29,8 +30,7 @@ using namespace gtsam::serializationTestHelpers;
TEST(dataSet, sfmDataSerialization) {
// Test the serialization of SfmData
const string filename = findExampleDataFile("dubrovnik-3-7-pre");
SfmData mydata;
CHECK(readBAL(filename, mydata));
SfmData mydata = SfmData::FromBalFile(filename);
// round-trip equality check on serialization and subsequent deserialization
EXPECT(equalsObj(mydata));
@ -42,8 +42,7 @@ TEST(dataSet, sfmDataSerialization) {
TEST(dataSet, sfmTrackSerialization) {
// Test the serialization of SfmTrack
const string filename = findExampleDataFile("dubrovnik-3-7-pre");
SfmData mydata;
CHECK(readBAL(filename, mydata));
SfmData mydata = SfmData::FromBalFile(filename);
SfmTrack track = mydata.track(0);

5
tests/testGeneralSFMFactorB.cpp
View File

@ -15,6 +15,7 @@
* @brief test general SFM class, with nonlinear optimization and BAL files
*/
#include <gtsam/sfm/SfmData.h>
#include <gtsam/slam/dataset.h>
#include <gtsam/slam/GeneralSFMFactor.h>
#include <gtsam/geometry/Point3.h>
@ -42,9 +43,7 @@ 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!");
SfmData db = SfmData::FromBalFile(filename);
SharedNoiseModel unit2 = noiseModel::Unit::Create(2);
NonlinearFactorGraph graph;

5
tests/testTranslationRecovery.cpp
View File

@ -18,6 +18,7 @@
#include <CppUnitLite/TestHarness.h>
#include <gtsam/sfm/TranslationRecovery.h>
#include <gtsam/sfm/SfmData.h>
#include <gtsam/slam/dataset.h>
using namespace std;
@ -42,9 +43,7 @@ Unit3 GetDirectionFromPoses(const Values& poses,
// sets up an optimization problem for the three unknown translations.
TEST(TranslationRecovery, BAL) {
const string filename = findExampleDataFile("dubrovnik-3-7-pre");
SfmData db;
bool success = readBAL(filename, db);
if (!success) throw runtime_error("Could not access file!");
SfmData db = SfmData::FromBalFile(filename);
// Get camera poses, as Values
size_t j = 0;

6
timing/timeSFMBAL.h
View File

@ -16,7 +16,7 @@
* @date July 5, 2015
*/
#include <gtsam/slam/dataset.h>
#include <gtsam/sfm/SfmData.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
@ -54,9 +54,7 @@ SfmData preamble(int argc, char* argv[]) {
filename = argv[argc - 1];
else
filename = findExampleDataFile("dubrovnik-16-22106-pre");
bool success = readBAL(filename, db);
if (!success) throw runtime_error("Could not access file!");
return db;
return SfmData::FromBalFile(filename);
}
// Create ordering and optimize

2
timing/timeSFMBALautodiff.cpp
View File

@ -59,7 +59,7 @@ int main(int argc, char* argv[]) {
Values initial;
size_t i = 0, j = 0;
for (const SfmCamera& camera: db.cameras) {
// readBAL converts to GTSAM format, so we need to convert back !
// SfmData::FromBalFile converts to GTSAM format, so we need to convert back !
Pose3 openGLpose = gtsam2openGL(camera.pose());
Vector9 v9;
v9 << Pose3::Logmap(openGLpose), camera.calibration();