diff --git a/examples/CreateSFMExampleData.cpp b/examples/CreateSFMExampleData.cpp index 5a829a886..aa0bde8f6 100644 --- a/examples/CreateSFMExampleData.cpp +++ b/examples/CreateSFMExampleData.cpp @@ -31,19 +31,19 @@ void createExampleBALFile(const string& filename, const vector& 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; diff --git a/examples/SFMExampleExpressions_bal.cpp b/examples/SFMExampleExpressions_bal.cpp index 320ad597d..22af9492f 100644 --- a/examples/SFMExampleExpressions_bal.cpp +++ b/examples/SFMExampleExpressions_bal.cpp @@ -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 camera0_(C(0)); + Expression 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 camera_(C(i)); + Expression camera_(C(i)); // Below an expression for the prediction of the measurement: - Point2_ predict_ = project2(camera_, point_); + Point2_ predict_ = project2(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 */ diff --git a/examples/SFMExample_bal.cpp b/examples/SFMExample_bal.cpp index 66f135c76..9d5ad0b33 100644 --- a/examples/SFMExample_bal.cpp +++ b/examples/SFMExample_bal.cpp @@ -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 MyFactor; +typedef GeneralSFMFactor 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(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 >(C(0), mydata.cameras[0], noiseModel::Isotropic::Sigma(9, 0.1)); + graph.emplace_shared >(C(0), mydata.cameras[0], noiseModel::Isotropic::Sigma(9, 0.1)); graph.emplace_shared > (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; diff --git a/examples/SFMExample_bal_COLAMD_METIS.cpp b/examples/SFMExample_bal_COLAMD_METIS.cpp index 50f8a5afd..b3dd3d25e 100644 --- a/examples/SFMExample_bal_COLAMD_METIS.cpp +++ b/examples/SFMExample_bal_COLAMD_METIS.cpp @@ -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 MyFactor; +typedef GeneralSFMFactor 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(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 >(C(0), mydata.cameras[0], noiseModel::Isotropic::Sigma(9, 0.1)); + graph.emplace_shared >(C(0), mydata.cameras[0], noiseModel::Isotropic::Sigma(9, 0.1)); graph.emplace_shared >(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 -----------------------**/ /** ----------------------------------------------------**/ diff --git a/gtsam.h b/gtsam.h index f6d3e85c1..a2276705e 100644 --- a/gtsam.h +++ b/gtsam.h @@ -2724,18 +2724,18 @@ virtual class EssentialMatrixFactor : gtsam::NoiseModelFactor { }; #include -class SfM_Track { +class SfmTrack { size_t number_measurements() const; pair measurement(size_t idx) const; - pair SIFT_index(size_t idx) const; + pair 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 camera(size_t idx) const; - gtsam::SfM_Track track(size_t idx) const; + gtsam::SfmTrack track(size_t idx) const; }; string findExampleDataFile(string name); diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index ff62bace4..45d009b1c 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -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; - 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(); 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(), 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; } diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index 83d857487..d6e7967b0 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -166,41 +166,39 @@ GTSAM_EXPORT std::map parse3DPoses(const std::string& filename); GTSAM_EXPORT GraphAndValues load3D(const std::string& filename); /// A measurement with its camera index -typedef std::pair SfM_Measurement; +typedef std::pair SfmMeasurement; -/// SfM_Track -typedef std::pair SIFT_Index; +/// SfmTrack +typedef std::pair 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 measurements; ///< The 2D image projections (id,(u,v)) - std::vector siftIndices; + std::vector measurements; ///< The 2D image projections (id,(u,v)) + std::vector 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 SfM_Camera; +typedef PinholeCamera SfmCamera; /// Define the structure for SfM data -struct SfM_Data { - std::vector cameras; ///< Set of cameras - std::vector tracks; ///< Sparse set of points - /// The number of camera poses +struct SfmData { + std::vector cameras; ///< Set of cameras + std::vector 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 diff --git a/gtsam/slam/tests/testDataset.cpp b/gtsam/slam/tests/testDataset.cpp index b7143c95e..9a3c797b2 100644 --- a/gtsam/slam/tests/testDataset.cpp +++ b/gtsam/slam/tests/testDataset.cpp @@ -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)); diff --git a/gtsam/slam/tests/testEssentialMatrixFactor.cpp b/gtsam/slam/tests/testEssentialMatrixFactor.cpp index fe73ef572..6d6daa33d 100644 --- a/gtsam/slam/tests/testEssentialMatrixFactor.cpp +++ b/gtsam/slam/tests/testEssentialMatrixFactor.cpp @@ -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(); diff --git a/gtsam/slam/tests/testSmartProjectionFactor.cpp b/gtsam/slam/tests/testSmartProjectionFactor.cpp index c6d1a5b2c..7b3158e9a 100644 --- a/gtsam/slam/tests/testSmartProjectionFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionFactor.cpp @@ -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)); } diff --git a/tests/testGeneralSFMFactorB.cpp b/tests/testGeneralSFMFactorB.cpp index 10df768b9..05b4c7f66 100644 --- a/tests/testGeneralSFMFactorB.cpp +++ b/tests/testGeneralSFMFactorB.cpp @@ -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(m.second, unit2, m.first, P(j)); } diff --git a/timing/timeSFMBAL.cpp b/timing/timeSFMBAL.cpp index 4dba540fd..4a58a57a6 100644 --- a/timing/timeSFMBAL.cpp +++ b/timing/timeSFMBAL.cpp @@ -32,12 +32,12 @@ typedef GeneralSFMFactor 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(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); diff --git a/timing/timeSFMBAL.h b/timing/timeSFMBAL.h index 448e9f459..548c4de70 100644 --- a/timing/timeSFMBAL.h +++ b/timing/timeSFMBAL.h @@ -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; diff --git a/timing/timeSFMBALautodiff.cpp b/timing/timeSFMBALautodiff.cpp index 43e8a8eb1..2d0f4a1fe 100644 --- a/timing/timeSFMBALautodiff.cpp +++ b/timing/timeSFMBALautodiff.cpp @@ -38,14 +38,14 @@ typedef PinholeCamera 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 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 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); } diff --git a/timing/timeSFMBALcamTnav.cpp b/timing/timeSFMBALcamTnav.cpp index 474cc3c96..355defed9 100644 --- a/timing/timeSFMBALcamTnav.cpp +++ b/timing/timeSFMBALcamTnav.cpp @@ -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; diff --git a/timing/timeSFMBALnavTcam.cpp b/timing/timeSFMBALnavTcam.cpp index e553ff0cf..e602ef241 100644 --- a/timing/timeSFMBALnavTcam.cpp +++ b/timing/timeSFMBALnavTcam.cpp @@ -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; diff --git a/timing/timeSFMBALsmart.cpp b/timing/timeSFMBALsmart.cpp index cc250f2e8..a69d895a5 100644 --- a/timing/timeSFMBALsmart.cpp +++ b/timing/timeSFMBALsmart.cpp @@ -31,13 +31,13 @@ typedef SmartProjectionFactor 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(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);