From b51b038028e9bb2862fd75e91a81b26081f22912 Mon Sep 17 00:00:00 2001 From: Luca Carlone Date: Fri, 18 Oct 2013 01:25:13 +0000 Subject: [PATCH] moved readBundler code from BundlerUtils to gtsam --- gtsam/slam/dataset.cpp | 102 ++++++++++++++++++++++++ gtsam/slam/dataset.h | 130 ++++++++++++++++++++----------- gtsam/slam/tests/testDataset.cpp | 26 +++++++ 3 files changed, 213 insertions(+), 45 deletions(-) diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index 465b378ff..bda75b536 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -50,6 +50,7 @@ string findExampleDataFile(const string& name) { namesToSearch.push_back(name); namesToSearch.push_back(name + ".graph"); namesToSearch.push_back(name + ".txt"); + namesToSearch.push_back(name + ".out"); // Find first name that exists BOOST_FOREACH(const fs::path& root, rootsToSearch) { @@ -407,4 +408,105 @@ pair load2D_robust( return make_pair(graph, initial); } +/* ************************************************************************* */ +bool readBundler(const string& cad, SfM_data &data) +{ + // Load the data file + ifstream is(cad.c_str(),ifstream::in); + if(!is) + { + cout << "Error in readBundler: can not find the file!!" << endl; + return false; + } + + // Ignore the first line + char aux[500]; + is.getline(aux,500); + + // Get the number of camera poses and 3D points + size_t nposes, npoints; + is >> nposes >> npoints; + + // Get the information for the camera poses + for( size_t i = 0; i < nposes; i++ ) + { + // Get the focal length and the radial distortion parameters + float f, k1, k2; + is >> f >> k1 >> k2; + Cal3DS2 K(f, f, 0, 0, 0, k1, k2); + + // Get the rotation matrix + float r11, r12, r13; + float r21, r22, r23; + float r31, r32, r33; + is >> r11 >> r12 >> r13 + >> r21 >> r22 >> r23 + >> r31 >> r32 >> r33; + + // Bundler-OpenGL rotation matrix + Rot3 R( + r11, r12, r13, + r21, r22, r23, + r31, r32, r33); + + // 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; + } + + // Our camera-to-world rotation matrix wRc' = [e1 -e2 -e3] * R + Rot3 cRw( + r11, r12, r13, + -r21, -r22, -r23, + -r31, -r32, -r33); + Rot3 wRc = cRw.inverse(); + + // Get the translation vector + float tx, ty, tz; + is >> tx >> ty >> tz; + + // Our camera-to-world translation wTc = -R'*t + Pose3 pose(wRc, R.unrotate(Point3(-tx,-ty,-tz))); + + data.cameras.push_back(SfM_Camera(pose,K)); + } + + // Get the information for the 3D points + for( size_t i = 0; i < npoints; i++ ) + { + SfM_Track track; + + // Get the 3D position + float x, y, z; + is >> x >> y >> z; + track.p = Point3(x,y,z); + + // Get the color information + float r, g, b; + is >> r >> g >> b; + track.r = r/255.0; + track.g = g/255.0; + track.b = b/255.0; + + // Now get the visibility information + size_t nvisible = 0; + is >> nvisible; + + for( size_t j = 0; j < nvisible; j++ ) + { + size_t cam_idx = 0, sift_idx = 0; + float u, v; + is >> cam_idx >> sift_idx >> u >> v; + track.measurements.push_back(make_pair(cam_idx,Point2(u,-v))); + } + + data.tracks.push_back(track); + } + + is.close(); + return true; +} + } // \namespace gtsam diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index 053da842a..0abc31033 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -20,62 +20,102 @@ #include #include +#include +#include +#include +#include +#include +#include // for pair #include namespace gtsam { #ifndef MATLAB_MEX_FILE - /** - * Find the full path to an example dataset distributed with gtsam. The name - * may be specified with or without a file extension - if no extension is - * give, this function first looks for the .graph extension, then .txt. We - * first check the gtsam source tree for the file, followed by the installed - * example dataset location. Both the source tree and installed locations - * are obtained from CMake during compilation. - * @return The full path and filename to the requested dataset. - * @throw std::invalid_argument if no matching file could be found using the - * search process described above. - */ - GTSAM_EXPORT std::string findExampleDataFile(const std::string& name); +/** + * Find the full path to an example dataset distributed with gtsam. The name + * may be specified with or without a file extension - if no extension is + * give, this function first looks for the .graph extension, then .txt. We + * first check the gtsam source tree for the file, followed by the installed + * example dataset location. Both the source tree and installed locations + * are obtained from CMake during compilation. + * @return The full path and filename to the requested dataset. + * @throw std::invalid_argument if no matching file could be found using the + * search process described above. + */ +GTSAM_EXPORT std::string findExampleDataFile(const std::string& name); #endif - /** - * Load TORO 2D Graph - * @param dataset/model pair as constructed by [dataset] - * @param maxID if non-zero cut out vertices >= maxID - * @param addNoise add noise to the edges - * @param smart try to reduce complexity of covariance to cheapest model - */ - GTSAM_EXPORT std::pair load2D( - std::pair > dataset, - int maxID = 0, bool addNoise = false, bool smart = true); +/** + * Load TORO 2D Graph + * @param dataset/model pair as constructed by [dataset] + * @param maxID if non-zero cut out vertices >= maxID + * @param addNoise add noise to the edges + * @param smart try to reduce complexity of covariance to cheapest model + */ +GTSAM_EXPORT std::pair load2D( + std::pair > dataset, + int maxID = 0, bool addNoise = false, bool smart = true); - /** - * Load TORO 2D Graph - * @param filename - * @param model optional noise model to use instead of one specified by file - * @param maxID if non-zero cut out vertices >= maxID - * @param addNoise add noise to the edges - * @param smart try to reduce complexity of covariance to cheapest model - */ - GTSAM_EXPORT std::pair load2D( - const std::string& filename, - boost::optional model = boost::optional< - noiseModel::Diagonal::shared_ptr>(), int maxID = 0, bool addNoise = false, - bool smart = true); +/** + * Load TORO 2D Graph + * @param filename + * @param model optional noise model to use instead of one specified by file + * @param maxID if non-zero cut out vertices >= maxID + * @param addNoise add noise to the edges + * @param smart try to reduce complexity of covariance to cheapest model + */ +GTSAM_EXPORT std::pair load2D( + const std::string& filename, + boost::optional model = boost::optional< + noiseModel::Diagonal::shared_ptr>(), int maxID = 0, bool addNoise = false, + bool smart = true); - GTSAM_EXPORT std::pair load2D_robust( - const std::string& filename, - gtsam::noiseModel::Base::shared_ptr& model, int maxID = 0); +GTSAM_EXPORT std::pair load2D_robust( + const std::string& filename, + gtsam::noiseModel::Base::shared_ptr& model, int maxID = 0); - /** save 2d graph */ - GTSAM_EXPORT void save2D(const NonlinearFactorGraph& graph, const Values& config, - const noiseModel::Diagonal::shared_ptr model, const std::string& filename); +/** save 2d graph */ +GTSAM_EXPORT void save2D(const NonlinearFactorGraph& graph, const Values& config, + const noiseModel::Diagonal::shared_ptr model, const std::string& filename); + +/** + * Load TORO 3D Graph + */ +GTSAM_EXPORT bool load3D(const std::string& filename); + +/// A measurement with its camera index +typedef std::pair SfM_Measurement; + +/// Define the structure for the 3D points +struct SfM_Track +{ + gtsam::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)) + size_t number_measurements() const { return measurements.size();} +}; + +/// Define the structure for the camera poses +typedef gtsam::PinholeCamera SfM_Camera; + +/// Define the structure for SfM data +struct SfM_data +{ + std::vector cameras; ///< Set of cameras + std::vector tracks; ///< Sparse set of points + size_t number_cameras() const { return cameras.size();} ///< The number of camera poses + size_t number_tracks() const { return tracks.size();} ///< The number of reconstructed 3D points +}; + +/** + * @brief This function parses a bundler output file and stores the data into a + * SfM_data structure + * @param cad The name of the bundler file + * @param data SfM structure where the data is stored + * @return true if the parsing was successful, false otherwise + */ +bool readBundler(const std::string& cad, SfM_data &data); - /** - * Load TORO 3D Graph - */ - GTSAM_EXPORT bool load3D(const std::string& filename); } // namespace gtsam diff --git a/gtsam/slam/tests/testDataset.cpp b/gtsam/slam/tests/testDataset.cpp index e830c74fd..bb9ba9b16 100644 --- a/gtsam/slam/tests/testDataset.cpp +++ b/gtsam/slam/tests/testDataset.cpp @@ -25,6 +25,7 @@ using namespace std; using namespace gtsam; +/* ************************************************************************* */ TEST(dataSet, findExampleDataFile) { const string expected_end = "examples/Data/example.graph"; const string actual = findExampleDataFile("example"); @@ -33,6 +34,31 @@ TEST(dataSet, findExampleDataFile) { EXPECT(assert_equal(expected_end, actual_end)); } +/* ************************************************************************* */ +TEST( dataSet, Balbianello) +{ + ///< The structure where we will save the SfM data + const string filename = findExampleDataFile("Balbianello"); + SfM_data 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]; + 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]; + Point2 predicted = camera0.project(track0.p), measured = track0.measurements[0].second; + EXPECT(assert_equal(measured,predicted,1)); + + // Check projection-derived error 0.5*(du^2/0.25+dv^2/0.25) + double du = predicted.x() - measured.x(), dv = predicted.y() - measured.y(); + EXPECT_DOUBLES_EQUAL(2.32894391, 2*du*du+2*dv*dv, 0.01); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */