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

Merge pull request #1082 from borglab/fix/sfm_reading 

Cleaned up loading SfmData
release/4.3a0
Frank Dellaert 2022-02-01 21:15:02 -05:00 committed by GitHub
parent ad0d7e4b21 f5f5f0d50d
commit fb1e3c3df1
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
20 changed files with 301 additions and 163 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();

8
examples/SFMExample_bal.cpp
View File

@ -10,7 +10,7 @@
* -------------------------------------------------------------------------- */
/**
* @file SFMExample.cpp
* @file SFMExample_bal.cpp
* @brief Solve a structure-from-motion problem from a "Bundle Adjustment in the Large" file
* @author Frank Dellaert
*/
@ -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();

4
gtsam/navigation/ImuBias.cpp
View File

@ -66,8 +66,8 @@ namespace imuBias {
// }
/// ostream operator
std::ostream& operator<<(std::ostream& os, const ConstantBias& bias) {
os << "acc = " << Point3(bias.accelerometer());
os << " gyro = " << Point3(bias.gyroscope());
os << "acc = " << bias.accelerometer().transpose();
os << " gyro = " << bias.gyroscope().transpose();
return os;
}

115
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>
@ -28,7 +29,6 @@ using std::cout;
using std::endl;
using gtsam::symbol_shorthand::P;
using gtsam::symbol_shorthand::X;
/* ************************************************************************** */
void SfmData::print(const std::string &s) const {
@ -99,14 +99,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 +135,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 +145,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 +179,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 +227,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 +235,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 +322,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;
@ -339,7 +355,7 @@ bool writeBALfromValues(const std::string &filename, const SfmData &data,
size_t nrPoses = values.count<Pose3>();
if (nrPoses == dataValues.cameras.size()) { // we only estimated camera poses
for (size_t i = 0; i < dataValues.cameras.size(); i++) { // for each camera
Pose3 pose = values.at<Pose3>(X(i));
Pose3 pose = values.at<Pose3>(i);
Cal3Bundler K = dataValues.cameras[i].calibration();
Camera camera(pose, K);
dataValues.cameras[i] = camera;
@ -387,6 +403,41 @@ 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, i, P(j));
}
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(*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;
@ -399,7 +450,7 @@ Values initialCamerasEstimate(const SfmData &db) {
Values initialCamerasAndPointsEstimate(const SfmData &db) {
Values initial;
size_t i = 0, j = 0;
for (const SfmCamera &camera : db.cameras) initial.insert((i++), camera);
for (const SfmCamera &camera : db.cameras) initial.insert(i++, camera);
for (const SfmTrack &track : db.tracks) initial.insert(P(j++), track.p);
return initial;
}

87
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,31 @@ struct SfmData {
/// The camera pose at frame index `idx`
SfmCamera camera(size_t idx) const { return cameras[idx]; }
/**
* @brief Create projection factors using keys 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.
*
* Note: pose keys are simply integer indices, points use Symbol('p', j).
*
* @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 +147,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 +169,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
@ -144,12 +179,12 @@ GTSAM_EXPORT bool writeBAL(const std::string& filename, SfmData& data);
* @param data SfM structure where the data is stored
* @param values structure where the graph values are stored (values can be
* either Pose3 or PinholeCamera<Cal3Bundler> for the cameras, and should be
* Point3 for the 3D points). Note that the current version assumes that the
* keys are "x1" for pose 1 (or "c1" for camera 1) and "l1" for landmark 1
* Point3 for the 3D points). Note: assumes that the keys are "i" for pose i
* and "Symbol::('p',j)" for landmark j.
* @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
@ -179,7 +214,10 @@ GTSAM_EXPORT Pose3 gtsam2openGL(const Rot3& R, double tx, double ty, double tz);
GTSAM_EXPORT Pose3 gtsam2openGL(const Pose3& PoseGTSAM);
/**
* @brief This function creates initial values for cameras from db
* @brief This function creates initial values for cameras from db.
*
* No symbol is used, so camera keys are simply integer indices.
*
* @param SfmData
* @return Values
*/
@ -187,6 +225,9 @@ GTSAM_EXPORT Values initialCamerasEstimate(const SfmData& db);
/**
* @brief This function creates initial values for cameras and points from db
*
* Note: Pose keys are simply integer indices, points use Symbol('p', j).
*
* @param SfmData
* @return Values
*/

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

19
gtsam/sfm/sfm.i
View File

@ -31,12 +31,23 @@ class SfmTrack {
#include <gtsam/sfm/SfmData.h>
class SfmData {
SfmData();
size_t numberCameras() const;
size_t numberTracks() const;
gtsam::PinholeCamera<gtsam::Cal3Bundler> camera(size_t idx) const;
gtsam::SfmTrack track(size_t idx) const;
static gtsam::SfmData FromBundlerFile(string filename);
static gtsam::SfmData FromBalFile(string filename);
void addTrack(const gtsam::SfmTrack& t);
void addCamera(const gtsam::SfmCamera& cam);
size_t numberTracks() const;
size_t numberCameras() const;
gtsam::SfmTrack track(size_t idx) const;
gtsam::PinholeCamera<gtsam::Cal3Bundler> camera(size_t idx) const;
gtsam::NonlinearFactorGraph generalSfmFactors(
const gtsam::SharedNoiseModel& model =
gtsam::noiseModel::Isotropic::Sigma(2, 1.0)) const;
gtsam::NonlinearFactorGraph sfmFactorGraph(
const gtsam::SharedNoiseModel& model =
gtsam::noiseModel::Isotropic::Sigma(2, 1.0),
size_t fixedCamera = 0, size_t fixedPoint = 0) const;
// enabling serialization functionality
void serialize() const;

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

@ -18,13 +18,13 @@
#include <CppUnitLite/TestHarness.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/sfm/SfmData.h>
using namespace std;
using namespace gtsam;
using gtsam::symbol_shorthand::P;
using gtsam::symbol_shorthand::X;
namespace gtsam {
GTSAM_EXPORT std::string createRewrittenFileName(const std::string& name);
@ -33,41 +33,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 +114,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,31 +160,28 @@ 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));
Values value;
Values values;
for (size_t i = 0; i < readData.numberCameras(); i++) { // for each camera
Pose3 pose = poseChange.compose(readData.cameras[i].pose());
value.insert(X(i), pose);
values.insert(i, pose);
}
for (size_t j = 0; j < readData.numberTracks(); j++) { // for each point
Point3 point = poseChange.transformFrom(readData.tracks[j].p);
value.insert(P(j), point);
values.insert(P(j), point);
}
// Write values and readData to a file
const string filenameToWrite = createRewrittenFileName(filenameToRead);
writeBALfromValues(filenameToWrite, readData, value);
writeBALfromValues(filenameToWrite, readData, values);
// 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
@ -195,11 +198,11 @@ TEST(dataSet, writeBALfromValues_Dubrovnik) {
EXPECT(assert_equal(expected, actual, 12));
Pose3 expectedPose = camera0.pose();
Pose3 actualPose = value.at<Pose3>(X(0));
Pose3 actualPose = values.at<Pose3>(0);
EXPECT(assert_equal(expectedPose, actualPose, 1e-7));
Point3 expectedPoint = track0.p;
Point3 actualPoint = value.at<Point3>(P(0));
Point3 actualPoint = values.at<Point3>(P(0));
EXPECT(assert_equal(expectedPoint, actualPoint, 1e-6));
}

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);

23
python/gtsam/examples/SFMExample_bal.py
View File

@ -15,9 +15,9 @@ import logging
import sys
import gtsam
from gtsam import (GeneralSFMFactorCal3Bundler,
from gtsam import (GeneralSFMFactorCal3Bundler, SfmData,
PriorFactorPinholeCameraCal3Bundler, PriorFactorPoint3)
from gtsam.symbol_shorthand import C, P # type: ignore
from gtsam.symbol_shorthand import P # type: ignore
from gtsam.utils import plot # type: ignore
from matplotlib import pyplot as plt
@ -26,12 +26,11 @@ logging.basicConfig(stream=sys.stdout, level=logging.INFO)
DEFAULT_BAL_DATASET = "dubrovnik-3-7-pre"
def plot_scene(scene_data: gtsam.SfmData, result: gtsam.Values) -> None:
def plot_scene(scene_data: SfmData, result: gtsam.Values) -> None:
"""Plot the SFM results."""
plot_vals = gtsam.Values()
for cam_idx in range(scene_data.numberCameras()):
plot_vals.insert(C(cam_idx),
result.atPinholeCameraCal3Bundler(C(cam_idx)).pose())
for i in range(scene_data.numberCameras()):
plot_vals.insert(i, result.atPinholeCameraCal3Bundler(i).pose())
for j in range(scene_data.numberTracks()):
plot_vals.insert(P(j), result.atPoint3(P(j)))
@ -46,7 +45,7 @@ def run(args: argparse.Namespace) -> None:
input_file = args.input_file
# Load the SfM data from file
scene_data = gtsam.readBal(input_file)
scene_data = SfmData.FromBalFile(input_file)
logging.info("read %d tracks on %d cameras\n", scene_data.numberTracks(),
scene_data.numberCameras())
@ -64,12 +63,12 @@ def run(args: argparse.Namespace) -> None:
# i represents the camera index, and uv is the 2d measurement
i, uv = track.measurement(m_idx)
# note use of shorthand symbols C and P
graph.add(GeneralSFMFactorCal3Bundler(uv, noise, C(i), P(j)))
graph.add(GeneralSFMFactorCal3Bundler(uv, noise, i, P(j)))
# Add a prior on pose x1. This indirectly specifies where the origin is.
graph.push_back(
PriorFactorPinholeCameraCal3Bundler(
C(0), scene_data.camera(0),
0, scene_data.camera(0),
gtsam.noiseModel.Isotropic.Sigma(9, 0.1)))
# Also add a prior on the position of the first landmark to fix the scale
graph.push_back(
@ -82,9 +81,9 @@ def run(args: argparse.Namespace) -> None:
i = 0
# add each PinholeCameraCal3Bundler
for cam_idx in range(scene_data.numberCameras()):
camera = scene_data.camera(cam_idx)
initial.insert(C(i), camera)
for i in range(scene_data.numberCameras()):
camera = scene_data.camera(i)
initial.insert(i, camera)
i += 1
# add each SfmTrack

8
python/gtsam/examples/SimpleRotation.py
View File

@ -31,7 +31,7 @@ def main():
- A measurement model with the correct dimensionality for the factor
"""
prior = gtsam.Rot2.fromAngle(np.deg2rad(30))
prior.print_('goal angle')
prior.print('goal angle')
model = gtsam.noiseModel.Isotropic.Sigma(dim=1, sigma=np.deg2rad(1))
key = X(1)
factor = gtsam.PriorFactorRot2(key, prior, model)
@ -48,7 +48,7 @@ def main():
"""
graph = gtsam.NonlinearFactorGraph()
graph.push_back(factor)
graph.print_('full graph')
graph.print('full graph')
"""
Step 3: Create an initial estimate
@ -65,7 +65,7 @@ def main():
"""
initial = gtsam.Values()
initial.insert(key, gtsam.Rot2.fromAngle(np.deg2rad(20)))
initial.print_('initial estimate')
initial.print('initial estimate')
"""
Step 4: Optimize
@ -77,7 +77,7 @@ def main():
with the final state of the optimization.
"""
result = gtsam.LevenbergMarquardtOptimizer(graph, initial).optimize()
result.print_('final result')
result.print('final result')
if __name__ == '__main__':

4
python/gtsam/examples/VisualISAMExample.py
View File

@ -10,8 +10,6 @@ A visualSLAM example for the structure-from-motion problem on a simulated datase
This version uses iSAM to solve the problem incrementally
"""
from __future__ import print_function
import numpy as np
import gtsam
from gtsam.examples import SFMdata
@ -94,7 +92,7 @@ def main():
current_estimate = isam.estimate()
print('*' * 50)
print('Frame {}:'.format(i))
current_estimate.print_('Current estimate: ')
current_estimate.print('Current estimate: ')
# Clear the factor graph and values for the next iteration
graph.resize(0)

72
python/gtsam/tests/test_SfmData.py
View File

@ -14,9 +14,9 @@ from __future__ import print_function
import unittest
import numpy as np
import gtsam
import numpy as np
from gtsam import Point2, Point3, SfmData, SfmTrack
from gtsam.utils.test_case import GtsamTestCase
@ -25,55 +25,97 @@ class TestSfmData(GtsamTestCase):
def setUp(self):
"""Initialize SfmData and SfmTrack"""
self.data = gtsam.SfmData()
self.data = SfmData()
# initialize SfmTrack with 3D point
self.tracks = gtsam.SfmTrack()
self.tracks = SfmTrack()
def test_tracks(self):
"""Test functions in SfmTrack"""
# measurement is of format (camera_idx, imgPoint)
# create arbitrary camera indices for two cameras
i1, i2 = 4,5
i1, i2 = 4, 5
# create arbitrary image measurements for cameras i1 and i2
uv_i1 = gtsam.Point2(12.6, 82)
uv_i1 = Point2(12.6, 82)
# translating point uv_i1 along X-axis
uv_i2 = gtsam.Point2(24.88, 82)
uv_i2 = Point2(24.88, 82)
# add measurements to the track
self.tracks.addMeasurement(i1, uv_i1)
self.tracks.addMeasurement(i2, uv_i2)
# Number of measurements in the track is 2
self.assertEqual(self.tracks.numberMeasurements(), 2)
# camera_idx in the first measurement of the track corresponds to i1
cam_idx, img_measurement = self.tracks.measurement(0)
self.assertEqual(cam_idx, i1)
np.testing.assert_array_almost_equal(
gtsam.Point3(0.,0.,0.),
Point3(0., 0., 0.),
self.tracks.point3()
)
def test_data(self):
"""Test functions in SfmData"""
# Create new track with 3 measurements
i1, i2, i3 = 3,5,6
uv_i1 = gtsam.Point2(21.23, 45.64)
i1, i2, i3 = 3, 5, 6
uv_i1 = Point2(21.23, 45.64)
# translating along X-axis
uv_i2 = gtsam.Point2(45.7, 45.64)
uv_i3 = gtsam.Point2(68.35, 45.64)
uv_i2 = Point2(45.7, 45.64)
uv_i3 = Point2(68.35, 45.64)
# add measurements and arbitrary point to the track
measurements = [(i1, uv_i1), (i2, uv_i2), (i3, uv_i3)]
pt = gtsam.Point3(1.0, 6.0, 2.0)
track2 = gtsam.SfmTrack(pt)
pt = Point3(1.0, 6.0, 2.0)
track2 = SfmTrack(pt)
track2.addMeasurement(i1, uv_i1)
track2.addMeasurement(i2, uv_i2)
track2.addMeasurement(i3, uv_i3)
self.data.addTrack(self.tracks)
self.data.addTrack(track2)
# Number of tracks in SfmData is 2
self.assertEqual(self.data.numberTracks(), 2)
# camera idx of first measurement of second track corresponds to i1
cam_idx, img_measurement = self.data.track(1).measurement(0)
self.assertEqual(cam_idx, i1)
def test_Balbianello(self):
""" Check that we can successfully read a bundler file and create a
factor graph from it
"""
# The structure where we will save the SfM data
filename = gtsam.findExampleDataFile("Balbianello.out")
sfm_data = SfmData.FromBundlerFile(filename)
# Check number of things
self.assertEqual(5, sfm_data.numberCameras())
self.assertEqual(544, sfm_data.numberTracks())
track0 = sfm_data.track(0)
self.assertEqual(3, track0.numberMeasurements())
# Check projection of a given point
self.assertEqual(0, track0.measurement(0)[0])
camera0 = sfm_data.camera(0)
expected = camera0.project(track0.point3())
actual = track0.measurement(0)[1]
self.gtsamAssertEquals(expected, actual, 1)
# We share *one* noiseModel between all projection factors
model = gtsam.noiseModel.Isotropic.Sigma(
2, 1.0) # one pixel in u and v
# Convert to NonlinearFactorGraph
graph = sfm_data.sfmFactorGraph(model)
self.assertEqual(1419, graph.size()) # regression
# Get initial estimate
values = gtsam.initialCamerasAndPointsEstimate(sfm_data)
self.assertEqual(549, values.size()) # regression
if __name__ == '__main__':
unittest.main()

11
python/gtsam/utils/visual_data_generator.py
View File

@ -1,12 +1,12 @@
from __future__ import print_function
from typing import Tuple
import math
import numpy as np
from math import pi
from typing import Tuple
import gtsam
from gtsam import Point3, Pose3, PinholeCameraCal3_S2, Cal3_S2
import numpy as np
from gtsam import Cal3_S2, PinholeCameraCal3_S2, Point3, Pose3
class Options:
@ -36,7 +36,7 @@ class GroundTruth:
self.cameras = [Pose3()] * nrCameras
self.points = [Point3(0, 0, 0)] * nrPoints
def print_(self, s="") -> None:
def print(self, s: str = "") -> None:
print(s)
print("K = ", self.K)
print("Cameras: ", len(self.cameras))
@ -88,7 +88,8 @@ def generate_data(options) -> Tuple[Data, GroundTruth]:
r = 10
for j in range(len(truth.points)):
theta = j * 2 * pi / nrPoints
truth.points[j] = Point3(r * math.cos(theta), r * math.sin(theta), 0)
truth.points[j] = Point3(
r * math.cos(theta), r * math.sin(theta), 0)
else: # 3D landmarks as vertices of a cube
truth.points = [
Point3(10, 10, 10), Point3(-10, 10, 10),

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();