implemented read of BAL datasets and removed redundant code

release/4.3a0
Luca Carlone 2013-10-18 01:25:16 +00:00
parent 85c52668c6
commit eff198372b
3 changed files with 212 additions and 114 deletions

View File

@ -409,10 +409,25 @@ pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D_robust(
}
/* ************************************************************************* */
bool readBundler(const string& cad, SfM_data &data)
Pose3 openGL2gtsam(const Rot3& R, double tx, double ty, double tz)
{
// Our camera-to-world rotation matrix wRc' = [e1 -e2 -e3] * R
Point3 r1 = R.r1(), r2 = R.r2(), r3 = R.r3(); //columns!
Rot3 cRw(
r1.x(), r2.x(), r3.x(),
-r1.y(), -r2.y(), -r3.y(),
-r1.z(), -r2.z(), -r3.z());
Rot3 wRc = cRw.inverse();
// Our camera-to-world translation wTc = -R'*t
return Pose3 (wRc, R.unrotate(Point3(-tx,-ty,-tz)));
}
/* ************************************************************************* */
bool readBundler(const string& filename, SfM_data &data)
{
// Load the data file
ifstream is(cad.c_str(),ifstream::in);
ifstream is(filename.c_str(),ifstream::in);
if(!is)
{
cout << "Error in readBundler: can not find the file!!" << endl;
@ -424,16 +439,16 @@ bool readBundler(const string& cad, SfM_data &data)
is.getline(aux,500);
// Get the number of camera poses and 3D points
size_t nposes, npoints;
is >> nposes >> npoints;
size_t nrPoses, nrPoints;
is >> nrPoses >> nrPoints;
// Get the information for the camera poses
for( size_t i = 0; i < nposes; i++ )
for( size_t i = 0; i < nrPoses; 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);
Cal3Bundler K(f, k1, k2);
// Get the rotation matrix
float r11, r12, r13;
@ -456,25 +471,17 @@ bool readBundler(const string& cad, SfM_data &data)
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)));
Pose3 pose = openGL2gtsam(R,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++ )
for( size_t j = 0; j < nrPoints; j++ )
{
SfM_Track track;
@ -494,11 +501,11 @@ bool readBundler(const string& cad, SfM_data &data)
size_t nvisible = 0;
is >> nvisible;
for( size_t j = 0; j < nvisible; j++ )
for( size_t k = 0; k < nvisible; k++ )
{
size_t cam_idx = 0, sift_idx = 0;
size_t cam_idx = 0, point_idx = 0;
float u, v;
is >> cam_idx >> sift_idx >> u >> v;
is >> cam_idx >> point_idx >> u >> v;
track.measurements.push_back(make_pair(cam_idx,Point2(u,-v)));
}
@ -509,4 +516,70 @@ bool readBundler(const string& cad, SfM_data &data)
return true;
}
/* ************************************************************************* */
bool readBAL(const string& filename, SfM_data &data)
{
// Load the data file
ifstream is(filename.c_str(),ifstream::in);
if(!is)
{
cout << "Error in readBAL: can not find the file!!" << endl;
return false;
}
// Get the number of camera poses and 3D points
size_t nrPoses, nrPoints, nrObservations;
is >> nrPoses >> nrPoints >> nrObservations;
data.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.push_back(make_pair(i,Point2(u,-v)));
}
// Get the information for the camera poses
for( size_t i = 0; i < nrPoses; i++ )
{
// Get the rodriguez vector
float wx, wy, wz;
is >> wx >> wy >> wz;
Rot3 R = Rot3::rodriguez(wx, wy, wz);// BAL-OpenGL rotation matrix
// Get the translation vector
float tx, ty, tz;
is >> tx >> ty >> tz;
Pose3 pose = openGL2gtsam(R,tx,ty,tz);
// Get the focal length and the radial distortion parameters
float f, k1, k2;
is >> f >> k1 >> k2;
Cal3Bundler K(f, k1, k2);
data.cameras.push_back(SfM_Camera(pose,K));
}
// Get the information for the 3D points
for( size_t j = 0; j < nrPoints; j++ )
{
// Get the 3D position
float x, y, z;
is >> x >> y >> z;
SfM_Track& track = data.tracks[j];
track.p = Point3(x,y,z);
track.r = 0.4;
track.g = 0.4;
track.b = 0.4;
}
is.close();
return true;
}
} // \namespace gtsam

View File

@ -22,7 +22,7 @@
#include <gtsam/linear/NoiseModel.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Cal3DS2.h>
#include <gtsam/geometry/Cal3Bundler.h>
#include <gtsam/geometry/PinholeCamera.h>
#include <vector>
@ -97,7 +97,7 @@ struct SfM_Track
};
/// Define the structure for the camera poses
typedef gtsam::PinholeCamera<gtsam::Cal3DS2> SfM_Camera;
typedef gtsam::PinholeCamera<gtsam::Cal3Bundler> SfM_Camera;
/// Define the structure for SfM data
struct SfM_data
@ -111,11 +111,19 @@ struct SfM_data
/**
* @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 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
*/
bool readBundler(const std::string& cad, SfM_data &data);
bool readBundler(const std::string& filename, SfM_data &data);
/**
* @brief This function parses a "Bundle Adjustment in the Large" (BAL) file and stores the data into a
* SfM_data 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
*/
bool readBAL(const std::string& filename, SfM_data &data);
} // namespace gtsam

View File

@ -51,12 +51,29 @@ TEST( dataSet, Balbianello)
// 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));
Point2 expected = camera0.project(track0.p), actual = track0.measurements[0].second;
EXPECT(assert_equal(expected,actual,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);
/* ************************************************************************* */
TEST( dataSet, Dubrovnik)
{
///< The structure where we will save the SfM data
const string filename = findExampleDataFile("dubrovnik-3-7-pre");
SfM_data 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];
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 expected = camera0.project(track0.p), actual = track0.measurements[0].second;
EXPECT(assert_equal(expected,actual,12));
}
/* ************************************************************************* */