moved readBundler code from BundlerUtils to gtsam
Luca Carlone
parent
2a8ecd1e57
commit
b51b038028
|
|
@ -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<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> 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
|
||||
|
|
|
|||
|
|
@ -20,62 +20,102 @@
|
|||
|
||||
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||
#include <gtsam/linear/NoiseModel.h>
|
||||
#include <gtsam/geometry/Point2.h>
|
||||
#include <gtsam/geometry/Pose3.h>
|
||||
#include <gtsam/geometry/Cal3DS2.h>
|
||||
#include <gtsam/geometry/PinholeCamera.h>
|
||||
|
||||
#include <vector>
|
||||
#include <utility> // for pair
|
||||
#include <string>
|
||||
|
||||
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<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D(
|
||||
std::pair<std::string, boost::optional<noiseModel::Diagonal::shared_ptr> > 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<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D(
|
||||
std::pair<std::string, boost::optional<noiseModel::Diagonal::shared_ptr> > 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<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D(
|
||||
const std::string& filename,
|
||||
boost::optional<gtsam::SharedDiagonal> 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<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D(
|
||||
const std::string& filename,
|
||||
boost::optional<gtsam::SharedDiagonal> model = boost::optional<
|
||||
noiseModel::Diagonal::shared_ptr>(), int maxID = 0, bool addNoise = false,
|
||||
bool smart = true);
|
||||
|
||||
GTSAM_EXPORT std::pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D_robust(
|
||||
const std::string& filename,
|
||||
gtsam::noiseModel::Base::shared_ptr& model, int maxID = 0);
|
||||
GTSAM_EXPORT std::pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> 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<size_t,gtsam::Point2> 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<SfM_Measurement> 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<gtsam::Cal3DS2> SfM_Camera;
|
||||
|
||||
/// 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
|
||||
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
|
||||
|
|
|
|||
|
|
@ -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); }
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
Loading…
Reference in New Issue