Formatting
dellaert
parent
a84a9a67d6
commit
204ddbee5e
|
|
@ -43,7 +43,7 @@ string findExampleDataFile(const string& name) {
|
|||
// Search source tree and installed location
|
||||
vector<string> rootsToSearch;
|
||||
rootsToSearch.push_back(GTSAM_SOURCE_TREE_DATASET_DIR); // Defined by CMake, see gtsam/gtsam/CMakeLists.txt
|
||||
rootsToSearch.push_back(GTSAM_INSTALLED_DATASET_DIR); // Defined by CMake, see gtsam/gtsam/CMakeLists.txt
|
||||
rootsToSearch.push_back(GTSAM_INSTALLED_DATASET_DIR); // Defined by CMake, see gtsam/gtsam/CMakeLists.txt
|
||||
|
||||
// Search for filename as given, and with .graph and .txt extensions
|
||||
vector<string> namesToSearch;
|
||||
|
|
@ -55,32 +55,34 @@ string findExampleDataFile(const string& name) {
|
|||
// Find first name that exists
|
||||
BOOST_FOREACH(const fs::path& root, rootsToSearch) {
|
||||
BOOST_FOREACH(const fs::path& name, namesToSearch) {
|
||||
if(fs::is_regular_file(root / name))
|
||||
if (fs::is_regular_file(root / name))
|
||||
return (root / name).string();
|
||||
}
|
||||
}
|
||||
|
||||
// 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");
|
||||
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");
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
string createRewrittenFileName(const string& name) {
|
||||
// Search source tree and installed location
|
||||
if(!exists(fs::path(name))) {
|
||||
throw std::invalid_argument(
|
||||
"gtsam::createRewrittenFileName could not find a matching file in\n"
|
||||
+ name);
|
||||
if (!exists(fs::path(name))) {
|
||||
throw std::invalid_argument(
|
||||
"gtsam::createRewrittenFileName could not find a matching file in\n"
|
||||
+ name);
|
||||
}
|
||||
|
||||
fs::path p(name);
|
||||
fs::path newpath = fs::path(p.parent_path().string()) / fs::path(p.stem().string() + "-rewritten.txt" );
|
||||
fs::path p(name);
|
||||
fs::path newpath = fs::path(p.parent_path().string())
|
||||
/ fs::path(p.stem().string() + "-rewritten.txt");
|
||||
|
||||
return newpath.string();
|
||||
return newpath.string();
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
||||
|
|
@ -95,7 +97,8 @@ pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D(
|
|||
|
||||
/* ************************************************************************* */
|
||||
pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D(
|
||||
const string& filename, boost::optional<noiseModel::Diagonal::shared_ptr> model, int maxID,
|
||||
const string& filename,
|
||||
boost::optional<noiseModel::Diagonal::shared_ptr> model, int maxID,
|
||||
bool addNoise, bool smart) {
|
||||
cout << "Will try to read " << filename << endl;
|
||||
ifstream is(filename.c_str());
|
||||
|
|
@ -109,7 +112,7 @@ pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D(
|
|||
|
||||
// load the poses
|
||||
while (is) {
|
||||
if(! (is >> tag))
|
||||
if (!(is >> tag))
|
||||
break;
|
||||
|
||||
if ((tag == "VERTEX2") || (tag == "VERTEX")) {
|
||||
|
|
@ -133,7 +136,7 @@ pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D(
|
|||
int id1, id2;
|
||||
bool haveLandmark = false;
|
||||
while (is) {
|
||||
if(! (is >> tag))
|
||||
if (!(is >> tag))
|
||||
break;
|
||||
|
||||
if ((tag == "EDGE2") || (tag == "EDGE") || (tag == "ODOMETRY")) {
|
||||
|
|
@ -144,20 +147,18 @@ pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D(
|
|||
is >> v1 >> v2 >> v3 >> v4 >> v5 >> v6;
|
||||
|
||||
// Try to guess covariance matrix layout
|
||||
Matrix m(3,3);
|
||||
if(v1 != 0.0 && v2 == 0.0 && v3 != 0.0 && v4 != 0.0 && v5 == 0.0 && v6 == 0.0)
|
||||
{
|
||||
Matrix m(3, 3);
|
||||
if (v1 != 0.0 && v2 == 0.0 && v3 != 0.0 && v4 != 0.0 && v5 == 0.0
|
||||
&& v6 == 0.0) {
|
||||
// Looks like [ v1 v2 v5; v2' v3 v6; v5' v6' v4 ]
|
||||
m << v1, v2, v5, v2, v3, v6, v5, v6, v4;
|
||||
}
|
||||
else if(v1 != 0.0 && v2 == 0.0 && v3 == 0.0 && v4 != 0.0 && v5 == 0.0 && v6 != 0.0)
|
||||
{
|
||||
m << v1, v2, v5, v2, v3, v6, v5, v6, v4;
|
||||
} else if (v1 != 0.0 && v2 == 0.0 && v3 == 0.0 && v4 != 0.0 && v5 == 0.0
|
||||
&& v6 != 0.0) {
|
||||
// Looks like [ v1 v2 v3; v2' v4 v5; v3' v5' v6 ]
|
||||
m << v1, v2, v3, v2, v4, v5, v3, v5, v6;
|
||||
}
|
||||
else
|
||||
{
|
||||
throw std::invalid_argument("load2D: unrecognized covariance matrix format in dataset file");
|
||||
m << v1, v2, v3, v2, v4, v5, v3, v5, v6;
|
||||
} else {
|
||||
throw std::invalid_argument(
|
||||
"load2D: unrecognized covariance matrix format in dataset file");
|
||||
}
|
||||
|
||||
// optional filter
|
||||
|
|
@ -203,22 +204,21 @@ pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D(
|
|||
|
||||
// Convert x,y to bearing,range
|
||||
bearing = std::atan2(lmy, lmx);
|
||||
range = std::sqrt(lmx*lmx + lmy*lmy);
|
||||
range = std::sqrt(lmx * lmx + lmy * lmy);
|
||||
|
||||
// In our experience, the x-y covariance on landmark sightings is not very good, so assume
|
||||
// it describes the uncertainty at a range of 10m, and convert that to bearing/range uncertainty.
|
||||
if(std::abs(v1 - v3) < 1e-4)
|
||||
{
|
||||
if (std::abs(v1 - v3) < 1e-4) {
|
||||
bearing_std = sqrt(v1 / 10.0);
|
||||
range_std = sqrt(v1);
|
||||
}
|
||||
else
|
||||
{
|
||||
} else {
|
||||
bearing_std = 1;
|
||||
range_std = 1;
|
||||
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;
|
||||
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;
|
||||
haveLandmark = true;
|
||||
}
|
||||
}
|
||||
|
|
@ -244,7 +244,7 @@ pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D(
|
|||
initial->insert(id1, Pose2());
|
||||
if (!initial->exists(L(id2))) {
|
||||
Pose2 pose = initial->at<Pose2>(id1);
|
||||
Point2 local(cos(bearing)*range,sin(bearing)*range);
|
||||
Point2 local(cos(bearing) * range, sin(bearing) * range);
|
||||
Point2 global = pose.transform_from(local);
|
||||
initial->insert(L(id2), global);
|
||||
}
|
||||
|
|
@ -265,18 +265,16 @@ void save2D(const NonlinearFactorGraph& graph, const Values& config,
|
|||
fstream stream(filename.c_str(), fstream::out);
|
||||
|
||||
// save poses
|
||||
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, config)
|
||||
{
|
||||
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, config) {
|
||||
const Pose2& pose = dynamic_cast<const Pose2&>(key_value.value);
|
||||
stream << "VERTEX2 " << key_value.key << " " << pose.x() << " "
|
||||
<< pose.y() << " " << pose.theta() << endl;
|
||||
stream << "VERTEX2 " << key_value.key << " " << pose.x() << " " << pose.y()
|
||||
<< " " << pose.theta() << endl;
|
||||
}
|
||||
|
||||
// save edges
|
||||
Matrix R = model->R();
|
||||
Matrix RR = trans(R) * R; //prod(trans(R),R);
|
||||
BOOST_FOREACH(boost::shared_ptr<NonlinearFactor> factor_, graph)
|
||||
{
|
||||
BOOST_FOREACH(boost::shared_ptr<NonlinearFactor> factor_, graph) {
|
||||
boost::shared_ptr<BetweenFactor<Pose2> > factor =
|
||||
boost::dynamic_pointer_cast<BetweenFactor<Pose2> >(factor_);
|
||||
if (!factor)
|
||||
|
|
@ -284,9 +282,9 @@ void save2D(const NonlinearFactorGraph& graph, const Values& config,
|
|||
|
||||
Pose2 pose = factor->measured().inverse();
|
||||
stream << "EDGE2 " << factor->key2() << " " << factor->key1() << " "
|
||||
<< pose.x() << " " << pose.y() << " " << pose.theta() << " "
|
||||
<< RR(0, 0) << " " << RR(0, 1) << " " << RR(1, 1) << " "
|
||||
<< RR(2, 2) << " " << RR(0, 2) << " " << RR(1, 2) << endl;
|
||||
<< pose.x() << " " << pose.y() << " " << pose.theta() << " " << RR(0, 0)
|
||||
<< " " << RR(0, 1) << " " << RR(1, 1) << " " << RR(2, 2) << " "
|
||||
<< RR(0, 2) << " " << RR(1, 2) << endl;
|
||||
}
|
||||
|
||||
stream.close();
|
||||
|
|
@ -411,14 +409,15 @@ pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D_robust(
|
|||
|
||||
noiseModel::Diagonal::shared_ptr measurementNoise =
|
||||
noiseModel::Diagonal::Sigmas((Vector(2) << bearing_std, range_std));
|
||||
*graph += BearingRangeFactor<Pose2, Point2>(id1, id2, bearing, range, measurementNoise);
|
||||
*graph += BearingRangeFactor<Pose2, Point2>(id1, id2, bearing, range,
|
||||
measurementNoise);
|
||||
|
||||
// Insert poses or points if they do not exist yet
|
||||
if (!initial->exists(id1))
|
||||
initial->insert(id1, Pose2());
|
||||
if (!initial->exists(id2)) {
|
||||
Pose2 pose = initial->at<Pose2>(id1);
|
||||
Point2 local(cos(bearing)*range,sin(bearing)*range);
|
||||
Point2 local(cos(bearing) * range, sin(bearing) * range);
|
||||
Point2 global = pose.transform_from(local);
|
||||
initial->insert(id2, global);
|
||||
}
|
||||
|
|
@ -427,69 +426,66 @@ 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);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot3 openGLFixedRotation(){ // this is due to different convention for cameras in gtsam and openGL
|
||||
Rot3 openGLFixedRotation() { // this is due to different convention for cameras in gtsam and openGL
|
||||
/* R = [ 1 0 0
|
||||
* 0 -1 0
|
||||
* 0 0 -1]
|
||||
*/
|
||||
Matrix3 R_mat = Matrix3::Zero(3,3);
|
||||
R_mat(0,0) = 1.0; R_mat(1,1) = -1.0; R_mat(2,2) = -1.0;
|
||||
Matrix3 R_mat = Matrix3::Zero(3, 3);
|
||||
R_mat(0, 0) = 1.0;
|
||||
R_mat(1, 1) = -1.0;
|
||||
R_mat(2, 2) = -1.0;
|
||||
return Rot3(R_mat);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Pose3 openGL2gtsam(const Rot3& R, double tx, double ty, double tz)
|
||||
{
|
||||
Pose3 openGL2gtsam(const Rot3& R, double tx, double ty, double tz) {
|
||||
Rot3 R90 = openGLFixedRotation();
|
||||
Rot3 wRc = ( R.inverse() ).compose(R90);
|
||||
Rot3 wRc = (R.inverse()).compose(R90);
|
||||
|
||||
// Our camera-to-world translation wTc = -R'*t
|
||||
return Pose3 (wRc, R.unrotate(Point3(-tx,-ty,-tz)));
|
||||
return Pose3(wRc, R.unrotate(Point3(-tx, -ty, -tz)));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Pose3 gtsam2openGL(const Rot3& R, double tx, double ty, double tz)
|
||||
{
|
||||
Pose3 gtsam2openGL(const Rot3& R, double tx, double ty, double tz) {
|
||||
Rot3 R90 = openGLFixedRotation();
|
||||
Rot3 cRw_openGL = R90.compose( R.inverse() );
|
||||
Point3 t_openGL = cRw_openGL.rotate(Point3(-tx,-ty,-tz));
|
||||
Rot3 cRw_openGL = R90.compose(R.inverse());
|
||||
Point3 t_openGL = cRw_openGL.rotate(Point3(-tx, -ty, -tz));
|
||||
return Pose3(cRw_openGL, t_openGL);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Pose3 gtsam2openGL(const Pose3& PoseGTSAM)
|
||||
{
|
||||
return gtsam2openGL(PoseGTSAM.rotation(), PoseGTSAM.x(), PoseGTSAM.y(), PoseGTSAM.z());
|
||||
Pose3 gtsam2openGL(const Pose3& PoseGTSAM) {
|
||||
return gtsam2openGL(PoseGTSAM.rotation(), PoseGTSAM.x(), PoseGTSAM.y(),
|
||||
PoseGTSAM.z());
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
bool readBundler(const string& filename, SfM_data &data)
|
||||
{
|
||||
bool readBundler(const string& filename, SfM_data &data) {
|
||||
// Load the data file
|
||||
ifstream is(filename.c_str(),ifstream::in);
|
||||
if(!is)
|
||||
{
|
||||
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);
|
||||
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++ )
|
||||
{
|
||||
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;
|
||||
|
|
@ -499,20 +495,15 @@ bool readBundler(const string& filename, SfM_data &data)
|
|||
float r11, r12, r13;
|
||||
float r21, r22, r23;
|
||||
float r31, r32, r33;
|
||||
is >> r11 >> r12 >> r13
|
||||
>> r21 >> r22 >> r23
|
||||
>> 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);
|
||||
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;
|
||||
if (r11 == 0 && r12 == 0 && r13 == 0) {
|
||||
cout << "Error in readBundler: zero rotation matrix for pose " << i
|
||||
<< endl;
|
||||
return false;
|
||||
}
|
||||
|
||||
|
|
@ -520,38 +511,36 @@ bool readBundler(const string& filename, SfM_data &data)
|
|||
float tx, ty, tz;
|
||||
is >> tx >> ty >> tz;
|
||||
|
||||
Pose3 pose = openGL2gtsam(R,tx,ty,tz);
|
||||
Pose3 pose = openGL2gtsam(R, tx, ty, tz);
|
||||
|
||||
data.cameras.push_back(SfM_Camera(pose,K));
|
||||
data.cameras.push_back(SfM_Camera(pose, K));
|
||||
}
|
||||
|
||||
// Get the information for the 3D points
|
||||
for( size_t j = 0; j < nrPoints; j++ )
|
||||
{
|
||||
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);
|
||||
track.p = Point3(x, y, z);
|
||||
|
||||
// Get the color information
|
||||
float r, g, b;
|
||||
is >> r >> g >> b;
|
||||
track.r = r/255.f;
|
||||
track.g = g/255.f;
|
||||
track.b = b/255.f;
|
||||
track.r = r / 255.f;
|
||||
track.g = g / 255.f;
|
||||
track.b = b / 255.f;
|
||||
|
||||
// Now get the visibility information
|
||||
size_t nvisible = 0;
|
||||
is >> nvisible;
|
||||
|
||||
for( size_t k = 0; k < nvisible; k++ )
|
||||
{
|
||||
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)));
|
||||
track.measurements.push_back(make_pair(cam_idx, Point2(u, -v)));
|
||||
}
|
||||
|
||||
data.tracks.push_back(track);
|
||||
|
|
@ -562,11 +551,11 @@ bool readBundler(const string& filename, SfM_data &data)
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
bool readG2o(const std::string& g2oFile, NonlinearFactorGraph& graph, Values& initial,
|
||||
const kernelFunctionType kernelFunction){
|
||||
bool readG2o(const std::string& g2oFile, NonlinearFactorGraph& graph,
|
||||
Values& initial, const kernelFunctionType kernelFunction) {
|
||||
|
||||
ifstream is(g2oFile.c_str());
|
||||
if (!is){
|
||||
if (!is) {
|
||||
throw std::invalid_argument("File not found!");
|
||||
return false;
|
||||
}
|
||||
|
|
@ -574,7 +563,7 @@ bool readG2o(const std::string& g2oFile, NonlinearFactorGraph& graph, Values& in
|
|||
// READ INITIAL GUESS FROM G2O FILE
|
||||
string tag;
|
||||
while (is) {
|
||||
if(! (is >> tag))
|
||||
if (!(is >> tag))
|
||||
break;
|
||||
|
||||
if (tag == "VERTEX_SE2" || tag == "VERTEX2") {
|
||||
|
|
@ -591,7 +580,7 @@ bool readG2o(const std::string& g2oFile, NonlinearFactorGraph& graph, Values& in
|
|||
|
||||
// READ MEASUREMENTS FROM G2O FILE
|
||||
while (is) {
|
||||
if(! (is >> tag))
|
||||
if (!(is >> tag))
|
||||
break;
|
||||
|
||||
if (tag == "EDGE_SE2" || tag == "EDGE2") {
|
||||
|
|
@ -602,23 +591,35 @@ bool readG2o(const std::string& g2oFile, NonlinearFactorGraph& graph, Values& in
|
|||
is >> id1 >> id2 >> x >> y >> yaw;
|
||||
is >> I11 >> I12 >> I13 >> I22 >> I23 >> I33;
|
||||
Pose2 l1Xl2(x, y, yaw);
|
||||
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Precisions((Vector(3) << I11, I22, I33));
|
||||
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Precisions(
|
||||
(Vector(3) << I11, I22, I33));
|
||||
|
||||
switch (kernelFunction) {
|
||||
{case QUADRATIC:
|
||||
NonlinearFactor::shared_ptr factor(new BetweenFactor<Pose2>(id1, id2, l1Xl2, model));
|
||||
{
|
||||
case QUADRATIC:
|
||||
NonlinearFactor::shared_ptr factor(
|
||||
new BetweenFactor<Pose2>(id1, id2, l1Xl2, model));
|
||||
graph.add(factor);
|
||||
break;}
|
||||
{case HUBER:
|
||||
NonlinearFactor::shared_ptr huberFactor(new BetweenFactor<Pose2>(id1, id2, l1Xl2,
|
||||
noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(1.345), model)));
|
||||
break;
|
||||
}
|
||||
{
|
||||
case HUBER:
|
||||
NonlinearFactor::shared_ptr huberFactor(
|
||||
new BetweenFactor<Pose2>(id1, id2, l1Xl2,
|
||||
noiseModel::Robust::Create(
|
||||
noiseModel::mEstimator::Huber::Create(1.345), model)));
|
||||
graph.add(huberFactor);
|
||||
break;}
|
||||
{case TUKEY:
|
||||
NonlinearFactor::shared_ptr tukeyFactor(new BetweenFactor<Pose2>(id1, id2, l1Xl2,
|
||||
noiseModel::Robust::Create(noiseModel::mEstimator::Tukey::Create(4.6851), model)));
|
||||
break;
|
||||
}
|
||||
{
|
||||
case TUKEY:
|
||||
NonlinearFactor::shared_ptr tukeyFactor(
|
||||
new BetweenFactor<Pose2>(id1, id2, l1Xl2,
|
||||
noiseModel::Robust::Create(
|
||||
noiseModel::mEstimator::Tukey::Create(4.6851), model)));
|
||||
graph.add(tukeyFactor);
|
||||
break;}
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
is.ignore(LINESIZE, '\n');
|
||||
|
|
@ -626,31 +627,32 @@ bool readG2o(const std::string& g2oFile, NonlinearFactorGraph& graph, Values& in
|
|||
// Output which kernel is used
|
||||
switch (kernelFunction) {
|
||||
case QUADRATIC:
|
||||
break;
|
||||
break;
|
||||
case HUBER:
|
||||
std::cout << "Robust kernel: Huber" << std::endl; break;
|
||||
std::cout << "Robust kernel: Huber" << std::endl;
|
||||
break;
|
||||
case TUKEY:
|
||||
std::cout << "Robust kernel: Tukey" << std::endl; break;
|
||||
std::cout << "Robust kernel: Tukey" << std::endl;
|
||||
break;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
bool writeG2o(const std::string& filename, const NonlinearFactorGraph& graph, const Values& estimate){
|
||||
bool writeG2o(const std::string& filename, const NonlinearFactorGraph& graph,
|
||||
const Values& estimate) {
|
||||
|
||||
fstream stream(filename.c_str(), fstream::out);
|
||||
|
||||
// save poses
|
||||
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, estimate)
|
||||
{
|
||||
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, estimate) {
|
||||
const Pose2& pose = dynamic_cast<const Pose2&>(key_value.value);
|
||||
stream << "VERTEX_SE2 " << key_value.key << " " << pose.x() << " "
|
||||
<< pose.y() << " " << pose.theta() << endl;
|
||||
}
|
||||
|
||||
// save edges
|
||||
BOOST_FOREACH(boost::shared_ptr<NonlinearFactor> factor_, graph)
|
||||
{
|
||||
BOOST_FOREACH(boost::shared_ptr<NonlinearFactor> factor_, graph) {
|
||||
boost::shared_ptr<BetweenFactor<Pose2> > factor =
|
||||
boost::dynamic_pointer_cast<BetweenFactor<Pose2> >(factor_);
|
||||
if (!factor)
|
||||
|
|
@ -660,25 +662,25 @@ bool writeG2o(const std::string& filename, const NonlinearFactorGraph& graph, co
|
|||
boost::shared_ptr<noiseModel::Diagonal> diagonalModel =
|
||||
boost::dynamic_pointer_cast<noiseModel::Diagonal>(model);
|
||||
if (!diagonalModel)
|
||||
throw std::invalid_argument("writeG2o: invalid noise model (current version assumes diagonal noise model)!");
|
||||
throw std::invalid_argument(
|
||||
"writeG2o: invalid noise model (current version assumes diagonal noise model)!");
|
||||
|
||||
Pose2 pose = factor->measured(); //.inverse();
|
||||
stream << "EDGE_SE2 " << factor->key1() << " " << factor->key2() << " "
|
||||
<< pose.x() << " " << pose.y() << " " << pose.theta() << " "
|
||||
<< diagonalModel->precision(0) << " " << 0.0 << " " << 0.0 << " "
|
||||
<< diagonalModel->precision(1) << " " << 0.0 << " " << diagonalModel->precision(2) << endl;
|
||||
<< diagonalModel->precision(1) << " " << 0.0 << " "
|
||||
<< diagonalModel->precision(2) << endl;
|
||||
}
|
||||
stream.close();
|
||||
return true;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
bool readBAL(const string& filename, SfM_data &data)
|
||||
{
|
||||
bool readBAL(const string& filename, SfM_data &data) {
|
||||
// Load the data file
|
||||
ifstream is(filename.c_str(),ifstream::in);
|
||||
if(!is)
|
||||
{
|
||||
ifstream is(filename.c_str(), ifstream::in);
|
||||
if (!is) {
|
||||
cout << "Error in readBAL: can not find the file!!" << endl;
|
||||
return false;
|
||||
}
|
||||
|
|
@ -690,44 +692,41 @@ bool readBAL(const string& filename, SfM_data &data)
|
|||
data.tracks.resize(nrPoints);
|
||||
|
||||
// Get the information for the observations
|
||||
for( size_t k = 0; k < nrObservations; k++ )
|
||||
{
|
||||
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)));
|
||||
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++ )
|
||||
{
|
||||
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
|
||||
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);
|
||||
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));
|
||||
data.cameras.push_back(SfM_Camera(pose, K));
|
||||
}
|
||||
|
||||
// Get the information for the 3D points
|
||||
for( size_t j = 0; j < nrPoints; j++ )
|
||||
{
|
||||
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.p = Point3(x, y, z);
|
||||
track.r = 0.4f;
|
||||
track.g = 0.4f;
|
||||
track.b = 0.4f;
|
||||
|
|
@ -738,8 +737,7 @@ bool readBAL(const string& filename, SfM_data &data)
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
bool writeBAL(const string& filename, SfM_data &data)
|
||||
{
|
||||
bool writeBAL(const string& filename, SfM_data &data) {
|
||||
// Open the output file
|
||||
ofstream os;
|
||||
os.open(filename.c_str());
|
||||
|
|
@ -750,49 +748,55 @@ bool writeBAL(const string& filename, SfM_data &data)
|
|||
}
|
||||
|
||||
// Write the number of camera poses and 3D points
|
||||
size_t nrObservations=0;
|
||||
for (size_t j = 0; j < data.number_tracks(); j++){
|
||||
size_t nrObservations = 0;
|
||||
for (size_t j = 0; j < data.number_tracks(); j++) {
|
||||
nrObservations += data.tracks[j].number_measurements();
|
||||
}
|
||||
|
||||
// Write observations
|
||||
os << data.number_cameras() << " " << data.number_tracks() << " " << nrObservations << endl;
|
||||
os << data.number_cameras() << " " << data.number_tracks() << " "
|
||||
<< nrObservations << endl;
|
||||
os << endl;
|
||||
|
||||
for (size_t j = 0; j < data.number_tracks(); j++){ // for each 3D point j
|
||||
for (size_t j = 0; j < data.number_tracks(); j++) { // for each 3D point j
|
||||
SfM_Track track = data.tracks[j];
|
||||
|
||||
for(size_t k = 0; k < track.number_measurements(); k++){ // for each observation of the 3D point j
|
||||
for (size_t k = 0; k < track.number_measurements(); k++) { // for each observation of the 3D point j
|
||||
size_t i = track.measurements[k].first; // camera id
|
||||
double u0 = data.cameras[i].calibration().u0();
|
||||
double v0 = data.cameras[i].calibration().v0();
|
||||
|
||||
if(u0 != 0 || v0 != 0){cout<< "writeBAL has not been tested for calibration with nonzero (u0,v0)"<< endl;}
|
||||
if (u0 != 0 || v0 != 0) {
|
||||
cout
|
||||
<< "writeBAL has not been tested for calibration with nonzero (u0,v0)"
|
||||
<< endl;
|
||||
}
|
||||
|
||||
double pixelBALx = track.measurements[k].second.x() - u0; // center of image is the origin
|
||||
double pixelBALy = - (track.measurements[k].second.y() - v0); // center of image is the origin
|
||||
double pixelBALy = -(track.measurements[k].second.y() - v0); // center of image is the origin
|
||||
Point2 pixelMeasurement(pixelBALx, pixelBALy);
|
||||
os << i /*camera id*/ << " " << j /*point id*/ << " "
|
||||
<< pixelMeasurement.x() /*u of the pixel*/ << " " << pixelMeasurement.y() /*v of the pixel*/ << endl;
|
||||
os << i /*camera id*/<< " " << j /*point id*/<< " "
|
||||
<< pixelMeasurement.x() /*u of the pixel*/<< " "
|
||||
<< pixelMeasurement.y() /*v of the pixel*/<< endl;
|
||||
}
|
||||
}
|
||||
os << endl;
|
||||
|
||||
// Write cameras
|
||||
for (size_t i = 0; i < data.number_cameras(); i++){ // for each camera
|
||||
for (size_t i = 0; i < data.number_cameras(); i++) { // for each camera
|
||||
Pose3 poseGTSAM = data.cameras[i].pose();
|
||||
Cal3Bundler cameraCalibration = data.cameras[i].calibration();
|
||||
Pose3 poseOpenGL = gtsam2openGL(poseGTSAM);
|
||||
os << Rot3::Logmap(poseOpenGL.rotation()) << endl;
|
||||
os << poseOpenGL.translation().vector() << endl;
|
||||
os << cameraCalibration.fx() << endl;
|
||||
os << cameraCalibration.k1() << endl;
|
||||
os << cameraCalibration.k2() << endl;
|
||||
os << Rot3::Logmap(poseOpenGL.rotation()) << endl;
|
||||
os << poseOpenGL.translation().vector() << endl;
|
||||
os << cameraCalibration.fx() << endl;
|
||||
os << cameraCalibration.k1() << endl;
|
||||
os << cameraCalibration.k2() << endl;
|
||||
os << endl;
|
||||
}
|
||||
|
||||
// Write the points
|
||||
for (size_t j = 0; j < data.number_tracks(); j++){ // for each 3D point j
|
||||
for (size_t j = 0; j < data.number_tracks(); j++) { // for each 3D point j
|
||||
Point3 point = data.tracks[j].p;
|
||||
os << point.x() << endl;
|
||||
os << point.y() << endl;
|
||||
|
|
@ -804,48 +808,55 @@ bool writeBAL(const string& filename, SfM_data &data)
|
|||
return true;
|
||||
}
|
||||
|
||||
bool writeBALfromValues(const string& filename, const SfM_data &data, Values& values){
|
||||
bool writeBALfromValues(const string& filename, const SfM_data &data,
|
||||
Values& values) {
|
||||
|
||||
SfM_data dataValues = data;
|
||||
|
||||
// Store poses or cameras in SfM_data
|
||||
Values valuesPoses = values.filter<Pose3>();
|
||||
if( valuesPoses.size() == dataValues.number_cameras() ){ // we only estimated camera poses
|
||||
for (size_t i = 0; i < dataValues.number_cameras(); i++){ // for each camera
|
||||
Key poseKey = symbol('x',i);
|
||||
if (valuesPoses.size() == dataValues.number_cameras()) { // we only estimated camera poses
|
||||
for (size_t i = 0; i < dataValues.number_cameras(); i++) { // for each camera
|
||||
Key poseKey = symbol('x', i);
|
||||
Pose3 pose = values.at<Pose3>(poseKey);
|
||||
Cal3Bundler K = dataValues.cameras[i].calibration();
|
||||
PinholeCamera<Cal3Bundler> camera(pose, K);
|
||||
dataValues.cameras[i] = camera;
|
||||
}
|
||||
} else {
|
||||
Values valuesCameras = values.filter< PinholeCamera<Cal3Bundler> >();
|
||||
if ( valuesCameras.size() == dataValues.number_cameras() ){ // we only estimated camera poses and calibration
|
||||
for (size_t i = 0; i < dataValues.number_cameras(); i++){ // for each camera
|
||||
Values valuesCameras = values.filter<PinholeCamera<Cal3Bundler> >();
|
||||
if (valuesCameras.size() == dataValues.number_cameras()) { // we only estimated camera poses and calibration
|
||||
for (size_t i = 0; i < dataValues.number_cameras(); i++) { // for each camera
|
||||
Key cameraKey = i; // symbol('c',i);
|
||||
PinholeCamera<Cal3Bundler> camera = values.at<PinholeCamera<Cal3Bundler> >(cameraKey);
|
||||
PinholeCamera<Cal3Bundler> camera =
|
||||
values.at<PinholeCamera<Cal3Bundler> >(cameraKey);
|
||||
dataValues.cameras[i] = camera;
|
||||
}
|
||||
}else{
|
||||
cout << "writeBALfromValues: different number of cameras in SfM_dataValues (#cameras= " << dataValues.number_cameras()
|
||||
<<") and values (#cameras " << valuesPoses.size() << ", #poses " << valuesCameras.size() << ")!!" << endl;
|
||||
} else {
|
||||
cout
|
||||
<< "writeBALfromValues: different number of cameras in SfM_dataValues (#cameras= "
|
||||
<< dataValues.number_cameras() << ") and values (#cameras "
|
||||
<< valuesPoses.size() << ", #poses " << valuesCameras.size() << ")!!"
|
||||
<< endl;
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
// Store 3D points in SfM_data
|
||||
Values valuesPoints = values.filter<Point3>();
|
||||
if( valuesPoints.size() != dataValues.number_tracks()){
|
||||
cout << "writeBALfromValues: different number of points in SfM_dataValues (#points= " << dataValues.number_tracks()
|
||||
<<") and values (#points " << valuesPoints.size() << ")!!" << endl;
|
||||
if (valuesPoints.size() != dataValues.number_tracks()) {
|
||||
cout
|
||||
<< "writeBALfromValues: different number of points in SfM_dataValues (#points= "
|
||||
<< dataValues.number_tracks() << ") and values (#points "
|
||||
<< valuesPoints.size() << ")!!" << endl;
|
||||
}
|
||||
|
||||
for (size_t j = 0; j < dataValues.number_tracks(); j++){ // for each point
|
||||
for (size_t j = 0; j < dataValues.number_tracks(); j++) { // for each point
|
||||
Key pointKey = P(j);
|
||||
if(values.exists(pointKey)){
|
||||
if (values.exists(pointKey)) {
|
||||
Point3 point = values.at<Point3>(pointKey);
|
||||
dataValues.tracks[j].p = point;
|
||||
}else{
|
||||
} else {
|
||||
dataValues.tracks[j].r = 1.0;
|
||||
dataValues.tracks[j].g = 0.0;
|
||||
dataValues.tracks[j].b = 0.0;
|
||||
|
|
@ -861,7 +872,7 @@ Values initialCamerasEstimate(const SfM_data& db) {
|
|||
Values initial;
|
||||
size_t i = 0; // NO POINTS: j = 0;
|
||||
BOOST_FOREACH(const SfM_Camera& camera, db.cameras)
|
||||
initial.insert(i++, camera);
|
||||
initial.insert(i++, camera);
|
||||
return initial;
|
||||
}
|
||||
|
||||
|
|
@ -869,9 +880,9 @@ Values initialCamerasAndPointsEstimate(const SfM_data& db) {
|
|||
Values initial;
|
||||
size_t i = 0, j = 0;
|
||||
BOOST_FOREACH(const SfM_Camera& camera, db.cameras)
|
||||
initial.insert((i++), camera);
|
||||
initial.insert((i++), camera);
|
||||
BOOST_FOREACH(const SfM_Track& track, db.tracks)
|
||||
initial.insert(P(j++), track.p);
|
||||
initial.insert(P(j++), track.p);
|
||||
return initial;
|
||||
}
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue