diff --git a/examples/SFMExampleExpressions_bal.cpp b/examples/SFMExampleExpressions_bal.cpp index 4fd6c1ada..3a02e6cab 100644 --- a/examples/SFMExampleExpressions_bal.cpp +++ b/examples/SFMExampleExpressions_bal.cpp @@ -28,7 +28,8 @@ // Header order is close to far #include #include -#include // for loading BAL datasets ! +#include // for loading BAL datasets ! +#include #include using namespace std; @@ -46,8 +47,7 @@ int main(int argc, char* argv[]) { if (argc > 1) filename = string(argv[1]); // Load the SfM data from file - SfmData mydata; - readBAL(filename, mydata); + SfmData mydata = SfmData::FromBalFile(filename); cout << boost::format("read %1% tracks on %2% cameras\n") % mydata.numberTracks() % mydata.numberCameras(); diff --git a/examples/SFMExample_bal.cpp b/examples/SFMExample_bal.cpp index 9d6e38a15..6944177c1 100644 --- a/examples/SFMExample_bal.cpp +++ b/examples/SFMExample_bal.cpp @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** - * @file SFMExample.cpp + * @file SFMExample_bal.cpp * @brief Solve a structure-from-motion problem from a "Bundle Adjustment in the Large" file * @author Frank Dellaert */ @@ -20,7 +20,8 @@ #include #include #include -#include // for loading BAL datasets ! +#include // for loading BAL datasets ! +#include #include using namespace std; @@ -41,8 +42,7 @@ int main (int argc, char* argv[]) { if (argc>1) filename = string(argv[1]); // Load the SfM data from file - SfmData mydata; - readBAL(filename, mydata); + SfmData mydata = SfmData::FromBalFile(filename); cout << boost::format("read %1% tracks on %2% cameras\n") % mydata.numberTracks() % mydata.numberCameras(); // Create a factor graph diff --git a/examples/SFMExample_bal_COLAMD_METIS.cpp b/examples/SFMExample_bal_COLAMD_METIS.cpp index b59510c9f..4d04dd16e 100644 --- a/examples/SFMExample_bal_COLAMD_METIS.cpp +++ b/examples/SFMExample_bal_COLAMD_METIS.cpp @@ -22,7 +22,8 @@ #include #include #include -#include // for loading BAL datasets ! +#include // for loading BAL datasets ! +#include #include @@ -45,8 +46,7 @@ int main(int argc, char* argv[]) { if (argc > 1) filename = string(argv[1]); // Load the SfM data from file - SfmData mydata; - readBAL(filename, mydata); + SfmData mydata = SfmData::FromBalFile(filename); cout << boost::format("read %1% tracks on %2% cameras\n") % mydata.numberTracks() % mydata.numberCameras(); diff --git a/gtsam/navigation/ImuBias.cpp b/gtsam/navigation/ImuBias.cpp index 0dbc5736f..bc11f95f8 100644 --- a/gtsam/navigation/ImuBias.cpp +++ b/gtsam/navigation/ImuBias.cpp @@ -66,8 +66,8 @@ namespace imuBias { // } /// ostream operator std::ostream& operator<<(std::ostream& os, const ConstantBias& bias) { - os << "acc = " << Point3(bias.accelerometer()); - os << " gyro = " << Point3(bias.gyroscope()); + os << "acc = " << bias.accelerometer().transpose(); + os << " gyro = " << bias.gyroscope().transpose(); return os; } diff --git a/gtsam/sfm/SfmData.cpp b/gtsam/sfm/SfmData.cpp index 2f5396fcd..43b1f5911 100644 --- a/gtsam/sfm/SfmData.cpp +++ b/gtsam/sfm/SfmData.cpp @@ -18,6 +18,7 @@ #include #include +#include #include #include @@ -28,7 +29,6 @@ using std::cout; using std::endl; using gtsam::symbol_shorthand::P; -using gtsam::symbol_shorthand::X; /* ************************************************************************** */ void SfmData::print(const std::string &s) const { @@ -99,14 +99,16 @@ Pose3 gtsam2openGL(const Pose3 &PoseGTSAM) { } /* ************************************************************************** */ -bool readBundler(const std::string &filename, SfmData &data) { +SfmData SfmData::FromBundlerFile(const std::string &filename) { // Load the data file std::ifstream is(filename.c_str(), std::ifstream::in); if (!is) { - cout << "Error in readBundler: can not find the file!!" << endl; - return false; + throw std::runtime_error( + "Error in FromBundlerFile: can not find the file!!"); } + SfmData sfmData; + // Ignore the first line char aux[500]; is.getline(aux, 500); @@ -133,9 +135,8 @@ bool readBundler(const std::string &filename, SfmData &data) { // Check for all-zero R, in which case quit if (r11 == 0 && r12 == 0 && r13 == 0) { - cout << "Error in readBundler: zero rotation matrix for pose " << i - << endl; - return false; + throw std::runtime_error( + "Error in FromBundlerFile: zero rotation matrix"); } // Get the translation vector @@ -144,11 +145,11 @@ bool readBundler(const std::string &filename, SfmData &data) { Pose3 pose = openGL2gtsam(R, tx, ty, tz); - data.cameras.emplace_back(pose, K); + sfmData.cameras.emplace_back(pose, K); } // Get the information for the 3D points - data.tracks.reserve(nrPoints); + sfmData.tracks.reserve(nrPoints); for (size_t j = 0; j < nrPoints; j++) { SfmTrack track; @@ -178,34 +179,34 @@ bool readBundler(const std::string &filename, SfmData &data) { track.siftIndices.emplace_back(cam_idx, point_idx); } - data.tracks.push_back(track); + sfmData.tracks.push_back(track); } - is.close(); - return true; + return sfmData; } /* ************************************************************************** */ -bool readBAL(const std::string &filename, SfmData &data) { +SfmData SfmData::FromBalFile(const std::string &filename) { // Load the data file std::ifstream is(filename.c_str(), std::ifstream::in); if (!is) { - cout << "Error in readBAL: can not find the file!!" << endl; - return false; + throw std::runtime_error("Error in FromBalFile: can not find the file!!"); } + SfmData sfmData; + // Get the number of camera poses and 3D points size_t nrPoses, nrPoints, nrObservations; is >> nrPoses >> nrPoints >> nrObservations; - data.tracks.resize(nrPoints); + sfmData.tracks.resize(nrPoints); // Get the information for the observations for (size_t k = 0; k < nrObservations; k++) { size_t i = 0, j = 0; float u, v; is >> i >> j >> u >> v; - data.tracks[j].measurements.emplace_back(i, Point2(u, -v)); + sfmData.tracks[j].measurements.emplace_back(i, Point2(u, -v)); } // Get the information for the camera poses @@ -226,7 +227,7 @@ bool readBAL(const std::string &filename, SfmData &data) { is >> f >> k1 >> k2; Cal3Bundler K(f, k1, k2); - data.cameras.emplace_back(pose, K); + sfmData.cameras.emplace_back(pose, K); } // Get the information for the 3D points @@ -234,26 +235,18 @@ bool readBAL(const std::string &filename, SfmData &data) { // Get the 3D position float x, y, z; is >> x >> y >> z; - SfmTrack &track = data.tracks[j]; + SfmTrack &track = sfmData.tracks[j]; track.p = Point3(x, y, z); track.r = 0.4f; track.g = 0.4f; track.b = 0.4f; } - is.close(); - return true; + return sfmData; } /* ************************************************************************** */ -SfmData readBal(const std::string &filename) { - SfmData data; - readBAL(filename, data); - return data; -} - -/* ************************************************************************** */ -bool writeBAL(const std::string &filename, SfmData &data) { +bool writeBAL(const std::string &filename, const SfmData &data) { // Open the output file std::ofstream os; os.open(filename.c_str()); @@ -329,9 +322,32 @@ bool writeBAL(const std::string &filename, SfmData &data) { return true; } +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 +bool readBundler(const std::string &filename, SfmData &data) { + try { + data = SfmData::FromBundlerFile(filename); + return true; + } catch (const std::exception & /* e */) { + return false; + } +} +bool readBAL(const std::string &filename, SfmData &data) { + try { + data = SfmData::FromBalFile(filename); + return true; + } catch (const std::exception & /* e */) { + return false; + } +} +#endif + +SfmData readBal(const std::string &filename) { + return SfmData::FromBalFile(filename); +} + /* ************************************************************************** */ bool writeBALfromValues(const std::string &filename, const SfmData &data, - Values &values) { + const Values &values) { using Camera = PinholeCamera; SfmData dataValues = data; @@ -339,7 +355,7 @@ bool writeBALfromValues(const std::string &filename, const SfmData &data, size_t nrPoses = values.count(); if (nrPoses == dataValues.cameras.size()) { // we only estimated camera poses for (size_t i = 0; i < dataValues.cameras.size(); i++) { // for each camera - Pose3 pose = values.at(X(i)); + Pose3 pose = values.at(i); Cal3Bundler K = dataValues.cameras[i].calibration(); Camera camera(pose, K); dataValues.cameras[i] = camera; @@ -387,6 +403,41 @@ bool writeBALfromValues(const std::string &filename, const SfmData &data, return writeBAL(filename, dataValues); } +/* ************************************************************************** */ +NonlinearFactorGraph SfmData::generalSfmFactors( + const SharedNoiseModel &model) const { + using ProjectionFactor = GeneralSFMFactor; + NonlinearFactorGraph factors; + + size_t j = 0; + for (const SfmTrack &track : tracks) { + for (const SfmMeasurement &m : track.measurements) { + size_t i = m.first; + Point2 uv = m.second; + factors.emplace_shared(uv, model, i, P(j)); + } + j += 1; + } + + return factors; +} + +/* ************************************************************************** */ +NonlinearFactorGraph SfmData::sfmFactorGraph( + const SharedNoiseModel &model, boost::optional fixedCamera, + boost::optional fixedPoint) const { + using ProjectionFactor = GeneralSFMFactor; + NonlinearFactorGraph graph = generalSfmFactors(model); + using noiseModel::Constrained; + if (fixedCamera) { + graph.addPrior(*fixedCamera, cameras[0], Constrained::All(9)); + } + if (fixedPoint) { + graph.addPrior(P(*fixedPoint), tracks[0].p, Constrained::All(3)); + } + return graph; +} + /* ************************************************************************** */ Values initialCamerasEstimate(const SfmData &db) { Values initial; @@ -399,7 +450,7 @@ Values initialCamerasEstimate(const SfmData &db) { Values initialCamerasAndPointsEstimate(const SfmData &db) { Values initial; size_t i = 0, j = 0; - for (const SfmCamera &camera : db.cameras) initial.insert((i++), camera); + for (const SfmCamera &camera : db.cameras) initial.insert(i++, camera); for (const SfmTrack &track : db.tracks) initial.insert(P(j++), track.p); return initial; } diff --git a/gtsam/sfm/SfmData.h b/gtsam/sfm/SfmData.h index e2a6985f2..afce12205 100644 --- a/gtsam/sfm/SfmData.h +++ b/gtsam/sfm/SfmData.h @@ -20,6 +20,7 @@ #include #include +#include #include #include @@ -35,11 +36,31 @@ typedef PinholeCamera SfmCamera; * @brief SfmData stores a bunch of SfmTracks * @addtogroup sfm */ -struct SfmData { +struct GTSAM_EXPORT SfmData { std::vector cameras; ///< Set of cameras std::vector tracks; ///< Sparse set of points + /// @name Create from file + /// @{ + + /** + * @brief Parses a bundler output file and return result as SfmData instance. + * @param filename The name of the bundler file + * @param data SfM structure where the data is stored + * @return true if the parsing was successful, false otherwise + */ + static SfmData FromBundlerFile(const std::string& filename); + + /** + * @brief Parse a "Bundle Adjustment in the Large" (BAL) file and return + * result as SfmData instance. + * @param filename The name of the BAL file. + * @return SfM structure where the data is stored. + */ + static SfmData FromBalFile(const std::string& filename); + + /// @} /// @name Standard Interface /// @{ @@ -61,6 +82,31 @@ struct SfmData { /// The camera pose at frame index `idx` SfmCamera camera(size_t idx) const { return cameras[idx]; } + /** + * @brief Create projection factors using keys i and P(j) + * + * @param model a noise model for projection errors + * @return NonlinearFactorGraph + */ + NonlinearFactorGraph generalSfmFactors( + const SharedNoiseModel& model = noiseModel::Isotropic::Sigma(2, + 1.0)) const; + + /** + * @brief Create factor graph with projection factors and gauge fix. + * + * Note: pose keys are simply integer indices, points use Symbol('p', j). + * + * @param model a noise model for projection errors + * @param fixedCamera which camera to fix, if any (use boost::none if none) + * @param fixedPoint which point to fix, if any (use boost::none if none) + * @return NonlinearFactorGraph + */ + NonlinearFactorGraph sfmFactorGraph( + const SharedNoiseModel& model = noiseModel::Isotropic::Sigma(2, 1.0), + boost::optional fixedCamera = 0, + boost::optional fixedPoint = 0) const; + /// @} /// @name Testable /// @{ @@ -101,23 +147,12 @@ struct SfmData { template <> struct traits : public Testable {}; -/** - * @brief This function parses a bundler output file and stores the data into a - * SfmData structure - * @param filename The name of the bundler file - * @param data SfM structure where the data is stored - * @return true if the parsing was successful, false otherwise - */ -GTSAM_EXPORT bool readBundler(const std::string& filename, SfmData& data); - -/** - * @brief This function parses a "Bundle Adjustment in the Large" (BAL) file and - * stores the data into a SfmData structure - * @param filename The name of the BAL file - * @param data SfM structure where the data is stored - * @return true if the parsing was successful, false otherwise - */ -GTSAM_EXPORT bool readBAL(const std::string& filename, SfmData& data); +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 +GTSAM_EXPORT bool GTSAM_DEPRECATED readBundler(const std::string& filename, + SfmData& data); +GTSAM_EXPORT bool GTSAM_DEPRECATED readBAL(const std::string& filename, + SfmData& data); +#endif /** * @brief This function parses a "Bundle Adjustment in the Large" (BAL) file and @@ -134,7 +169,7 @@ GTSAM_EXPORT SfmData readBal(const std::string& filename); * @param data SfM structure where the data is stored * @return true if the parsing was successful, false otherwise */ -GTSAM_EXPORT bool writeBAL(const std::string& filename, SfmData& data); +GTSAM_EXPORT bool writeBAL(const std::string& filename, const SfmData& data); /** * @brief This function writes a "Bundle Adjustment in the Large" (BAL) file @@ -144,12 +179,12 @@ GTSAM_EXPORT bool writeBAL(const std::string& filename, SfmData& data); * @param data SfM structure where the data is stored * @param values structure where the graph values are stored (values can be * either Pose3 or PinholeCamera for the cameras, and should be - * Point3 for the 3D points). Note that the current version assumes that the - * keys are "x1" for pose 1 (or "c1" for camera 1) and "l1" for landmark 1 + * Point3 for the 3D points). Note: assumes that the keys are "i" for pose i + * and "Symbol::('p',j)" for landmark j. * @return true if the parsing was successful, false otherwise */ GTSAM_EXPORT bool writeBALfromValues(const std::string& filename, - const SfmData& data, Values& values); + const SfmData& data, const Values& values); /** * @brief This function converts an openGL camera pose to an GTSAM camera pose @@ -179,7 +214,10 @@ GTSAM_EXPORT Pose3 gtsam2openGL(const Rot3& R, double tx, double ty, double tz); GTSAM_EXPORT Pose3 gtsam2openGL(const Pose3& PoseGTSAM); /** - * @brief This function creates initial values for cameras from db + * @brief This function creates initial values for cameras from db. + * + * No symbol is used, so camera keys are simply integer indices. + * * @param SfmData * @return Values */ @@ -187,6 +225,9 @@ GTSAM_EXPORT Values initialCamerasEstimate(const SfmData& db); /** * @brief This function creates initial values for cameras and points from db + * + * Note: Pose keys are simply integer indices, points use Symbol('p', j). + * * @param SfmData * @return Values */ diff --git a/gtsam/sfm/SfmTrack.h b/gtsam/sfm/SfmTrack.h index ee9128d04..37731b32a 100644 --- a/gtsam/sfm/SfmTrack.h +++ b/gtsam/sfm/SfmTrack.h @@ -38,7 +38,7 @@ typedef std::pair SiftIndex; * @brief An SfmTrack stores SfM measurements grouped in a track * @addtogroup sfm */ -struct SfmTrack { +struct GTSAM_EXPORT SfmTrack { Point3 p; ///< 3D position of the point float r, g, b; ///< RGB color of the 3D point diff --git a/gtsam/sfm/sfm.i b/gtsam/sfm/sfm.i index 14f183d16..bf9a73ac5 100644 --- a/gtsam/sfm/sfm.i +++ b/gtsam/sfm/sfm.i @@ -31,12 +31,23 @@ class SfmTrack { #include class SfmData { SfmData(); - size_t numberCameras() const; - size_t numberTracks() const; - gtsam::PinholeCamera camera(size_t idx) const; - gtsam::SfmTrack track(size_t idx) const; + static gtsam::SfmData FromBundlerFile(string filename); + static gtsam::SfmData FromBalFile(string filename); + void addTrack(const gtsam::SfmTrack& t); void addCamera(const gtsam::SfmCamera& cam); + size_t numberTracks() const; + size_t numberCameras() const; + gtsam::SfmTrack track(size_t idx) const; + gtsam::PinholeCamera camera(size_t idx) const; + + gtsam::NonlinearFactorGraph generalSfmFactors( + const gtsam::SharedNoiseModel& model = + gtsam::noiseModel::Isotropic::Sigma(2, 1.0)) const; + gtsam::NonlinearFactorGraph sfmFactorGraph( + const gtsam::SharedNoiseModel& model = + gtsam::noiseModel::Isotropic::Sigma(2, 1.0), + size_t fixedCamera = 0, size_t fixedPoint = 0) const; // enabling serialization functionality void serialize() const; diff --git a/gtsam/sfm/tests/testSfmData.cpp b/gtsam/sfm/tests/testSfmData.cpp index bc1c8b645..7bd5d27e7 100644 --- a/gtsam/sfm/tests/testSfmData.cpp +++ b/gtsam/sfm/tests/testSfmData.cpp @@ -18,13 +18,13 @@ #include #include +#include #include using namespace std; using namespace gtsam; using gtsam::symbol_shorthand::P; -using gtsam::symbol_shorthand::X; namespace gtsam { GTSAM_EXPORT std::string createRewrittenFileName(const std::string& name); @@ -33,41 +33,50 @@ GTSAM_EXPORT std::string findExampleDataFile(const std::string& name); /* ************************************************************************* */ TEST(dataSet, Balbianello) { - ///< The structure where we will save the SfM data + // The structure where we will save the SfM data const string filename = findExampleDataFile("Balbianello"); - SfmData mydata; - CHECK(readBundler(filename, mydata)); + SfmData sfmData = SfmData::FromBundlerFile(filename); // Check number of things - EXPECT_LONGS_EQUAL(5, mydata.numberCameras()); - EXPECT_LONGS_EQUAL(544, mydata.numberTracks()); - const SfmTrack& track0 = mydata.tracks[0]; + EXPECT_LONGS_EQUAL(5, sfmData.numberCameras()); + EXPECT_LONGS_EQUAL(544, sfmData.numberTracks()); + const SfmTrack& track0 = sfmData.tracks[0]; EXPECT_LONGS_EQUAL(3, track0.numberMeasurements()); // Check projection of a given point EXPECT_LONGS_EQUAL(0, track0.measurements[0].first); - const SfmCamera& camera0 = mydata.cameras[0]; + const SfmCamera& camera0 = sfmData.cameras[0]; Point2 expected = camera0.project(track0.p), actual = track0.measurements[0].second; EXPECT(assert_equal(expected, actual, 1)); + + // We share *one* noiseModel between all projection factors + auto model = noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v + + // Convert to NonlinearFactorGraph + NonlinearFactorGraph graph = sfmData.sfmFactorGraph(model); + EXPECT_LONGS_EQUAL(1419, graph.size()); // regression + + // Get initial estimate + Values values = initialCamerasAndPointsEstimate(sfmData); + EXPECT_LONGS_EQUAL(549, values.size()); // regression } /* ************************************************************************* */ TEST(dataSet, readBAL_Dubrovnik) { - ///< The structure where we will save the SfM data + // The structure where we will save the SfM data const string filename = findExampleDataFile("dubrovnik-3-7-pre"); - SfmData mydata; - CHECK(readBAL(filename, mydata)); + SfmData sfmData = SfmData::FromBalFile(filename); // Check number of things - EXPECT_LONGS_EQUAL(3, mydata.numberCameras()); - EXPECT_LONGS_EQUAL(7, mydata.numberTracks()); - const SfmTrack& track0 = mydata.tracks[0]; + EXPECT_LONGS_EQUAL(3, sfmData.numberCameras()); + EXPECT_LONGS_EQUAL(7, sfmData.numberTracks()); + const SfmTrack& track0 = sfmData.tracks[0]; EXPECT_LONGS_EQUAL(3, track0.numberMeasurements()); // Check projection of a given point EXPECT_LONGS_EQUAL(0, track0.measurements[0].first); - const SfmCamera& camera0 = mydata.cameras[0]; + const SfmCamera& camera0 = sfmData.cameras[0]; Point2 expected = camera0.project(track0.p), actual = track0.measurements[0].second; EXPECT(assert_equal(expected, actual, 12)); @@ -105,18 +114,15 @@ TEST(dataSet, gtsam2openGL) { /* ************************************************************************* */ TEST(dataSet, writeBAL_Dubrovnik) { - ///< Read a file using the unit tested readBAL const string filenameToRead = findExampleDataFile("dubrovnik-3-7-pre"); - SfmData readData; - readBAL(filenameToRead, readData); + SfmData readData = SfmData::FromBalFile(filenameToRead); // Write readData to file filenameToWrite const string filenameToWrite = createRewrittenFileName(filenameToRead); CHECK(writeBAL(filenameToWrite, readData)); // Read what we wrote - SfmData writtenData; - CHECK(readBAL(filenameToWrite, writtenData)); + SfmData writtenData = SfmData::FromBalFile(filenameToWrite); // Check that what we read is the same as what we wrote EXPECT_LONGS_EQUAL(readData.numberCameras(), writtenData.numberCameras()); @@ -154,31 +160,28 @@ TEST(dataSet, writeBAL_Dubrovnik) { /* ************************************************************************* */ TEST(dataSet, writeBALfromValues_Dubrovnik) { - ///< Read a file using the unit tested readBAL const string filenameToRead = findExampleDataFile("dubrovnik-3-7-pre"); - SfmData readData; - readBAL(filenameToRead, readData); + SfmData readData = SfmData::FromBalFile(filenameToRead); Pose3 poseChange = Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10), Point3(0.3, 0.1, 0.3)); - Values value; + Values values; for (size_t i = 0; i < readData.numberCameras(); i++) { // for each camera Pose3 pose = poseChange.compose(readData.cameras[i].pose()); - value.insert(X(i), pose); + values.insert(i, pose); } for (size_t j = 0; j < readData.numberTracks(); j++) { // for each point Point3 point = poseChange.transformFrom(readData.tracks[j].p); - value.insert(P(j), point); + values.insert(P(j), point); } // Write values and readData to a file const string filenameToWrite = createRewrittenFileName(filenameToRead); - writeBALfromValues(filenameToWrite, readData, value); + writeBALfromValues(filenameToWrite, readData, values); // Read the file we wrote - SfmData writtenData; - readBAL(filenameToWrite, writtenData); + SfmData writtenData = SfmData::FromBalFile(filenameToWrite); // Check that the reprojection errors are the same and the poses are correct // Check number of things @@ -195,11 +198,11 @@ TEST(dataSet, writeBALfromValues_Dubrovnik) { EXPECT(assert_equal(expected, actual, 12)); Pose3 expectedPose = camera0.pose(); - Pose3 actualPose = value.at(X(0)); + Pose3 actualPose = values.at(0); EXPECT(assert_equal(expectedPose, actualPose, 1e-7)); Point3 expectedPoint = track0.p; - Point3 actualPoint = value.at(P(0)); + Point3 actualPoint = values.at(P(0)); EXPECT(assert_equal(expectedPoint, actualPoint, 1e-6)); } diff --git a/gtsam/slam/tests/testEssentialMatrixFactor.cpp b/gtsam/slam/tests/testEssentialMatrixFactor.cpp index 1a5c64c8c..ef22bad2a 100644 --- a/gtsam/slam/tests/testEssentialMatrixFactor.cpp +++ b/gtsam/slam/tests/testEssentialMatrixFactor.cpp @@ -15,6 +15,7 @@ #include #include #include +#include #include using namespace std::placeholders; @@ -34,8 +35,7 @@ gtsam::Rot3 cRb = gtsam::Rot3(bX, bZ, -bY).inverse(); namespace example1 { const string filename = findExampleDataFile("18pointExample1.txt"); -SfmData data; -bool readOK = readBAL(filename, data); +SfmData data = SfmData::FromBalFile(filename); Rot3 c1Rc2 = data.cameras[1].pose().rotation(); Point3 c1Tc2 = data.cameras[1].pose().translation(); // TODO: maybe default value not good; assert with 0th @@ -53,8 +53,6 @@ Vector vB(size_t i) { return EssentialMatrix::Homogeneous(pB(i)); } //************************************************************************* TEST(EssentialMatrixFactor, testData) { - CHECK(readOK); - // Check E matrix Matrix expected(3, 3); expected << 0, 0, 0, 0, 0, -0.1, 0.1, 0, 0; @@ -538,8 +536,7 @@ TEST(EssentialMatrixFactor4, minimizationWithStrongCal3BundlerPrior) { namespace example2 { const string filename = findExampleDataFile("5pointExample2.txt"); -SfmData data; -bool readOK = readBAL(filename, data); +SfmData data = SfmData::FromBalFile(filename); Rot3 aRb = data.cameras[1].pose().rotation(); Point3 aTb = data.cameras[1].pose().translation(); EssentialMatrix trueE(aRb, Unit3(aTb)); diff --git a/gtsam/slam/tests/testSerializationDataset.cpp b/gtsam/slam/tests/testSerializationDataset.cpp index 6ef82f07f..dcac3d47e 100644 --- a/gtsam/slam/tests/testSerializationDataset.cpp +++ b/gtsam/slam/tests/testSerializationDataset.cpp @@ -16,6 +16,7 @@ * @date Jan 1, 2021 */ +#include #include #include @@ -29,8 +30,7 @@ using namespace gtsam::serializationTestHelpers; TEST(dataSet, sfmDataSerialization) { // Test the serialization of SfmData const string filename = findExampleDataFile("dubrovnik-3-7-pre"); - SfmData mydata; - CHECK(readBAL(filename, mydata)); + SfmData mydata = SfmData::FromBalFile(filename); // round-trip equality check on serialization and subsequent deserialization EXPECT(equalsObj(mydata)); @@ -42,8 +42,7 @@ TEST(dataSet, sfmDataSerialization) { TEST(dataSet, sfmTrackSerialization) { // Test the serialization of SfmTrack const string filename = findExampleDataFile("dubrovnik-3-7-pre"); - SfmData mydata; - CHECK(readBAL(filename, mydata)); + SfmData mydata = SfmData::FromBalFile(filename); SfmTrack track = mydata.track(0); diff --git a/python/gtsam/examples/SFMExample_bal.py b/python/gtsam/examples/SFMExample_bal.py index 8db1c2cb4..3d71590a9 100644 --- a/python/gtsam/examples/SFMExample_bal.py +++ b/python/gtsam/examples/SFMExample_bal.py @@ -15,9 +15,9 @@ import logging import sys import gtsam -from gtsam import (GeneralSFMFactorCal3Bundler, +from gtsam import (GeneralSFMFactorCal3Bundler, SfmData, PriorFactorPinholeCameraCal3Bundler, PriorFactorPoint3) -from gtsam.symbol_shorthand import C, P # type: ignore +from gtsam.symbol_shorthand import P # type: ignore from gtsam.utils import plot # type: ignore from matplotlib import pyplot as plt @@ -26,12 +26,11 @@ logging.basicConfig(stream=sys.stdout, level=logging.INFO) DEFAULT_BAL_DATASET = "dubrovnik-3-7-pre" -def plot_scene(scene_data: gtsam.SfmData, result: gtsam.Values) -> None: +def plot_scene(scene_data: SfmData, result: gtsam.Values) -> None: """Plot the SFM results.""" plot_vals = gtsam.Values() - for cam_idx in range(scene_data.numberCameras()): - plot_vals.insert(C(cam_idx), - result.atPinholeCameraCal3Bundler(C(cam_idx)).pose()) + for i in range(scene_data.numberCameras()): + plot_vals.insert(i, result.atPinholeCameraCal3Bundler(i).pose()) for j in range(scene_data.numberTracks()): plot_vals.insert(P(j), result.atPoint3(P(j))) @@ -46,7 +45,7 @@ def run(args: argparse.Namespace) -> None: input_file = args.input_file # Load the SfM data from file - scene_data = gtsam.readBal(input_file) + scene_data = SfmData.FromBalFile(input_file) logging.info("read %d tracks on %d cameras\n", scene_data.numberTracks(), scene_data.numberCameras()) @@ -64,12 +63,12 @@ def run(args: argparse.Namespace) -> None: # i represents the camera index, and uv is the 2d measurement i, uv = track.measurement(m_idx) # note use of shorthand symbols C and P - graph.add(GeneralSFMFactorCal3Bundler(uv, noise, C(i), P(j))) + graph.add(GeneralSFMFactorCal3Bundler(uv, noise, i, P(j))) # Add a prior on pose x1. This indirectly specifies where the origin is. graph.push_back( PriorFactorPinholeCameraCal3Bundler( - C(0), scene_data.camera(0), + 0, scene_data.camera(0), gtsam.noiseModel.Isotropic.Sigma(9, 0.1))) # Also add a prior on the position of the first landmark to fix the scale graph.push_back( @@ -82,9 +81,9 @@ def run(args: argparse.Namespace) -> None: i = 0 # add each PinholeCameraCal3Bundler - for cam_idx in range(scene_data.numberCameras()): - camera = scene_data.camera(cam_idx) - initial.insert(C(i), camera) + for i in range(scene_data.numberCameras()): + camera = scene_data.camera(i) + initial.insert(i, camera) i += 1 # add each SfmTrack diff --git a/python/gtsam/examples/SimpleRotation.py b/python/gtsam/examples/SimpleRotation.py index 0fef261f8..3d5fd9e45 100644 --- a/python/gtsam/examples/SimpleRotation.py +++ b/python/gtsam/examples/SimpleRotation.py @@ -31,7 +31,7 @@ def main(): - A measurement model with the correct dimensionality for the factor """ prior = gtsam.Rot2.fromAngle(np.deg2rad(30)) - prior.print_('goal angle') + prior.print('goal angle') model = gtsam.noiseModel.Isotropic.Sigma(dim=1, sigma=np.deg2rad(1)) key = X(1) factor = gtsam.PriorFactorRot2(key, prior, model) @@ -48,7 +48,7 @@ def main(): """ graph = gtsam.NonlinearFactorGraph() graph.push_back(factor) - graph.print_('full graph') + graph.print('full graph') """ Step 3: Create an initial estimate @@ -65,7 +65,7 @@ def main(): """ initial = gtsam.Values() initial.insert(key, gtsam.Rot2.fromAngle(np.deg2rad(20))) - initial.print_('initial estimate') + initial.print('initial estimate') """ Step 4: Optimize @@ -77,7 +77,7 @@ def main(): with the final state of the optimization. """ result = gtsam.LevenbergMarquardtOptimizer(graph, initial).optimize() - result.print_('final result') + result.print('final result') if __name__ == '__main__': diff --git a/python/gtsam/examples/VisualISAMExample.py b/python/gtsam/examples/VisualISAMExample.py index f99d3f3e6..9691b3c46 100644 --- a/python/gtsam/examples/VisualISAMExample.py +++ b/python/gtsam/examples/VisualISAMExample.py @@ -10,8 +10,6 @@ A visualSLAM example for the structure-from-motion problem on a simulated datase This version uses iSAM to solve the problem incrementally """ -from __future__ import print_function - import numpy as np import gtsam from gtsam.examples import SFMdata @@ -94,7 +92,7 @@ def main(): current_estimate = isam.estimate() print('*' * 50) print('Frame {}:'.format(i)) - current_estimate.print_('Current estimate: ') + current_estimate.print('Current estimate: ') # Clear the factor graph and values for the next iteration graph.resize(0) diff --git a/python/gtsam/tests/test_SfmData.py b/python/gtsam/tests/test_SfmData.py index 4a45f91ba..18c9ef722 100644 --- a/python/gtsam/tests/test_SfmData.py +++ b/python/gtsam/tests/test_SfmData.py @@ -14,9 +14,9 @@ from __future__ import print_function import unittest -import numpy as np - import gtsam +import numpy as np +from gtsam import Point2, Point3, SfmData, SfmTrack from gtsam.utils.test_case import GtsamTestCase @@ -25,55 +25,97 @@ class TestSfmData(GtsamTestCase): def setUp(self): """Initialize SfmData and SfmTrack""" - self.data = gtsam.SfmData() + self.data = SfmData() # initialize SfmTrack with 3D point - self.tracks = gtsam.SfmTrack() + self.tracks = SfmTrack() def test_tracks(self): """Test functions in SfmTrack""" # measurement is of format (camera_idx, imgPoint) # create arbitrary camera indices for two cameras - i1, i2 = 4,5 + i1, i2 = 4, 5 + # create arbitrary image measurements for cameras i1 and i2 - uv_i1 = gtsam.Point2(12.6, 82) + uv_i1 = Point2(12.6, 82) + # translating point uv_i1 along X-axis - uv_i2 = gtsam.Point2(24.88, 82) + uv_i2 = Point2(24.88, 82) + # add measurements to the track self.tracks.addMeasurement(i1, uv_i1) self.tracks.addMeasurement(i2, uv_i2) + # Number of measurements in the track is 2 self.assertEqual(self.tracks.numberMeasurements(), 2) + # camera_idx in the first measurement of the track corresponds to i1 cam_idx, img_measurement = self.tracks.measurement(0) self.assertEqual(cam_idx, i1) np.testing.assert_array_almost_equal( - gtsam.Point3(0.,0.,0.), + Point3(0., 0., 0.), self.tracks.point3() ) - def test_data(self): """Test functions in SfmData""" # Create new track with 3 measurements - i1, i2, i3 = 3,5,6 - uv_i1 = gtsam.Point2(21.23, 45.64) + i1, i2, i3 = 3, 5, 6 + uv_i1 = Point2(21.23, 45.64) + # translating along X-axis - uv_i2 = gtsam.Point2(45.7, 45.64) - uv_i3 = gtsam.Point2(68.35, 45.64) + uv_i2 = Point2(45.7, 45.64) + uv_i3 = Point2(68.35, 45.64) + # add measurements and arbitrary point to the track measurements = [(i1, uv_i1), (i2, uv_i2), (i3, uv_i3)] - pt = gtsam.Point3(1.0, 6.0, 2.0) - track2 = gtsam.SfmTrack(pt) + pt = Point3(1.0, 6.0, 2.0) + track2 = SfmTrack(pt) track2.addMeasurement(i1, uv_i1) track2.addMeasurement(i2, uv_i2) track2.addMeasurement(i3, uv_i3) self.data.addTrack(self.tracks) self.data.addTrack(track2) + # Number of tracks in SfmData is 2 self.assertEqual(self.data.numberTracks(), 2) + # camera idx of first measurement of second track corresponds to i1 cam_idx, img_measurement = self.data.track(1).measurement(0) self.assertEqual(cam_idx, i1) + def test_Balbianello(self): + """ Check that we can successfully read a bundler file and create a + factor graph from it + """ + # The structure where we will save the SfM data + filename = gtsam.findExampleDataFile("Balbianello.out") + sfm_data = SfmData.FromBundlerFile(filename) + + # Check number of things + self.assertEqual(5, sfm_data.numberCameras()) + self.assertEqual(544, sfm_data.numberTracks()) + track0 = sfm_data.track(0) + self.assertEqual(3, track0.numberMeasurements()) + + # Check projection of a given point + self.assertEqual(0, track0.measurement(0)[0]) + camera0 = sfm_data.camera(0) + expected = camera0.project(track0.point3()) + actual = track0.measurement(0)[1] + self.gtsamAssertEquals(expected, actual, 1) + + # We share *one* noiseModel between all projection factors + model = gtsam.noiseModel.Isotropic.Sigma( + 2, 1.0) # one pixel in u and v + + # Convert to NonlinearFactorGraph + graph = sfm_data.sfmFactorGraph(model) + self.assertEqual(1419, graph.size()) # regression + + # Get initial estimate + values = gtsam.initialCamerasAndPointsEstimate(sfm_data) + self.assertEqual(549, values.size()) # regression + + if __name__ == '__main__': unittest.main() diff --git a/python/gtsam/utils/visual_data_generator.py b/python/gtsam/utils/visual_data_generator.py index 51852760a..972f25477 100644 --- a/python/gtsam/utils/visual_data_generator.py +++ b/python/gtsam/utils/visual_data_generator.py @@ -1,12 +1,12 @@ from __future__ import print_function -from typing import Tuple import math -import numpy as np from math import pi +from typing import Tuple import gtsam -from gtsam import Point3, Pose3, PinholeCameraCal3_S2, Cal3_S2 +import numpy as np +from gtsam import Cal3_S2, PinholeCameraCal3_S2, Point3, Pose3 class Options: @@ -36,7 +36,7 @@ class GroundTruth: self.cameras = [Pose3()] * nrCameras self.points = [Point3(0, 0, 0)] * nrPoints - def print_(self, s="") -> None: + def print(self, s: str = "") -> None: print(s) print("K = ", self.K) print("Cameras: ", len(self.cameras)) @@ -88,7 +88,8 @@ def generate_data(options) -> Tuple[Data, GroundTruth]: r = 10 for j in range(len(truth.points)): theta = j * 2 * pi / nrPoints - truth.points[j] = Point3(r * math.cos(theta), r * math.sin(theta), 0) + truth.points[j] = Point3( + r * math.cos(theta), r * math.sin(theta), 0) else: # 3D landmarks as vertices of a cube truth.points = [ Point3(10, 10, 10), Point3(-10, 10, 10), diff --git a/tests/testGeneralSFMFactorB.cpp b/tests/testGeneralSFMFactorB.cpp index 0bf609adc..fa27e1370 100644 --- a/tests/testGeneralSFMFactorB.cpp +++ b/tests/testGeneralSFMFactorB.cpp @@ -15,6 +15,7 @@ * @brief test general SFM class, with nonlinear optimization and BAL files */ +#include #include #include #include @@ -42,9 +43,7 @@ using symbol_shorthand::P; /* ************************************************************************* */ TEST(PinholeCamera, BAL) { string filename = findExampleDataFile("dubrovnik-3-7-pre"); - SfmData db; - bool success = readBAL(filename, db); - if (!success) throw runtime_error("Could not access file!"); + SfmData db = SfmData::FromBalFile(filename); SharedNoiseModel unit2 = noiseModel::Unit::Create(2); NonlinearFactorGraph graph; diff --git a/tests/testTranslationRecovery.cpp b/tests/testTranslationRecovery.cpp index 2915a375e..4fe643047 100644 --- a/tests/testTranslationRecovery.cpp +++ b/tests/testTranslationRecovery.cpp @@ -18,6 +18,7 @@ #include #include +#include #include using namespace std; @@ -42,9 +43,7 @@ Unit3 GetDirectionFromPoses(const Values& poses, // sets up an optimization problem for the three unknown translations. TEST(TranslationRecovery, BAL) { const string filename = findExampleDataFile("dubrovnik-3-7-pre"); - SfmData db; - bool success = readBAL(filename, db); - if (!success) throw runtime_error("Could not access file!"); + SfmData db = SfmData::FromBalFile(filename); // Get camera poses, as Values size_t j = 0; diff --git a/timing/timeSFMBAL.h b/timing/timeSFMBAL.h index 347500cd2..79d7432c8 100644 --- a/timing/timeSFMBAL.h +++ b/timing/timeSFMBAL.h @@ -16,7 +16,7 @@ * @date July 5, 2015 */ -#include +#include #include #include #include @@ -54,9 +54,7 @@ SfmData preamble(int argc, char* argv[]) { filename = argv[argc - 1]; else filename = findExampleDataFile("dubrovnik-16-22106-pre"); - bool success = readBAL(filename, db); - if (!success) throw runtime_error("Could not access file!"); - return db; + return SfmData::FromBalFile(filename); } // Create ordering and optimize diff --git a/timing/timeSFMBALautodiff.cpp b/timing/timeSFMBALautodiff.cpp index 083d36729..1a7e35929 100644 --- a/timing/timeSFMBALautodiff.cpp +++ b/timing/timeSFMBALautodiff.cpp @@ -59,7 +59,7 @@ int main(int argc, char* argv[]) { Values initial; size_t i = 0, j = 0; for (const SfmCamera& camera: db.cameras) { - // readBAL converts to GTSAM format, so we need to convert back ! + // SfmData::FromBalFile converts to GTSAM format, so we need to convert back ! Pose3 openGLpose = gtsam2openGL(camera.pose()); Vector9 v9; v9 << Pose3::Logmap(openGLpose), camera.calibration();