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

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
examples/CreateSFMExampleData.cpp
View File

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

20
examples/SFMExampleExpressions_bal.cpp
View File

@ -37,7 +37,7 @@ using namespace noiseModel;
using symbol_shorthand::C;
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
/* ************************************************************************* */
@ -49,7 +49,7 @@ int main(int argc, char* argv[]) {
filename = string(argv[1]);
// Load the SfM data from file
SfM_Data mydata;
SfmData mydata;
readBAL(filename, mydata);
cout
<< 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
// 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:
Pose3_ pose0_(&SfM_Camera::getPose, camera0_);
Pose3_ pose0_(&SfmCamera::getPose, camera0_);
// Finally, we say it should be equal to first guess
graph.addExpressionFactor(pose0_, mydata.cameras[0].pose(),
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
size_t j = 0;
for(const SfM_Track& track: mydata.tracks) {
for(const SfmTrack& track: mydata.tracks) {
// Leaf expression for j^th point
Point3_ point_('p', j);
for(const SfM_Measurement& m: track.measurements) {
for(const SfmMeasurement& m: track.measurements) {
size_t i = m.first;
Point2 uv = m.second;
// 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:
Point2_ predict_ = project2<SfM_Camera>(camera_, point_);
Point2_ predict_ = project2<SfmCamera>(camera_, point_);
// Again, here we use an ExpressionFactor
graph.addExpressionFactor(predict_, uv, noise);
}
@ -98,9 +98,9 @@ int main(int argc, char* argv[]) {
Values initial;
size_t i = 0;
j = 0;
for(const SfM_Camera& camera: mydata.cameras)
for(const SfmCamera& camera: mydata.cameras)
initial.insert(C(i++), camera);
for(const SfM_Track& track: mydata.tracks)
for(const SfmTrack& track: mydata.tracks)
initial.insert(P(j++), track.p);
/* Optimize the graph and print results */

14
examples/SFMExample_bal.cpp
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.
// An SFM_Camera is defined in datase.h as a camera with unknown Cal3Bundler calibration
// 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[]) {
@ -42,7 +42,7 @@ int main (int argc, char* argv[]) {
if (argc>1) filename = string(argv[1]);
// Load the SfM data from file
SfM_Data mydata;
SfmData mydata;
readBAL(filename, mydata);
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
size_t j = 0;
for(const SfM_Track& track: mydata.tracks) {
for(const SfM_Measurement& m: track.measurements) {
for(const SfmTrack& track: mydata.tracks) {
for(const SfmMeasurement& m: track.measurements) {
size_t i = m.first;
Point2 uv = m.second;
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.
// 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));
// Create initial estimate
Values initial;
size_t i = 0; j = 0;
for(const SfM_Camera& camera: mydata.cameras) initial.insert(C(i++), camera);
for(const SfM_Track& track: mydata.tracks) initial.insert(P(j++), track.p);
for(const SfmCamera& camera: mydata.cameras) initial.insert(C(i++), camera);
for(const SfmTrack& track: mydata.tracks) initial.insert(P(j++), track.p);
/* Optimize the graph and print results */
Values result;

14
examples/SFMExample_bal_COLAMD_METIS.cpp
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.
// An SFM_Camera is defined in datase.h as a camera with unknown Cal3Bundler calibration
// 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[]) {
@ -47,7 +47,7 @@ int main (int argc, char* argv[]) {
if (argc>1) filename = string(argv[1]);
// Load the SfM data from file
SfM_Data mydata;
SfmData mydata;
readBAL(filename, mydata);
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
size_t j = 0;
for(const SfM_Track& track: mydata.tracks) {
for(const SfM_Measurement& m: track.measurements) {
for(const SfmTrack& track: mydata.tracks) {
for(const SfmMeasurement& m: track.measurements) {
size_t i = m.first;
Point2 uv = m.second;
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.
// 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));
// Create initial estimate
Values initial;
size_t i = 0; j = 0;
for(const SfM_Camera& camera: mydata.cameras) initial.insert(C(i++), camera);
for(const SfM_Track& track: mydata.tracks) initial.insert(P(j++), track.p);
for(const SfmCamera& camera: mydata.cameras) initial.insert(C(i++), camera);
for(const SfmTrack& track: mydata.tracks) initial.insert(P(j++), track.p);
/** --------------- COMPARISON -----------------------**/
/** ----------------------------------------------------**/

8
gtsam.h
View File

@ -2724,18 +2724,18 @@ virtual class EssentialMatrixFactor : gtsam::NoiseModelFactor {
};
#include <gtsam/slam/dataset.h>
class SfM_Track {
class SfmTrack {
size_t number_measurements() 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_tracks() const;
//TODO(Varun) Need to fix issue #237 first before this can work
// 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);

32
gtsam/slam/dataset.cpp
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
ifstream is(filename.c_str(), ifstream::in);
if (!is) {
@ -697,7 +697,7 @@ bool readBundler(const string& filename, SfM_Data &data) {
// Get the information for the 3D points
data.tracks.reserve(nrPoints);
for (size_t j = 0; j < nrPoints; j++) {
SfM_Track track;
SfmTrack track;
// Get the 3D position
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
ifstream is(filename.c_str(), ifstream::in);
if (!is) {
@ -781,7 +781,7 @@ bool readBAL(const string& filename, SfM_Data &data) {
// Get the 3D position
float 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.r = 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
ofstream os;
os.open(filename.c_str());
@ -815,7 +815,7 @@ bool writeBAL(const string& filename, SfM_Data &data) {
os << endl;
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
size_t i = track.measurements[k].first; // camera id
@ -866,12 +866,12 @@ bool writeBAL(const string& filename, SfM_Data &data) {
return true;
}
bool writeBALfromValues(const string& filename, const SfM_Data &data,
bool writeBALfromValues(const string& filename, const SfmData &data,
Values& values) {
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>();
if (nrPoses == dataValues.number_cameras()) { // we only estimated camera poses
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();
if (nrPoints != nrTracks) {
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);
}
Values initialCamerasEstimate(const SfM_Data& db) {
Values initialCamerasEstimate(const SfmData& db) {
Values initial;
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);
return initial;
}
Values initialCamerasAndPointsEstimate(const SfM_Data& db) {
Values initialCamerasAndPointsEstimate(const SfmData& db) {
Values initial;
size_t i = 0, j = 0;
for(const SfM_Camera& camera: db.cameras)
for(const SfmCamera& camera: db.cameras)
initial.insert((i++), camera);
for(const SfM_Track& track: db.tracks)
for(const SfmTrack& track: db.tracks)
initial.insert(P(j++), track.p);
return initial;
}

66
gtsam/slam/dataset.h
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);
/// A measurement with its camera index
typedef std::pair<size_t, Point2> SfM_Measurement;
typedef std::pair<size_t, Point2> SfmMeasurement;
/// SfM_Track
typedef std::pair<size_t, size_t> SIFT_Index;
/// SfmTrack
typedef std::pair<size_t, size_t> SiftIndex;
/// Define the structure for the 3D points
struct SfM_Track {
/// Construct empty track
SfM_Track(): p(0,0,0) {}
struct SfmTrack {
SfmTrack(): p(0,0,0) {}
Point3 p; ///< 3D position of the 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<SIFT_Index> siftIndices;
std::vector<SfmMeasurement> measurements; ///< The 2D image projections (id,(u,v))
std::vector<SiftIndex> siftIndices;
/// Total number of measurements in this track
size_t number_measurements() const {
return measurements.size();
}
/// 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];
}
/// 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];
}
};
/// Define the structure for the camera poses
typedef PinholeCamera<Cal3Bundler> SfM_Camera;
typedef PinholeCamera<Cal3Bundler> SfmCamera;
/// Define the structure for SfM data
struct SfM_Data {
std::vector<SfM_Camera> cameras; ///< Set of cameras
std::vector<SfM_Track> tracks; ///< Sparse set of points
/// The number of camera poses
struct SfmData {
std::vector<SfmCamera> cameras; ///< Set of cameras
std::vector<SfmTrack> tracks; ///< Sparse set of points
size_t number_cameras() const {
return cameras.size();
}
@ -209,48 +207,54 @@ struct SfM_Data {
return tracks.size();
}
/// The camera pose at frame index `idx`
SfM_Camera camera(size_t idx) const {
SfmCamera camera(size_t idx) const {
return cameras[idx];
}
/// The track formed by series of landmark measurements
SfM_Track track(size_t idx) const {
SfmTrack track(size_t idx) const {
return tracks[idx];
}
};
/// Alias for backwards compatibility
typedef SfM_Data SfM_data;
/// Aliases for backwards compatibility
#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
* SfM_Data structure
* 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, 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
* SfM_Data structure
* 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, 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
* SfM_Data structure
* SfmData structure
* @param filename The name of the BAL file to write
* @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, 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
* 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)
* @param filename The name of the BAL file to write
* @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
*/
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
@ -291,16 +295,16 @@ GTSAM_EXPORT Pose3 gtsam2openGL(const Pose3& PoseGTSAM);
/**
* @brief This function creates initial values for cameras from db
* @param SfM_Data
* @param SfmData
* @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
* @param SfM_Data
* @param SfmData
* @return Values
*/
GTSAM_EXPORT Values initialCamerasAndPointsEstimate(const SfM_Data& db);
GTSAM_EXPORT Values initialCamerasAndPointsEstimate(const SfmData& db);
} // namespace gtsam

28
gtsam/slam/tests/testDataset.cpp
View File

@ -106,18 +106,18 @@ TEST( dataSet, Balbianello)
{
///< The structure where we will save the SfM data
const string filename = findExampleDataFile("Balbianello");
SfM_Data mydata;
SfmData mydata;
CHECK(readBundler(filename, mydata));
// Check number of things
EXPECT_LONGS_EQUAL(5,mydata.number_cameras());
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());
// Check projection of a given point
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;
EXPECT(assert_equal(expected,actual,1));
}
@ -389,18 +389,18 @@ TEST( dataSet, readBAL_Dubrovnik)
{
///< The structure where we will save the SfM data
const string filename = findExampleDataFile("dubrovnik-3-7-pre");
SfM_Data mydata;
SfmData mydata;
CHECK(readBAL(filename, mydata));
// Check number of things
EXPECT_LONGS_EQUAL(3,mydata.number_cameras());
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());
// Check projection of a given point
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;
EXPECT(assert_equal(expected,actual,12));
}
@ -444,7 +444,7 @@ TEST( dataSet, writeBAL_Dubrovnik)
{
///< Read a file using the unit tested readBAL
const string filenameToRead = findExampleDataFile("dubrovnik-3-7-pre");
SfM_Data readData;
SfmData readData;
readBAL(filenameToRead, readData);
// Write readData to file filenameToWrite
@ -452,7 +452,7 @@ TEST( dataSet, writeBAL_Dubrovnik)
CHECK(writeBAL(filenameToWrite, readData));
// Read what we wrote
SfM_Data writtenData;
SfmData writtenData;
CHECK(readBAL(filenameToWrite, writtenData));
// 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++){
// check point
SfM_Track expectedTrack = writtenData.tracks[j];
SfM_Track actualTrack = readData.tracks[j];
SfmTrack expectedTrack = writtenData.tracks[j];
SfmTrack actualTrack = readData.tracks[j];
Point3 expectedPoint = expectedTrack.p;
Point3 actualPoint = actualTrack.p;
EXPECT(assert_equal(expectedPoint,actualPoint));
@ -492,7 +492,7 @@ TEST( dataSet, writeBALfromValues_Dubrovnik){
///< Read a file using the unit tested readBAL
const string filenameToRead = findExampleDataFile("dubrovnik-3-7-pre");
SfM_Data readData;
SfmData readData;
readBAL(filenameToRead, readData);
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);
// Read the file we wrote
SfM_Data writtenData;
SfmData writtenData;
readBAL(filenameToWrite, writtenData);
// Check that the reprojection errors are the same and the poses are correct
// Check number of things
EXPECT_LONGS_EQUAL(3,writtenData.number_cameras());
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());
// Check projection of a given point
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;
EXPECT(assert_equal(expected,actual,12));

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

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

4
gtsam/slam/tests/testSmartProjectionFactor.cpp
View File

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

4
tests/testGeneralSFMFactorB.cpp
View File

@ -42,7 +42,7 @@ using symbol_shorthand::P;
/* ************************************************************************* */
TEST(PinholeCamera, BAL) {
string filename = findExampleDataFile("dubrovnik-3-7-pre");
SfM_Data db;
SfmData db;
bool success = readBAL(filename, db);
if (!success) throw runtime_error("Could not access file!");
@ -50,7 +50,7 @@ TEST(PinholeCamera, BAL) {
NonlinearFactorGraph graph;
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));
}

8
timing/timeSFMBAL.cpp
View File

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

6
timing/timeSFMBAL.h
View File

@ -38,7 +38,7 @@ static bool gUseSchur = true;
static SharedNoiseModel gNoiseModel = noiseModel::Unit::Create(2);
// parse options and read BAL file
SfM_Data preamble(int argc, char* argv[]) {
SfmData preamble(int argc, char* argv[]) {
// primitive argument parsing:
if (argc > 2) {
if (strcmp(argv[1], "--colamd"))
@ -48,7 +48,7 @@ SfM_Data preamble(int argc, char* argv[]) {
}
// Load BAL file
SfM_Data db;
SfmData db;
string filename;
if (argc > 1)
filename = argv[argc - 1];
@ -60,7 +60,7 @@ SfM_Data preamble(int argc, char* argv[]) {
}
// 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) {
using symbol_shorthand::P;

8
timing/timeSFMBALautodiff.cpp
View File

@ -38,14 +38,14 @@ typedef PinholeCamera<Cal3Bundler> Camera;
int main(int argc, char* argv[]) {
// parse options and read BAL file
SfM_Data db = preamble(argc, argv);
SfmData db = preamble(argc, argv);
AdaptAutoDiff<SnavelyProjection, 2, 9, 3> snavely;
// Build graph
NonlinearFactorGraph graph;
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;
Point2 z = m.second;
Expression<Vector9> camera_(C(i));
@ -58,14 +58,14 @@ int main(int argc, char* argv[]) {
Values initial;
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 !
Pose3 openGLpose = gtsam2openGL(camera.pose());
Vector9 v9;
v9 << Pose3::Logmap(openGLpose), camera.calibration();
initial.insert(C(i++), v9);
}
for (const SfM_Track& track: db.tracks) {
for (const SfmTrack& track: db.tracks) {
Vector3 v3 = track.p;
initial.insert(P(j++), v3);
}

8
timing/timeSFMBALcamTnav.cpp
View File

@ -29,12 +29,12 @@ using namespace gtsam;
int main(int argc, char* argv[]) {
// parse options and read BAL file
SfM_Data db = preamble(argc, argv);
SfmData db = preamble(argc, argv);
// Build graph using conventional GeneralSFMFactor
NonlinearFactorGraph graph;
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;
Point2 z = m.second;
Pose3_ camTnav_(C(i));
@ -49,12 +49,12 @@ int main(int argc, char* argv[]) {
Values initial;
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(K(i), camera.calibration());
i += 1;
}
for (const SfM_Track& track: db.tracks)
for (const SfmTrack& track: db.tracks)
initial.insert(P(j++), track.p);
bool separateCalibration = true;

8
timing/timeSFMBALnavTcam.cpp
View File

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

6
timing/timeSFMBALsmart.cpp
View File

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