follow Google style guide naming convention for Sfm related data structs

release/4.3a0
Varun Agrawal 2020-03-06 17:59:09 -05:00
parent 719975022c
commit 75d5409d78
17 changed files with 125 additions and 121 deletions

View File

@ -31,19 +31,19 @@ void createExampleBALFile(const string& filename, const vector<Point3>& P,
Cal3Bundler()) { Cal3Bundler()) {
// Class that will gather all data // Class that will gather all data
SfM_Data data; SfmData data;
// Create two cameras // Create two cameras
Rot3 aRb = Rot3::Yaw(M_PI_2); Rot3 aRb = Rot3::Yaw(M_PI_2);
Point3 aTb(0.1, 0, 0); Point3 aTb(0.1, 0, 0);
Pose3 identity, aPb(aRb, aTb); Pose3 identity, aPb(aRb, aTb);
data.cameras.push_back(SfM_Camera(pose1, K)); data.cameras.push_back(SfmCamera(pose1, K));
data.cameras.push_back(SfM_Camera(pose2, K)); data.cameras.push_back(SfmCamera(pose2, K));
for(const Point3& p: P) { for(const Point3& p: P) {
// Create the track // Create the track
SfM_Track track; SfmTrack track;
track.p = p; track.p = p;
track.r = 1; track.r = 1;
track.g = 1; track.g = 1;

View File

@ -37,7 +37,7 @@ using namespace noiseModel;
using symbol_shorthand::C; using symbol_shorthand::C;
using symbol_shorthand::P; using symbol_shorthand::P;
// An SfM_Camera is defined in datase.h as a camera with unknown Cal3Bundler calibration // An SfmCamera is defined in datase.h as a camera with unknown Cal3Bundler calibration
// and has a total of 9 free parameters // and has a total of 9 free parameters
/* ************************************************************************* */ /* ************************************************************************* */
@ -49,7 +49,7 @@ int main(int argc, char* argv[]) {
filename = string(argv[1]); filename = string(argv[1]);
// Load the SfM data from file // Load the SfM data from file
SfM_Data mydata; SfmData mydata;
readBAL(filename, mydata); readBAL(filename, mydata);
cout cout
<< boost::format("read %1% tracks on %2% cameras\n") << boost::format("read %1% tracks on %2% cameras\n")
@ -60,9 +60,9 @@ int main(int argc, char* argv[]) {
// Here we don't use a PriorFactor but directly the ExpressionFactor class // Here we don't use a PriorFactor but directly the ExpressionFactor class
// First, we create an expression to the pose from the first camera // First, we create an expression to the pose from the first camera
Expression<SfM_Camera> camera0_(C(0)); Expression<SfmCamera> camera0_(C(0));
// Then, to get its pose: // Then, to get its pose:
Pose3_ pose0_(&SfM_Camera::getPose, camera0_); Pose3_ pose0_(&SfmCamera::getPose, camera0_);
// Finally, we say it should be equal to first guess // Finally, we say it should be equal to first guess
graph.addExpressionFactor(pose0_, mydata.cameras[0].pose(), graph.addExpressionFactor(pose0_, mydata.cameras[0].pose(),
noiseModel::Isotropic::Sigma(6, 0.1)); noiseModel::Isotropic::Sigma(6, 0.1));
@ -78,16 +78,16 @@ int main(int argc, char* argv[]) {
// Simulated measurements from each camera pose, adding them to the factor graph // Simulated measurements from each camera pose, adding them to the factor graph
size_t j = 0; size_t j = 0;
for(const SfM_Track& track: mydata.tracks) { for(const SfmTrack& track: mydata.tracks) {
// Leaf expression for j^th point // Leaf expression for j^th point
Point3_ point_('p', j); Point3_ point_('p', j);
for(const SfM_Measurement& m: track.measurements) { for(const SfmMeasurement& m: track.measurements) {
size_t i = m.first; size_t i = m.first;
Point2 uv = m.second; Point2 uv = m.second;
// Leaf expression for i^th camera // Leaf expression for i^th camera
Expression<SfM_Camera> camera_(C(i)); Expression<SfmCamera> camera_(C(i));
// Below an expression for the prediction of the measurement: // Below an expression for the prediction of the measurement:
Point2_ predict_ = project2<SfM_Camera>(camera_, point_); Point2_ predict_ = project2<SfmCamera>(camera_, point_);
// Again, here we use an ExpressionFactor // Again, here we use an ExpressionFactor
graph.addExpressionFactor(predict_, uv, noise); graph.addExpressionFactor(predict_, uv, noise);
} }
@ -98,9 +98,9 @@ int main(int argc, char* argv[]) {
Values initial; Values initial;
size_t i = 0; size_t i = 0;
j = 0; j = 0;
for(const SfM_Camera& camera: mydata.cameras) for(const SfmCamera& camera: mydata.cameras)
initial.insert(C(i++), camera); initial.insert(C(i++), camera);
for(const SfM_Track& track: mydata.tracks) for(const SfmTrack& track: mydata.tracks)
initial.insert(P(j++), track.p); initial.insert(P(j++), track.p);
/* Optimize the graph and print results */ /* Optimize the graph and print results */

View File

@ -32,7 +32,7 @@ using symbol_shorthand::P;
// We will be using a projection factor that ties a SFM_Camera to a 3D point. // We will be using a projection factor that ties a SFM_Camera to a 3D point.
// An SFM_Camera is defined in datase.h as a camera with unknown Cal3Bundler calibration // An SFM_Camera is defined in datase.h as a camera with unknown Cal3Bundler calibration
// and has a total of 9 free parameters // and has a total of 9 free parameters
typedef GeneralSFMFactor<SfM_Camera,Point3> MyFactor; typedef GeneralSFMFactor<SfmCamera,Point3> MyFactor;
/* ************************************************************************* */ /* ************************************************************************* */
int main (int argc, char* argv[]) { int main (int argc, char* argv[]) {
@ -42,7 +42,7 @@ int main (int argc, char* argv[]) {
if (argc>1) filename = string(argv[1]); if (argc>1) filename = string(argv[1]);
// Load the SfM data from file // Load the SfM data from file
SfM_Data mydata; SfmData mydata;
readBAL(filename, mydata); readBAL(filename, mydata);
cout << boost::format("read %1% tracks on %2% cameras\n") % mydata.number_tracks() % mydata.number_cameras(); cout << boost::format("read %1% tracks on %2% cameras\n") % mydata.number_tracks() % mydata.number_cameras();
@ -55,8 +55,8 @@ int main (int argc, char* argv[]) {
// Add measurements to the factor graph // Add measurements to the factor graph
size_t j = 0; size_t j = 0;
for(const SfM_Track& track: mydata.tracks) { for(const SfmTrack& track: mydata.tracks) {
for(const SfM_Measurement& m: track.measurements) { for(const SfmMeasurement& m: track.measurements) {
size_t i = m.first; size_t i = m.first;
Point2 uv = m.second; Point2 uv = m.second;
graph.emplace_shared<MyFactor>(uv, noise, C(i), P(j)); // note use of shorthand symbols C and P graph.emplace_shared<MyFactor>(uv, noise, C(i), P(j)); // note use of shorthand symbols C and P
@ -66,14 +66,14 @@ int main (int argc, char* argv[]) {
// Add a prior on pose x1. This indirectly specifies where the origin is. // Add a prior on pose x1. This indirectly specifies where the origin is.
// and a prior on the position of the first landmark to fix the scale // and a prior on the position of the first landmark to fix the scale
graph.emplace_shared<PriorFactor<SfM_Camera> >(C(0), mydata.cameras[0], noiseModel::Isotropic::Sigma(9, 0.1)); graph.emplace_shared<PriorFactor<SfmCamera> >(C(0), mydata.cameras[0], noiseModel::Isotropic::Sigma(9, 0.1));
graph.emplace_shared<PriorFactor<Point3> > (P(0), mydata.tracks[0].p, noiseModel::Isotropic::Sigma(3, 0.1)); graph.emplace_shared<PriorFactor<Point3> > (P(0), mydata.tracks[0].p, noiseModel::Isotropic::Sigma(3, 0.1));
// Create initial estimate // Create initial estimate
Values initial; Values initial;
size_t i = 0; j = 0; size_t i = 0; j = 0;
for(const SfM_Camera& camera: mydata.cameras) initial.insert(C(i++), camera); for(const SfmCamera& camera: mydata.cameras) initial.insert(C(i++), camera);
for(const SfM_Track& track: mydata.tracks) initial.insert(P(j++), track.p); for(const SfmTrack& track: mydata.tracks) initial.insert(P(j++), track.p);
/* Optimize the graph and print results */ /* Optimize the graph and print results */
Values result; Values result;

View File

@ -37,7 +37,7 @@ using symbol_shorthand::P;
// We will be using a projection factor that ties a SFM_Camera to a 3D point. // We will be using a projection factor that ties a SFM_Camera to a 3D point.
// An SFM_Camera is defined in datase.h as a camera with unknown Cal3Bundler calibration // An SFM_Camera is defined in datase.h as a camera with unknown Cal3Bundler calibration
// and has a total of 9 free parameters // and has a total of 9 free parameters
typedef GeneralSFMFactor<SfM_Camera,Point3> MyFactor; typedef GeneralSFMFactor<SfmCamera,Point3> MyFactor;
/* ************************************************************************* */ /* ************************************************************************* */
int main (int argc, char* argv[]) { int main (int argc, char* argv[]) {
@ -47,7 +47,7 @@ int main (int argc, char* argv[]) {
if (argc>1) filename = string(argv[1]); if (argc>1) filename = string(argv[1]);
// Load the SfM data from file // Load the SfM data from file
SfM_Data mydata; SfmData mydata;
readBAL(filename, mydata); readBAL(filename, mydata);
cout << boost::format("read %1% tracks on %2% cameras\n") % mydata.number_tracks() % mydata.number_cameras(); cout << boost::format("read %1% tracks on %2% cameras\n") % mydata.number_tracks() % mydata.number_cameras();
@ -60,8 +60,8 @@ int main (int argc, char* argv[]) {
// Add measurements to the factor graph // Add measurements to the factor graph
size_t j = 0; size_t j = 0;
for(const SfM_Track& track: mydata.tracks) { for(const SfmTrack& track: mydata.tracks) {
for(const SfM_Measurement& m: track.measurements) { for(const SfmMeasurement& m: track.measurements) {
size_t i = m.first; size_t i = m.first;
Point2 uv = m.second; Point2 uv = m.second;
graph.emplace_shared<MyFactor>(uv, noise, C(i), P(j)); // note use of shorthand symbols C and P graph.emplace_shared<MyFactor>(uv, noise, C(i), P(j)); // note use of shorthand symbols C and P
@ -71,14 +71,14 @@ int main (int argc, char* argv[]) {
// Add a prior on pose x1. This indirectly specifies where the origin is. // Add a prior on pose x1. This indirectly specifies where the origin is.
// and a prior on the position of the first landmark to fix the scale // and a prior on the position of the first landmark to fix the scale
graph.emplace_shared<PriorFactor<SfM_Camera> >(C(0), mydata.cameras[0], noiseModel::Isotropic::Sigma(9, 0.1)); graph.emplace_shared<PriorFactor<SfmCamera> >(C(0), mydata.cameras[0], noiseModel::Isotropic::Sigma(9, 0.1));
graph.emplace_shared<PriorFactor<Point3> >(P(0), mydata.tracks[0].p, noiseModel::Isotropic::Sigma(3, 0.1)); graph.emplace_shared<PriorFactor<Point3> >(P(0), mydata.tracks[0].p, noiseModel::Isotropic::Sigma(3, 0.1));
// Create initial estimate // Create initial estimate
Values initial; Values initial;
size_t i = 0; j = 0; size_t i = 0; j = 0;
for(const SfM_Camera& camera: mydata.cameras) initial.insert(C(i++), camera); for(const SfmCamera& camera: mydata.cameras) initial.insert(C(i++), camera);
for(const SfM_Track& track: mydata.tracks) initial.insert(P(j++), track.p); for(const SfmTrack& track: mydata.tracks) initial.insert(P(j++), track.p);
/** --------------- COMPARISON -----------------------**/ /** --------------- COMPARISON -----------------------**/
/** ----------------------------------------------------**/ /** ----------------------------------------------------**/

View File

@ -2724,18 +2724,18 @@ virtual class EssentialMatrixFactor : gtsam::NoiseModelFactor {
}; };
#include <gtsam/slam/dataset.h> #include <gtsam/slam/dataset.h>
class SfM_Track { class SfmTrack {
size_t number_measurements() const; size_t number_measurements() const;
pair<size_t, gtsam::Point2> measurement(size_t idx) const; pair<size_t, gtsam::Point2> measurement(size_t idx) const;
pair<size_t, size_t> SIFT_index(size_t idx) const; pair<size_t, size_t> siftIndex(size_t idx) const;
}; };
class SfM_Data { class SfmData {
size_t number_cameras() const; size_t number_cameras() const;
size_t number_tracks() const; size_t number_tracks() const;
//TODO(Varun) Need to fix issue #237 first before this can work //TODO(Varun) Need to fix issue #237 first before this can work
// gtsam::PinholeCamera<gtsam::Cal3Bundler> camera(size_t idx) const; // gtsam::PinholeCamera<gtsam::Cal3Bundler> camera(size_t idx) const;
gtsam::SfM_Track track(size_t idx) const; gtsam::SfmTrack track(size_t idx) const;
}; };
string findExampleDataFile(string name); string findExampleDataFile(string name);

View File

@ -646,7 +646,7 @@ Pose3 gtsam2openGL(const Pose3& PoseGTSAM) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
bool readBundler(const string& filename, SfM_Data &data) { bool readBundler(const string& filename, SfmData &data) {
// Load the data file // Load the data file
ifstream is(filename.c_str(), ifstream::in); ifstream is(filename.c_str(), ifstream::in);
if (!is) { if (!is) {
@ -697,7 +697,7 @@ bool readBundler(const string& filename, SfM_Data &data) {
// Get the information for the 3D points // Get the information for the 3D points
data.tracks.reserve(nrPoints); data.tracks.reserve(nrPoints);
for (size_t j = 0; j < nrPoints; j++) { for (size_t j = 0; j < nrPoints; j++) {
SfM_Track track; SfmTrack track;
// Get the 3D position // Get the 3D position
float x, y, z; float x, y, z;
@ -733,7 +733,7 @@ bool readBundler(const string& filename, SfM_Data &data) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
bool readBAL(const string& filename, SfM_Data &data) { bool readBAL(const string& filename, SfmData &data) {
// Load the data file // Load the data file
ifstream is(filename.c_str(), ifstream::in); ifstream is(filename.c_str(), ifstream::in);
if (!is) { if (!is) {
@ -781,7 +781,7 @@ bool readBAL(const string& filename, SfM_Data &data) {
// Get the 3D position // Get the 3D position
float x, y, z; float x, y, z;
is >> x >> y >> z; is >> x >> y >> z;
SfM_Track& track = data.tracks[j]; SfmTrack& track = data.tracks[j];
track.p = Point3(x, y, z); track.p = Point3(x, y, z);
track.r = 0.4f; track.r = 0.4f;
track.g = 0.4f; track.g = 0.4f;
@ -793,7 +793,7 @@ bool readBAL(const string& filename, SfM_Data &data) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
bool writeBAL(const string& filename, SfM_Data &data) { bool writeBAL(const string& filename, SfmData &data) {
// Open the output file // Open the output file
ofstream os; ofstream os;
os.open(filename.c_str()); os.open(filename.c_str());
@ -815,7 +815,7 @@ bool writeBAL(const string& filename, SfM_Data &data) {
os << endl; os << endl;
for (size_t j = 0; j < data.number_tracks(); j++) { // for each 3D point j for (size_t j = 0; j < data.number_tracks(); j++) { // for each 3D point j
const SfM_Track& track = data.tracks[j]; const SfmTrack& track = data.tracks[j];
for (size_t k = 0; k < track.number_measurements(); k++) { // for each observation of the 3D point j for (size_t k = 0; k < track.number_measurements(); k++) { // for each observation of the 3D point j
size_t i = track.measurements[k].first; // camera id size_t i = track.measurements[k].first; // camera id
@ -866,12 +866,12 @@ bool writeBAL(const string& filename, SfM_Data &data) {
return true; return true;
} }
bool writeBALfromValues(const string& filename, const SfM_Data &data, bool writeBALfromValues(const string& filename, const SfmData &data,
Values& values) { Values& values) {
using Camera = PinholeCamera<Cal3Bundler>; using Camera = PinholeCamera<Cal3Bundler>;
SfM_Data dataValues = data; SfmData dataValues = data;
// Store poses or cameras in SfM_Data // Store poses or cameras in SfmData
size_t nrPoses = values.count<Pose3>(); size_t nrPoses = values.count<Pose3>();
if (nrPoses == dataValues.number_cameras()) { // we only estimated camera poses if (nrPoses == dataValues.number_cameras()) { // we only estimated camera poses
for (size_t i = 0; i < dataValues.number_cameras(); i++) { // for each camera for (size_t i = 0; i < dataValues.number_cameras(); i++) { // for each camera
@ -899,7 +899,7 @@ bool writeBALfromValues(const string& filename, const SfM_Data &data,
} }
} }
// Store 3D points in SfM_Data // Store 3D points in SfmData
size_t nrPoints = values.count<Point3>(), nrTracks = dataValues.number_tracks(); size_t nrPoints = values.count<Point3>(), nrTracks = dataValues.number_tracks();
if (nrPoints != nrTracks) { if (nrPoints != nrTracks) {
cout cout
@ -921,24 +921,24 @@ bool writeBALfromValues(const string& filename, const SfM_Data &data,
} }
} }
// Write SfM_Data to file // Write SfmData to file
return writeBAL(filename, dataValues); return writeBAL(filename, dataValues);
} }
Values initialCamerasEstimate(const SfM_Data& db) { Values initialCamerasEstimate(const SfmData& db) {
Values initial; Values initial;
size_t i = 0; // NO POINTS: j = 0; size_t i = 0; // NO POINTS: j = 0;
for(const SfM_Camera& camera: db.cameras) for(const SfmCamera& camera: db.cameras)
initial.insert(i++, camera); initial.insert(i++, camera);
return initial; return initial;
} }
Values initialCamerasAndPointsEstimate(const SfM_Data& db) { Values initialCamerasAndPointsEstimate(const SfmData& db) {
Values initial; Values initial;
size_t i = 0, j = 0; size_t i = 0, j = 0;
for(const SfM_Camera& camera: db.cameras) for(const SfmCamera& camera: db.cameras)
initial.insert((i++), camera); initial.insert((i++), camera);
for(const SfM_Track& track: db.tracks) for(const SfmTrack& track: db.tracks)
initial.insert(P(j++), track.p); initial.insert(P(j++), track.p);
return initial; return initial;
} }

View File

@ -166,41 +166,39 @@ GTSAM_EXPORT std::map<Key, Pose3> parse3DPoses(const std::string& filename);
GTSAM_EXPORT GraphAndValues load3D(const std::string& filename); GTSAM_EXPORT GraphAndValues load3D(const std::string& filename);
/// A measurement with its camera index /// A measurement with its camera index
typedef std::pair<size_t, Point2> SfM_Measurement; typedef std::pair<size_t, Point2> SfmMeasurement;
/// SfM_Track /// SfmTrack
typedef std::pair<size_t, size_t> SIFT_Index; typedef std::pair<size_t, size_t> SiftIndex;
/// Define the structure for the 3D points /// Define the structure for the 3D points
struct SfM_Track { struct SfmTrack {
/// Construct empty track SfmTrack(): p(0,0,0) {}
SfM_Track(): p(0,0,0) {}
Point3 p; ///< 3D position of the point Point3 p; ///< 3D position of the point
float r, g, b; ///< RGB color of the 3D point float r, g, b; ///< RGB color of the 3D point
std::vector<SfM_Measurement> measurements; ///< The 2D image projections (id,(u,v)) std::vector<SfmMeasurement> measurements; ///< The 2D image projections (id,(u,v))
std::vector<SIFT_Index> siftIndices; std::vector<SiftIndex> siftIndices;
/// Total number of measurements in this track /// Total number of measurements in this track
size_t number_measurements() const { size_t number_measurements() const {
return measurements.size(); return measurements.size();
} }
/// Get the measurement (camera index, Point2) at pose index `idx` /// Get the measurement (camera index, Point2) at pose index `idx`
SfM_Measurement measurement(size_t idx) const { SfmMeasurement measurement(size_t idx) const {
return measurements[idx]; return measurements[idx];
} }
/// Get the SIFT feature index corresponding to the measurement at `idx` /// Get the SIFT feature index corresponding to the measurement at `idx`
SIFT_Index SIFT_index(size_t idx) const { SiftIndex siftIndex(size_t idx) const {
return siftIndices[idx]; return siftIndices[idx];
} }
}; };
/// Define the structure for the camera poses /// Define the structure for the camera poses
typedef PinholeCamera<Cal3Bundler> SfM_Camera; typedef PinholeCamera<Cal3Bundler> SfmCamera;
/// Define the structure for SfM data /// Define the structure for SfM data
struct SfM_Data { struct SfmData {
std::vector<SfM_Camera> cameras; ///< Set of cameras std::vector<SfmCamera> cameras; ///< Set of cameras
std::vector<SfM_Track> tracks; ///< Sparse set of points std::vector<SfmTrack> tracks; ///< Sparse set of points
/// The number of camera poses
size_t number_cameras() const { size_t number_cameras() const {
return cameras.size(); return cameras.size();
} }
@ -209,48 +207,54 @@ struct SfM_Data {
return tracks.size(); return tracks.size();
} }
/// The camera pose at frame index `idx` /// The camera pose at frame index `idx`
SfM_Camera camera(size_t idx) const { SfmCamera camera(size_t idx) const {
return cameras[idx]; return cameras[idx];
} }
/// The track formed by series of landmark measurements /// The track formed by series of landmark measurements
SfM_Track track(size_t idx) const { SfmTrack track(size_t idx) const {
return tracks[idx]; return tracks[idx];
} }
}; };
/// Alias for backwards compatibility /// Aliases for backwards compatibility
typedef SfM_Data SfM_data; #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
typedef SfmMeasurement SfM_Measurement;
typedef SiftIndex SIFT_Index;
typedef SfmTrack SfM_Track;
typedef SfmCamera SfM_Camera;
typedef SfmData SfM_data;
#endif
/** /**
* @brief This function parses a bundler output file and stores the data into a * @brief This function parses a bundler output file and stores the data into a
* SfM_Data structure * SfmData structure
* @param filename The name of the bundler file * @param filename The name of the bundler file
* @param data SfM structure where the data is stored * @param data SfM structure where the data is stored
* @return true if the parsing was successful, false otherwise * @return true if the parsing was successful, false otherwise
*/ */
GTSAM_EXPORT bool readBundler(const std::string& filename, SfM_Data &data); 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 * @brief This function parses a "Bundle Adjustment in the Large" (BAL) file and stores the data into a
* SfM_Data structure * SfmData structure
* @param filename The name of the BAL file * @param filename The name of the BAL file
* @param data SfM structure where the data is stored * @param data SfM structure where the data is stored
* @return true if the parsing was successful, false otherwise * @return true if the parsing was successful, false otherwise
*/ */
GTSAM_EXPORT bool readBAL(const std::string& filename, SfM_Data &data); GTSAM_EXPORT bool readBAL(const std::string& filename, SfmData &data);
/** /**
* @brief This function writes a "Bundle Adjustment in the Large" (BAL) file from a * @brief This function writes a "Bundle Adjustment in the Large" (BAL) file from a
* SfM_Data structure * SfmData structure
* @param filename The name of the BAL file to write * @param filename The name of the BAL file to write
* @param data SfM structure where the data is stored * @param data SfM structure where the data is stored
* @return true if the parsing was successful, false otherwise * @return true if the parsing was successful, false otherwise
*/ */
GTSAM_EXPORT bool writeBAL(const std::string& filename, SfM_Data &data); GTSAM_EXPORT bool writeBAL(const std::string& filename, SfmData &data);
/** /**
* @brief This function writes a "Bundle Adjustment in the Large" (BAL) file from a * @brief This function writes a "Bundle Adjustment in the Large" (BAL) file from a
* SfM_Data structure and a value structure (measurements are the same as the SfM input data, * SfmData structure and a value structure (measurements are the same as the SfM input data,
* while camera poses and values are read from Values) * while camera poses and values are read from Values)
* @param filename The name of the BAL file to write * @param filename The name of the BAL file to write
* @param data SfM structure where the data is stored * @param data SfM structure where the data is stored
@ -260,7 +264,7 @@ GTSAM_EXPORT bool writeBAL(const std::string& filename, SfM_Data &data);
* @return true if the parsing was successful, false otherwise * @return true if the parsing was successful, false otherwise
*/ */
GTSAM_EXPORT bool writeBALfromValues(const std::string& filename, GTSAM_EXPORT bool writeBALfromValues(const std::string& filename,
const SfM_Data &data, Values& values); const SfmData &data, Values& values);
/** /**
* @brief This function converts an openGL camera pose to an GTSAM camera pose * @brief This function converts an openGL camera pose to an GTSAM camera pose
@ -291,16 +295,16 @@ 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
* @param SfM_Data * @param SfmData
* @return Values * @return Values
*/ */
GTSAM_EXPORT Values initialCamerasEstimate(const SfM_Data& db); GTSAM_EXPORT Values initialCamerasEstimate(const SfmData& db);
/** /**
* @brief This function creates initial values for cameras and points from db * @brief This function creates initial values for cameras and points from db
* @param SfM_Data * @param SfmData
* @return Values * @return Values
*/ */
GTSAM_EXPORT Values initialCamerasAndPointsEstimate(const SfM_Data& db); GTSAM_EXPORT Values initialCamerasAndPointsEstimate(const SfmData& db);
} // namespace gtsam } // namespace gtsam

View File

@ -106,18 +106,18 @@ 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"); const string filename = findExampleDataFile("Balbianello");
SfM_Data mydata; SfmData mydata;
CHECK(readBundler(filename, mydata)); CHECK(readBundler(filename, mydata));
// Check number of things // Check number of things
EXPECT_LONGS_EQUAL(5,mydata.number_cameras()); EXPECT_LONGS_EQUAL(5,mydata.number_cameras());
EXPECT_LONGS_EQUAL(544,mydata.number_tracks()); EXPECT_LONGS_EQUAL(544,mydata.number_tracks());
const SfM_Track& track0 = mydata.tracks[0]; const SfmTrack& track0 = mydata.tracks[0];
EXPECT_LONGS_EQUAL(3,track0.number_measurements()); EXPECT_LONGS_EQUAL(3,track0.number_measurements());
// Check projection of a given point // Check projection of a given point
EXPECT_LONGS_EQUAL(0,track0.measurements[0].first); EXPECT_LONGS_EQUAL(0,track0.measurements[0].first);
const SfM_Camera& camera0 = mydata.cameras[0]; const SfmCamera& camera0 = mydata.cameras[0];
Point2 expected = camera0.project(track0.p), actual = track0.measurements[0].second; Point2 expected = camera0.project(track0.p), actual = track0.measurements[0].second;
EXPECT(assert_equal(expected,actual,1)); EXPECT(assert_equal(expected,actual,1));
} }
@ -389,18 +389,18 @@ 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"); const string filename = findExampleDataFile("dubrovnik-3-7-pre");
SfM_Data mydata; SfmData mydata;
CHECK(readBAL(filename, mydata)); CHECK(readBAL(filename, mydata));
// Check number of things // Check number of things
EXPECT_LONGS_EQUAL(3,mydata.number_cameras()); EXPECT_LONGS_EQUAL(3,mydata.number_cameras());
EXPECT_LONGS_EQUAL(7,mydata.number_tracks()); EXPECT_LONGS_EQUAL(7,mydata.number_tracks());
const SfM_Track& track0 = mydata.tracks[0]; const SfmTrack& track0 = mydata.tracks[0];
EXPECT_LONGS_EQUAL(3,track0.number_measurements()); EXPECT_LONGS_EQUAL(3,track0.number_measurements());
// Check projection of a given point // Check projection of a given point
EXPECT_LONGS_EQUAL(0,track0.measurements[0].first); EXPECT_LONGS_EQUAL(0,track0.measurements[0].first);
const SfM_Camera& camera0 = mydata.cameras[0]; const SfmCamera& camera0 = mydata.cameras[0];
Point2 expected = camera0.project(track0.p), actual = track0.measurements[0].second; Point2 expected = camera0.project(track0.p), actual = track0.measurements[0].second;
EXPECT(assert_equal(expected,actual,12)); EXPECT(assert_equal(expected,actual,12));
} }
@ -444,7 +444,7 @@ TEST( dataSet, writeBAL_Dubrovnik)
{ {
///< Read a file using the unit tested readBAL ///< Read a file using the unit tested readBAL
const string filenameToRead = findExampleDataFile("dubrovnik-3-7-pre"); const string filenameToRead = findExampleDataFile("dubrovnik-3-7-pre");
SfM_Data readData; SfmData readData;
readBAL(filenameToRead, readData); readBAL(filenameToRead, readData);
// Write readData to file filenameToWrite // Write readData to file filenameToWrite
@ -452,7 +452,7 @@ TEST( dataSet, writeBAL_Dubrovnik)
CHECK(writeBAL(filenameToWrite, readData)); CHECK(writeBAL(filenameToWrite, readData));
// Read what we wrote // Read what we wrote
SfM_Data writtenData; SfmData writtenData;
CHECK(readBAL(filenameToWrite, writtenData)); CHECK(readBAL(filenameToWrite, writtenData));
// Check that what we read is the same as what we wrote // Check that what we read is the same as what we wrote
@ -467,8 +467,8 @@ TEST( dataSet, writeBAL_Dubrovnik)
for (size_t j = 0; j < readData.number_tracks(); j++){ for (size_t j = 0; j < readData.number_tracks(); j++){
// check point // check point
SfM_Track expectedTrack = writtenData.tracks[j]; SfmTrack expectedTrack = writtenData.tracks[j];
SfM_Track actualTrack = readData.tracks[j]; SfmTrack actualTrack = readData.tracks[j];
Point3 expectedPoint = expectedTrack.p; Point3 expectedPoint = expectedTrack.p;
Point3 actualPoint = actualTrack.p; Point3 actualPoint = actualTrack.p;
EXPECT(assert_equal(expectedPoint,actualPoint)); EXPECT(assert_equal(expectedPoint,actualPoint));
@ -492,7 +492,7 @@ TEST( dataSet, writeBALfromValues_Dubrovnik){
///< Read a file using the unit tested readBAL ///< Read a file using the unit tested readBAL
const string filenameToRead = findExampleDataFile("dubrovnik-3-7-pre"); const string filenameToRead = findExampleDataFile("dubrovnik-3-7-pre");
SfM_Data readData; SfmData readData;
readBAL(filenameToRead, readData); readBAL(filenameToRead, readData);
Pose3 poseChange = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.3,0.1,0.3)); Pose3 poseChange = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.3,0.1,0.3));
@ -514,19 +514,19 @@ TEST( dataSet, writeBALfromValues_Dubrovnik){
writeBALfromValues(filenameToWrite, readData, value); writeBALfromValues(filenameToWrite, readData, value);
// Read the file we wrote // Read the file we wrote
SfM_Data writtenData; SfmData writtenData;
readBAL(filenameToWrite, writtenData); readBAL(filenameToWrite, writtenData);
// Check that the reprojection errors are the same and the poses are correct // Check that the reprojection errors are the same and the poses are correct
// Check number of things // Check number of things
EXPECT_LONGS_EQUAL(3,writtenData.number_cameras()); EXPECT_LONGS_EQUAL(3,writtenData.number_cameras());
EXPECT_LONGS_EQUAL(7,writtenData.number_tracks()); EXPECT_LONGS_EQUAL(7,writtenData.number_tracks());
const SfM_Track& track0 = writtenData.tracks[0]; const SfmTrack& track0 = writtenData.tracks[0];
EXPECT_LONGS_EQUAL(3,track0.number_measurements()); EXPECT_LONGS_EQUAL(3,track0.number_measurements());
// Check projection of a given point // Check projection of a given point
EXPECT_LONGS_EQUAL(0,track0.measurements[0].first); EXPECT_LONGS_EQUAL(0,track0.measurements[0].first);
const SfM_Camera& camera0 = writtenData.cameras[0]; const SfmCamera& camera0 = writtenData.cameras[0];
Point2 expected = camera0.project(track0.p), actual = track0.measurements[0].second; Point2 expected = camera0.project(track0.p), actual = track0.measurements[0].second;
EXPECT(assert_equal(expected,actual,12)); EXPECT(assert_equal(expected,actual,12));

View File

@ -34,7 +34,7 @@ gtsam::Rot3 cRb = gtsam::Rot3(bX, bZ, -bY).inverse();
namespace example1 { namespace example1 {
const string filename = findExampleDataFile("5pointExample1.txt"); const string filename = findExampleDataFile("5pointExample1.txt");
SfM_Data data; SfmData data;
bool readOK = readBAL(filename, data); bool readOK = readBAL(filename, data);
Rot3 c1Rc2 = data.cameras[1].pose().rotation(); Rot3 c1Rc2 = data.cameras[1].pose().rotation();
Point3 c1Tc2 = data.cameras[1].pose().translation(); Point3 c1Tc2 = data.cameras[1].pose().translation();
@ -357,7 +357,7 @@ TEST (EssentialMatrixFactor3, minimization) {
namespace example2 { namespace example2 {
const string filename = findExampleDataFile("5pointExample2.txt"); const string filename = findExampleDataFile("5pointExample2.txt");
SfM_Data data; SfmData data;
bool readOK = readBAL(filename, data); bool readOK = readBAL(filename, data);
Rot3 aRb = data.cameras[1].pose().rotation(); Rot3 aRb = data.cameras[1].pose().rotation();
Point3 aTb = data.cameras[1].pose().translation(); Point3 aTb = data.cameras[1].pose().translation();

View File

@ -281,14 +281,14 @@ TEST(SmartProjectionFactor, perturbPoseAndOptimizeFromSfM_tracks ) {
KeyVector views {c1, c2, c3}; KeyVector views {c1, c2, c3};
SfM_Track track1; SfmTrack track1;
for (size_t i = 0; i < 3; ++i) { for (size_t i = 0; i < 3; ++i) {
track1.measurements.emplace_back(i + 1, measurements_cam1.at(i)); track1.measurements.emplace_back(i + 1, measurements_cam1.at(i));
} }
SmartFactor::shared_ptr smartFactor1(new SmartFactor(unit2)); SmartFactor::shared_ptr smartFactor1(new SmartFactor(unit2));
smartFactor1->add(track1); smartFactor1->add(track1);
SfM_Track track2; SfmTrack track2;
for (size_t i = 0; i < 3; ++i) { for (size_t i = 0; i < 3; ++i) {
track2.measurements.emplace_back(i + 1, measurements_cam2.at(i)); track2.measurements.emplace_back(i + 1, measurements_cam2.at(i));
} }

View File

@ -42,7 +42,7 @@ using symbol_shorthand::P;
/* ************************************************************************* */ /* ************************************************************************* */
TEST(PinholeCamera, BAL) { TEST(PinholeCamera, BAL) {
string filename = findExampleDataFile("dubrovnik-3-7-pre"); string filename = findExampleDataFile("dubrovnik-3-7-pre");
SfM_Data db; SfmData db;
bool success = readBAL(filename, db); bool success = readBAL(filename, db);
if (!success) throw runtime_error("Could not access file!"); if (!success) throw runtime_error("Could not access file!");
@ -50,7 +50,7 @@ TEST(PinholeCamera, BAL) {
NonlinearFactorGraph graph; NonlinearFactorGraph graph;
for (size_t j = 0; j < db.number_tracks(); j++) { for (size_t j = 0; j < db.number_tracks(); j++) {
for (const SfM_Measurement& m: db.tracks[j].measurements) for (const SfmMeasurement& m: db.tracks[j].measurements)
graph.emplace_shared<sfmFactor>(m.second, unit2, m.first, P(j)); graph.emplace_shared<sfmFactor>(m.second, unit2, m.first, P(j));
} }

View File

@ -32,12 +32,12 @@ typedef GeneralSFMFactor<Camera, Point3> SfmFactor;
int main(int argc, char* argv[]) { int main(int argc, char* argv[]) {
// parse options and read BAL file // parse options and read BAL file
SfM_Data db = preamble(argc, argv); SfmData db = preamble(argc, argv);
// Build graph using conventional GeneralSFMFactor // Build graph using conventional GeneralSFMFactor
NonlinearFactorGraph graph; NonlinearFactorGraph graph;
for (size_t j = 0; j < db.number_tracks(); j++) { for (size_t j = 0; j < db.number_tracks(); j++) {
for (const SfM_Measurement& m: db.tracks[j].measurements) { for (const SfmMeasurement& m: db.tracks[j].measurements) {
size_t i = m.first; size_t i = m.first;
Point2 z = m.second; Point2 z = m.second;
graph.emplace_shared<SfmFactor>(z, gNoiseModel, C(i), P(j)); graph.emplace_shared<SfmFactor>(z, gNoiseModel, C(i), P(j));
@ -46,9 +46,9 @@ int main(int argc, char* argv[]) {
Values initial; Values initial;
size_t i = 0, j = 0; size_t i = 0, j = 0;
for (const SfM_Camera& camera: db.cameras) for (const SfmCamera& camera: db.cameras)
initial.insert(C(i++), camera); initial.insert(C(i++), camera);
for (const SfM_Track& track: db.tracks) for (const SfmTrack& track: db.tracks)
initial.insert(P(j++), track.p); initial.insert(P(j++), track.p);
return optimize(db, graph, initial); return optimize(db, graph, initial);

View File

@ -38,7 +38,7 @@ static bool gUseSchur = true;
static SharedNoiseModel gNoiseModel = noiseModel::Unit::Create(2); static SharedNoiseModel gNoiseModel = noiseModel::Unit::Create(2);
// parse options and read BAL file // parse options and read BAL file
SfM_Data preamble(int argc, char* argv[]) { SfmData preamble(int argc, char* argv[]) {
// primitive argument parsing: // primitive argument parsing:
if (argc > 2) { if (argc > 2) {
if (strcmp(argv[1], "--colamd")) if (strcmp(argv[1], "--colamd"))
@ -48,7 +48,7 @@ SfM_Data preamble(int argc, char* argv[]) {
} }
// Load BAL file // Load BAL file
SfM_Data db; SfmData db;
string filename; string filename;
if (argc > 1) if (argc > 1)
filename = argv[argc - 1]; filename = argv[argc - 1];
@ -60,7 +60,7 @@ SfM_Data preamble(int argc, char* argv[]) {
} }
// Create ordering and optimize // Create ordering and optimize
int optimize(const SfM_Data& db, const NonlinearFactorGraph& graph, int optimize(const SfmData& db, const NonlinearFactorGraph& graph,
const Values& initial, bool separateCalibration = false) { const Values& initial, bool separateCalibration = false) {
using symbol_shorthand::P; using symbol_shorthand::P;

View File

@ -38,14 +38,14 @@ typedef PinholeCamera<Cal3Bundler> Camera;
int main(int argc, char* argv[]) { int main(int argc, char* argv[]) {
// parse options and read BAL file // parse options and read BAL file
SfM_Data db = preamble(argc, argv); SfmData db = preamble(argc, argv);
AdaptAutoDiff<SnavelyProjection, 2, 9, 3> snavely; AdaptAutoDiff<SnavelyProjection, 2, 9, 3> snavely;
// Build graph // Build graph
NonlinearFactorGraph graph; NonlinearFactorGraph graph;
for (size_t j = 0; j < db.number_tracks(); j++) { for (size_t j = 0; j < db.number_tracks(); j++) {
for (const SfM_Measurement& m: db.tracks[j].measurements) { for (const SfmMeasurement& m: db.tracks[j].measurements) {
size_t i = m.first; size_t i = m.first;
Point2 z = m.second; Point2 z = m.second;
Expression<Vector9> camera_(C(i)); Expression<Vector9> camera_(C(i));
@ -58,14 +58,14 @@ int main(int argc, char* argv[]) {
Values initial; Values initial;
size_t i = 0, j = 0; size_t i = 0, j = 0;
for (const SfM_Camera& camera: db.cameras) { for (const SfmCamera& camera: db.cameras) {
// readBAL converts to GTSAM format, so we need to convert back ! // readBAL converts to GTSAM format, so we need to convert back !
Pose3 openGLpose = gtsam2openGL(camera.pose()); Pose3 openGLpose = gtsam2openGL(camera.pose());
Vector9 v9; Vector9 v9;
v9 << Pose3::Logmap(openGLpose), camera.calibration(); v9 << Pose3::Logmap(openGLpose), camera.calibration();
initial.insert(C(i++), v9); initial.insert(C(i++), v9);
} }
for (const SfM_Track& track: db.tracks) { for (const SfmTrack& track: db.tracks) {
Vector3 v3 = track.p; Vector3 v3 = track.p;
initial.insert(P(j++), v3); initial.insert(P(j++), v3);
} }

View File

@ -29,12 +29,12 @@ using namespace gtsam;
int main(int argc, char* argv[]) { int main(int argc, char* argv[]) {
// parse options and read BAL file // parse options and read BAL file
SfM_Data db = preamble(argc, argv); SfmData db = preamble(argc, argv);
// Build graph using conventional GeneralSFMFactor // Build graph using conventional GeneralSFMFactor
NonlinearFactorGraph graph; NonlinearFactorGraph graph;
for (size_t j = 0; j < db.number_tracks(); j++) { for (size_t j = 0; j < db.number_tracks(); j++) {
for (const SfM_Measurement& m: db.tracks[j].measurements) { for (const SfmMeasurement& m: db.tracks[j].measurements) {
size_t i = m.first; size_t i = m.first;
Point2 z = m.second; Point2 z = m.second;
Pose3_ camTnav_(C(i)); Pose3_ camTnav_(C(i));
@ -49,12 +49,12 @@ int main(int argc, char* argv[]) {
Values initial; Values initial;
size_t i = 0, j = 0; size_t i = 0, j = 0;
for (const SfM_Camera& camera: db.cameras) { for (const SfmCamera& camera: db.cameras) {
initial.insert(C(i), camera.pose().inverse()); // inverse !!! initial.insert(C(i), camera.pose().inverse()); // inverse !!!
initial.insert(K(i), camera.calibration()); initial.insert(K(i), camera.calibration());
i += 1; i += 1;
} }
for (const SfM_Track& track: db.tracks) for (const SfmTrack& track: db.tracks)
initial.insert(P(j++), track.p); initial.insert(P(j++), track.p);
bool separateCalibration = true; bool separateCalibration = true;

View File

@ -29,13 +29,13 @@ using namespace gtsam;
int main(int argc, char* argv[]) { int main(int argc, char* argv[]) {
// parse options and read BAL file // parse options and read BAL file
SfM_Data db = preamble(argc, argv); SfmData db = preamble(argc, argv);
// Build graph using conventional GeneralSFMFactor // Build graph using conventional GeneralSFMFactor
NonlinearFactorGraph graph; NonlinearFactorGraph graph;
for (size_t j = 0; j < db.number_tracks(); j++) { for (size_t j = 0; j < db.number_tracks(); j++) {
Point3_ nav_point_(P(j)); Point3_ nav_point_(P(j));
for (const SfM_Measurement& m: db.tracks[j].measurements) { for (const SfmMeasurement& m: db.tracks[j].measurements) {
size_t i = m.first; size_t i = m.first;
Point2 z = m.second; Point2 z = m.second;
Pose3_ navTcam_(C(i)); Pose3_ navTcam_(C(i));
@ -49,12 +49,12 @@ int main(int argc, char* argv[]) {
Values initial; Values initial;
size_t i = 0, j = 0; size_t i = 0, j = 0;
for (const SfM_Camera& camera: db.cameras) { for (const SfmCamera& camera: db.cameras) {
initial.insert(C(i), camera.pose()); initial.insert(C(i), camera.pose());
initial.insert(K(i), camera.calibration()); initial.insert(K(i), camera.calibration());
i += 1; i += 1;
} }
for (const SfM_Track& track: db.tracks) for (const SfmTrack& track: db.tracks)
initial.insert(P(j++), track.p); initial.insert(P(j++), track.p);
bool separateCalibration = true; bool separateCalibration = true;

View File

@ -31,13 +31,13 @@ typedef SmartProjectionFactor<Camera> SfmFactor;
int main(int argc, char* argv[]) { int main(int argc, char* argv[]) {
// parse options and read BAL file // parse options and read BAL file
SfM_Data db = preamble(argc, argv); SfmData db = preamble(argc, argv);
// Add smart factors to graph // Add smart factors to graph
NonlinearFactorGraph graph; NonlinearFactorGraph graph;
for (size_t j = 0; j < db.number_tracks(); j++) { for (size_t j = 0; j < db.number_tracks(); j++) {
auto smartFactor = boost::make_shared<SfmFactor>(gNoiseModel); auto smartFactor = boost::make_shared<SfmFactor>(gNoiseModel);
for (const SfM_Measurement& m : db.tracks[j].measurements) { for (const SfmMeasurement& m : db.tracks[j].measurements) {
size_t i = m.first; size_t i = m.first;
Point2 z = m.second; Point2 z = m.second;
smartFactor->add(z, C(i)); smartFactor->add(z, C(i));
@ -48,7 +48,7 @@ int main(int argc, char* argv[]) {
Values initial; Values initial;
size_t i = 0; size_t i = 0;
gUseSchur = false; gUseSchur = false;
for (const SfM_Camera& camera : db.cameras) for (const SfmCamera& camera : db.cameras)
initial.insert(C(i++), camera); initial.insert(C(i++), camera);
return optimize(db, graph, initial); return optimize(db, graph, initial);