implemented read of BAL datasets and removed redundant code
Luca Carlone
parent
85c52668c6
commit
eff198372b
|
|
@ -62,10 +62,10 @@ string findExampleDataFile(const string& name) {
|
|||
|
||||
// If we did not return already, then we did not find the file
|
||||
throw std::invalid_argument(
|
||||
"gtsam::findExampleDataFile could not find a matching file in\n"
|
||||
SOURCE_TREE_DATASET_DIR " or\n"
|
||||
INSTALLED_DATASET_DIR " named\n" +
|
||||
name + ", " + name + ".graph, or " + name + ".txt");
|
||||
"gtsam::findExampleDataFile could not find a matching file in\n"
|
||||
SOURCE_TREE_DATASET_DIR " or\n"
|
||||
INSTALLED_DATASET_DIR " named\n" +
|
||||
name + ", " + name + ".graph, or " + name + ".txt");
|
||||
}
|
||||
#endif
|
||||
|
||||
|
|
@ -179,7 +179,7 @@ pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D(
|
|||
continue;
|
||||
|
||||
noiseModel::Diagonal::shared_ptr measurementNoise =
|
||||
noiseModel::Diagonal::Sigmas(Vector_(2, bearing_std, range_std));
|
||||
noiseModel::Diagonal::Sigmas(Vector_(2, bearing_std, range_std));
|
||||
*graph += BearingRangeFactor<Pose2, Point2>(id1, id2, bearing, range, measurementNoise);
|
||||
|
||||
// Insert poses or points if they do not exist yet
|
||||
|
|
@ -217,7 +217,7 @@ pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D(
|
|||
{
|
||||
if(!haveLandmark) {
|
||||
cout << "Warning: load2D is a very simple dataset loader and is ignoring the\n"
|
||||
"non-uniform covariance on LANDMARK measurements in this file." << endl;
|
||||
"non-uniform covariance on LANDMARK measurements in this file." << endl;
|
||||
haveLandmark = true;
|
||||
}
|
||||
}
|
||||
|
|
@ -229,7 +229,7 @@ pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D(
|
|||
}
|
||||
|
||||
cout << "load2D read a graph file with " << initial->size()
|
||||
<< " vertices and " << graph->nrFactors() << " factors" << endl;
|
||||
<< " vertices and " << graph->nrFactors() << " factors" << endl;
|
||||
|
||||
return make_pair(graph, initial);
|
||||
}
|
||||
|
|
@ -386,7 +386,7 @@ pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D_robust(
|
|||
continue;
|
||||
|
||||
noiseModel::Diagonal::shared_ptr measurementNoise =
|
||||
noiseModel::Diagonal::Sigmas(Vector_(2, bearing_std, range_std));
|
||||
noiseModel::Diagonal::Sigmas(Vector_(2, bearing_std, range_std));
|
||||
*graph += BearingRangeFactor<Pose2, Point2>(id1, id2, bearing, range, measurementNoise);
|
||||
|
||||
// Insert poses or points if they do not exist yet
|
||||
|
|
@ -403,110 +403,183 @@ pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D_robust(
|
|||
}
|
||||
|
||||
cout << "load2D read a graph file with " << initial->size()
|
||||
<< " vertices and " << graph->nrFactors() << " factors" << endl;
|
||||
<< " vertices and " << graph->nrFactors() << " factors" << endl;
|
||||
|
||||
return make_pair(graph, initial);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
bool readBundler(const string& cad, SfM_data &data)
|
||||
Pose3 openGL2gtsam(const Rot3& R, double tx, double ty, double tz)
|
||||
{
|
||||
// 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;
|
||||
}
|
||||
// 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();
|
||||
|
||||
// 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;
|
||||
// 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(filename.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 nrPoses, nrPoints;
|
||||
is >> nrPoses >> nrPoints;
|
||||
|
||||
// Get the information for the camera poses
|
||||
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;
|
||||
Cal3Bundler K(f, 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;
|
||||
}
|
||||
|
||||
// Get the translation vector
|
||||
float tx, ty, tz;
|
||||
is >> 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 j = 0; j < nrPoints; j++ )
|
||||
{
|
||||
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 k = 0; k < nvisible; k++ )
|
||||
{
|
||||
size_t cam_idx = 0, point_idx = 0;
|
||||
float u, v;
|
||||
is >> cam_idx >> point_idx >> u >> v;
|
||||
track.measurements.push_back(make_pair(cam_idx,Point2(u,-v)));
|
||||
}
|
||||
|
||||
data.tracks.push_back(track);
|
||||
}
|
||||
|
||||
is.close();
|
||||
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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
Loading…
Reference in New Issue