diff --git a/.cproject b/.cproject index ec6a2bf6f..b075667ed 100644 --- a/.cproject +++ b/.cproject @@ -311,6 +311,14 @@ true true + + make + -j2 + testGaussianFactor.run + true + true + true + make -j2 @@ -337,7 +345,6 @@ make - tests/testBayesTree.run true false @@ -345,7 +352,6 @@ make - testBinaryBayesNet.run true false @@ -393,7 +399,6 @@ make - testSymbolicBayesNet.run true false @@ -401,7 +406,6 @@ make - tests/testSymbolicFactor.run true false @@ -409,7 +413,6 @@ make - testSymbolicFactorGraph.run true false @@ -425,20 +428,11 @@ make - tests/testBayesTree true false true - - make - -j2 - testGaussianFactor.run - true - true - true - make -j5 @@ -511,22 +505,6 @@ false true - - make - -j2 - tests/testPose2.run - true - true - true - - - make - -j2 - tests/testPose3.run - true - true - true - make -j2 @@ -543,6 +521,22 @@ true true + + make + -j2 + tests/testPose2.run + true + true + true + + + make + -j2 + tests/testPose3.run + true + true + true + make -j2 @@ -567,26 +561,18 @@ true true - + make - -j2 - all + -j5 + testValues.run true true true - + make - -j2 - check - true - true - true - - - make - -j2 - clean + -j5 + testOrdering.run true true true @@ -671,18 +657,26 @@ true true - + make - -j5 - testValues.run + -j2 + all true true true - + make - -j5 - testOrdering.run + -j2 + check + true + true + true + + + make + -j2 + clean true true true @@ -921,7 +915,6 @@ make - testGraph.run true false @@ -929,7 +922,6 @@ make - testJunctionTree.run true false @@ -937,7 +929,6 @@ make - testSymbolicBayesNetB.run true false @@ -1049,7 +1040,6 @@ make - testErrors.run true false @@ -1505,6 +1495,7 @@ make + testSimulated2DOriented.run true false @@ -1544,6 +1535,7 @@ make + testSimulated2D.run true false @@ -1551,6 +1543,7 @@ make + testSimulated3D.run true false @@ -1748,10 +1741,10 @@ true true - + make -j5 - Pose2SLAMwSPCG_advanced.run + Pose2SLAMExample_graph.run true true true @@ -1766,6 +1759,7 @@ make + tests/testGaussianISAM2 true false @@ -1787,6 +1781,102 @@ true true + + make + -j2 + testRot3.run + true + true + true + + + make + -j2 + testRot2.run + true + true + true + + + make + -j2 + testPose3.run + true + true + true + + + make + -j2 + timeRot3.run + true + true + true + + + make + -j2 + testPose2.run + true + true + true + + + make + -j2 + testCal3_S2.run + true + true + true + + + make + -j2 + testSimpleCamera.run + true + true + true + + + make + -j2 + testHomography2.run + true + true + true + + + make + -j2 + testCalibratedCamera.run + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + testPoint2.run + true + true + true + make -j2 @@ -1988,7 +2078,6 @@ cpack - -G DEB true false @@ -1996,7 +2085,6 @@ cpack - -G RPM true false @@ -2004,7 +2092,6 @@ cpack - -G TGZ true false @@ -2012,7 +2099,6 @@ cpack - --config CPackSourceConfig.cmake true false @@ -2066,98 +2152,42 @@ true true - + make - -j2 - testRot3.run + -j5 + wrap.testSpirit.run true true true - + make - -j2 - testRot2.run + -j5 + wrap.testWrap.run true true true - + make - -j2 - testPose3.run + -j5 + check.wrap true true true - + make - -j2 - timeRot3.run + -j5 + wrap_gtsam true true true - + make - -j2 - testPose2.run - true - true - true - - - make - -j2 - testCal3_S2.run - true - true - true - - - make - -j2 - testSimpleCamera.run - true - true - true - - - make - -j2 - testHomography2.run - true - true - true - - - make - -j2 - testCalibratedCamera.run - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - testPoint2.run + -j5 + wrap true true true @@ -2201,46 +2231,6 @@ false true - - make - -j5 - wrap.testSpirit.run - true - true - true - - - make - -j5 - wrap.testWrap.run - true - true - true - - - make - -j5 - check.wrap - true - true - true - - - make - -j5 - wrap_gtsam - true - true - true - - - make - -j5 - wrap - true - true - true - diff --git a/examples/Pose2SLAMExample_graph.cpp b/examples/Pose2SLAMExample_graph.cpp new file mode 100644 index 000000000..6abeceb55 --- /dev/null +++ b/examples/Pose2SLAMExample_graph.cpp @@ -0,0 +1,53 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file Pose2SLAMExample_graph->cpp + * @brief Read graph from file and perform GraphSLAM + * @date June 3, 2012 + * @author Frank Dellaert + */ + +#include +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; + +int main(int argc, char** argv) { + + // Read File and create graph and initial estimate + // we are in build/examples, data is in examples/Data + pose2SLAM::Graph::shared_ptr graph ; + pose2SLAM::Values::shared_ptr initial; + SharedDiagonal model(Vector_(3, 0.05, 0.05, 5.0*M_PI/180.0)); + boost::tie(graph,initial) = load2D("../../examples/Data/w100-odom.graph",model); + initial->print("Initial estimate:\n"); + + // Add a Gaussian prior on first poses + Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin + SharedDiagonal priorNoise(Vector_(3, 0.01, 0.01, 0.01)); + graph->addPrior(0, priorMean, priorNoise); + + // Single Step Optimization using Levenberg-Marquardt + pose2SLAM::Values result = graph->optimize(*initial); + result.print("\nFinal result:\n"); + + // Plot the covariance of the last pose + Marginals marginals(*graph, result); + cout.precision(2); + cout << "\nP3:\n" << marginals.marginalCovariance(99) << endl; + +return 0; +} diff --git a/examples/matlab/Pose2SLAMExample_graph.m b/examples/matlab/Pose2SLAMExample_graph.m index af0b1f3fb..7118efe01 100644 --- a/examples/matlab/Pose2SLAMExample_graph.m +++ b/examples/matlab/Pose2SLAMExample_graph.m @@ -10,64 +10,30 @@ % @author Frank Dellaert %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -%% Create graph container and add factors to it -graph = pose2SLAMGraph; -initial = pose2SLAMValues; -odometryNoise = gtsamSharedNoiseModel_Sigmas([0.2; 0.2; 0.1]); -constraint=gtsamPose2; % identity +%% Initialize graph, initial estimate, and odometry noise +model = gtsamSharedNoiseModel_Sigmas([0.05; 0.05; 5*pi/180]); +[graph,initial]=load2D('../Data/w100-odom.graph',model); +initial.print('Initial estimate:\n'); %% Add a Gaussian prior on pose x_1 priorMean = gtsamPose2(0.0, 0.0, 0.0); % prior mean is at origin -priorNoise = gtsamSharedNoiseModel_Sigmas([0.3; 0.3; 0.1]); % 30cm std on x,y, 0.1 rad on theta +priorNoise = gtsamSharedNoiseModel_Sigmas([0.01; 0.01; 0.01]); graph.addPrior(0, priorMean, priorNoise); % add directly to graph -%% Read File and create graph and initial estimate -fid = fopen('../Data/w100-odom.graph'); -if fid < 0 - error('Cannot open the file '); -end - -columns=textscan(fid,'%s','delimiter','\n'); -fclose(fid); -lines=columns{1}; -n=size(lines,1); - -for i=1:n - line_i=lines{i}; - if strcmp('VERTEX2',line_i(1:7)) - v = textscan(line_i,'%s %d %f %f %f',1); - initial.insertPose(v{2}, gtsamPose2(v{3}, v{4}, v{5})); - elseif strcmp('EDGE2',line_i(1:5)) - e = textscan(line_i,'%s %d %d %f %f %f',1); - graph.addOdometry(e{2}, e{3}, gtsamPose2(e{4}, e{5}, e{6}), odometryNoise); - end -end - %% Plot Initial Estimate figure(1);clf -plot(initial.xs(),initial.ys(),'r-*'); axis equal - -addEquivalences=0; -if addEquivalences - %% Add equivalence constraints - for i=1:n - line_i=cell2mat(lines(i)); - if strcmp('EQUIV',line_i(1:5)) - equiv = textscan(line_i,'%s %d %d',1); - graph.addOdometry(e{2}, e{3}, constraint, odometryNoise); - end - end - -end +plot(initial.xs(),initial.ys(),'g-*'); axis equal %% Optimize using Levenberg-Marquardt optimization with an ordering from colamd result = graph.optimize(initial); -hold on; plot(result.xs(),result.ys(),'g-*') +hold on; plot(result.xs(),result.ys(),'b-*') +result.print('\nFinal result:\n'); %% Plot Covariance Ellipses -% marginals = graph.marginals(result); -% for i=0:result2.size()-1 -% pose_i = result.pose(i); -% P_i=marginals.marginalCovariance(i); -% covarianceEllipse([pose_i.x;pose_i.y],P_i,'g') -% end +marginals = graph.marginals(result); +for i=1:result.size()-1 + pose_i = result.pose(i); + P{i}=marginals.marginalCovariance(i); + covarianceEllipse([pose_i.x;pose_i.y],P{i},'b') +end +fprintf(1,'%.5f %.5f %.5f\n',P{99}) \ No newline at end of file diff --git a/examples/matlab/load2D.m b/examples/matlab/load2D.m new file mode 100644 index 000000000..e459dfc63 --- /dev/null +++ b/examples/matlab/load2D.m @@ -0,0 +1,29 @@ +function [graph,initial] = load2D(filename,model) +% load2D: read TORO pose graph +% cannot read noise model from file yet, uses specified model + +fid = fopen(filename); +if fid < 0 + error(['load2D: Cannot open file ' filename]); +end + +% scan all lines into a cell array +columns=textscan(fid,'%s','delimiter','\n'); +fclose(fid); +lines=columns{1}; + +% loop over lines and add vertices +graph = pose2SLAMGraph; +initial = pose2SLAMValues; +n=size(lines,1); +for i=1:n + line_i=lines{i}; + if strcmp('VERTEX2',line_i(1:7)) + v = textscan(line_i,'%s %d %f %f %f',1); + initial.insertPose(v{2}, gtsamPose2(v{3}, v{4}, v{5})); + elseif strcmp('EDGE2',line_i(1:5)) + e = textscan(line_i,'%s %d %d %f %f %f',1); + graph.addOdometry(e{2}, e{3}, gtsamPose2(e{4}, e{5}, e{6}), model); + end +end + diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index 2aec8e55a..d4be33f31 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -16,7 +16,6 @@ * @brief utility functions for loading datasets */ - #include #include #include @@ -28,192 +27,219 @@ using namespace gtsam; #define LINESIZE 81920 -typedef boost::shared_ptr sharedPose2Graph; -typedef pose2SLAM::Odometry Pose2Factor; ///< Typedef for Constraint class for backwards compatibility - namespace gtsam { -/* ************************************************************************* */ -pair > dataset(const string& dataset, const string& user_path) { - string path = user_path, set = dataset; - boost::optional null_model; - boost::optional identity(noiseModel::Unit::Create(3)); - boost::optional small(noiseModel::Diagonal::Variances( - gtsam::Vector_(3, 0.0001, 0.0001, 0.0003))); + /* ************************************************************************* */ + pair > dataset(const string& dataset, + const string& user_path) { + string path = user_path, set = dataset; + boost::optional null_model; + boost::optional identity(noiseModel::Unit::Create(3)); + boost::optional small( + noiseModel::Diagonal::Variances( + gtsam::Vector_(3, 0.0001, 0.0001, 0.0003))); - if (path.empty()) path = string(getenv("HOME")) + "/data"; - if (set.empty()) set = string(getenv("DATASET")); + if (path.empty()) + path = string(getenv("HOME")) + "/data"; + if (set.empty()) + set = string(getenv("DATASET")); - if (set == "intel") return make_pair(path + "/Intel/intel.graph", null_model); - if (set == "intel-gfs") return make_pair(path + "/Intel/intel.gfs.graph", null_model); - if (set == "Killian-gfs") return make_pair(path + "/Killian/Killian.gfs.graph", null_model); - if (set == "Killian") return make_pair(path + "/Killian/Killian.graph", small); - if (set == "Killian-noised") return make_pair(path + "/Killian/Killian-noised.graph", null_model); - if (set == "3") return make_pair(path + "/TORO/w3-odom.graph", identity); - if (set == "100") return make_pair(path + "/TORO/w100-odom.graph", identity); - if (set == "10K") return make_pair(path + "/TORO/w10000-odom.graph", identity); - if (set == "10K2") return make_pair(path + "/hogman/data/2D/w10000.graph", - noiseModel::Diagonal::Variances(gtsam::Vector_(3, 0.1, 0.1, 0.05))); - if (set == "Eiffel100") return make_pair(path + "/TORO/w100-Eiffel.graph", identity); - if (set == "Eiffel10K") return make_pair(path + "/TORO/w10000-Eiffel.graph", identity); - if (set == "olson") return make_pair(path + "/Olson/olson06icra.graph", null_model); - if (set == "victoria") return make_pair(path + "/VictoriaPark/victoria_park.graph", null_model); - if (set == "beijing") return make_pair(path + "/Beijing/beijingData_trips.graph", null_model); + if (set == "intel") + return make_pair(path + "/Intel/intel.graph", null_model); + if (set == "intel-gfs") + return make_pair(path + "/Intel/intel.gfs.graph", null_model); + if (set == "Killian-gfs") + return make_pair(path + "/Killian/Killian.gfs.graph", null_model); + if (set == "Killian") + return make_pair(path + "/Killian/Killian.graph", small); + if (set == "Killian-noised") + return make_pair(path + "/Killian/Killian-noised.graph", null_model); + if (set == "3") + return make_pair(path + "/TORO/w3-odom.graph", identity); + if (set == "100") + return make_pair(path + "/TORO/w100-odom.graph", identity); + if (set == "10K") + return make_pair(path + "/TORO/w10000-odom.graph", identity); + if (set == "10K2") + return make_pair(path + "/hogman/data/2D/w10000.graph", + noiseModel::Diagonal::Variances(gtsam::Vector_(3, 0.1, 0.1, 0.05))); + if (set == "Eiffel100") + return make_pair(path + "/TORO/w100-Eiffel.graph", identity); + if (set == "Eiffel10K") + return make_pair(path + "/TORO/w10000-Eiffel.graph", identity); + if (set == "olson") + return make_pair(path + "/Olson/olson06icra.graph", null_model); + if (set == "victoria") + return make_pair(path + "/VictoriaPark/victoria_park.graph", null_model); + if (set == "beijing") + return make_pair(path + "/Beijing/beijingData_trips.graph", null_model); - return make_pair("unknown", null_model); -} - -/* ************************************************************************* */ - -pair load2D( - pair > dataset, - int maxID, bool addNoise, bool smart) { - return load2D(dataset.first, dataset.second, maxID, addNoise, smart); -} - -pair load2D(const string& filename, - boost::optional model, int maxID, bool addNoise, bool smart) { - cout << "Will try to read " << filename << endl; - ifstream is(filename.c_str()); - if (!is) { - cout << "load2D: can not find the file!"; - exit(-1); + return make_pair("unknown", null_model); } - Values::shared_ptr poses(new Values); - sharedPose2Graph graph(new pose2SLAM::Graph); + /* ************************************************************************* */ - string tag; + pair load2D( + pair > dataset, + int maxID, bool addNoise, bool smart) { + return load2D(dataset.first, dataset.second, maxID, addNoise, smart); + } - // load the poses - while (is) { - is >> tag; - - if ((tag == "VERTEX2") || (tag == "VERTEX")) { - int id; - double x, y, yaw; - is >> id >> x >> y >> yaw; - // optional filter - if (maxID && id >= maxID) continue; - poses->insert(id, Pose2(x, y, yaw)); + pair load2D( + const string& filename, boost::optional model, int maxID, + bool addNoise, bool smart) { + cout << "Will try to read " << filename << endl; + ifstream is(filename.c_str()); + if (!is) { + cout << "load2D: can not find the file!"; + exit(-1); } - is.ignore(LINESIZE, '\n'); - } - is.clear(); /* clears the end-of-file and error flags */ - is.seekg(0, ios::beg); - // load the factors - while (is) { - is >> tag; + pose2SLAM::Values::shared_ptr poses(new pose2SLAM::Values); + pose2SLAM::Graph::shared_ptr graph(new pose2SLAM::Graph); - if ((tag == "EDGE2") || (tag == "EDGE") || (tag == "ODOMETRY")) { - int id1, id2; - double x, y, yaw; + string tag; - is >> id1 >> id2 >> x >> y >> yaw; - Matrix m = eye(3); - is >> m(0, 0) >> m(0, 1) >> m(1, 1) >> m(2, 2) >> m(0, 2) >> m(1, 2); - m(2, 0) = m(0, 2); - m(2, 1) = m(1, 2); - m(1, 0) = m(0, 1); + // load the poses + while (is) { + is >> tag; - // optional filter - if (maxID && (id1 >= maxID || id2 >= maxID)) continue; - - Pose2 l1Xl2(x, y, yaw); - - // SharedNoiseModel noise = noiseModel::Gaussian::Covariance(m, smart); - if (!model) { - Vector variances = Vector_(3,m(0,0),m(1,1),m(2,2)); - model = noiseModel::Diagonal::Variances(variances, smart); + if ((tag == "VERTEX2") || (tag == "VERTEX")) { + int id; + double x, y, yaw; + is >> id >> x >> y >> yaw; + // optional filter + if (maxID && id >= maxID) + continue; + poses->insert(id, Pose2(x, y, yaw)); } - - if (addNoise) - l1Xl2 = l1Xl2.retract((*model)->sample()); - - // Insert vertices if pure odometry file - if (!poses->exists(id1)) poses->insert(id1, Pose2()); - if (!poses->exists(id2)) poses->insert(id2, poses->at(id1) * l1Xl2); - - pose2SLAM::Graph::sharedFactor factor(new Pose2Factor(id1, id2, l1Xl2, *model)); - graph->push_back(factor); + is.ignore(LINESIZE, '\n'); } - is.ignore(LINESIZE, '\n'); - } + is.clear(); /* clears the end-of-file and error flags */ + is.seekg(0, ios::beg); - cout << "load2D read a graph file with " << poses->size() << " vertices and " - << graph->nrFactors() << " factors" << endl; + // load the factors + while (is) { + is >> tag; - return make_pair(graph, poses); -} + if ((tag == "EDGE2") || (tag == "EDGE") || (tag == "ODOMETRY")) { + int id1, id2; + double x, y, yaw; -/* ************************************************************************* */ -void save2D(const pose2SLAM::Graph& graph, const Values& config, const SharedDiagonal model, - const string& filename) { + is >> id1 >> id2 >> x >> y >> yaw; + Matrix m = eye(3); + is >> m(0, 0) >> m(0, 1) >> m(1, 1) >> m(2, 2) >> m(0, 2) >> m(1, 2); + m(2, 0) = m(0, 2); + m(2, 1) = m(1, 2); + m(1, 0) = m(0, 1); - fstream stream(filename.c_str(), fstream::out); + // optional filter + if (maxID && (id1 >= maxID || id2 >= maxID)) + continue; - // save poses - BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, config) { - const Pose2& pose = dynamic_cast(key_value.value); - stream << "VERTEX2 " << key_value.key << " " << pose.x() << " " << pose.y() << " " << pose.theta() << endl; - } + Pose2 l1Xl2(x, y, yaw); - // save edges - Matrix R = model->R(); - Matrix RR = trans(R)*R;//prod(trans(R),R); - BOOST_FOREACH(boost::shared_ptr factor_, graph) { - boost::shared_ptr factor = boost::dynamic_pointer_cast(factor_); - if (!factor) continue; + // SharedNoiseModel noise = noiseModel::Gaussian::Covariance(m, smart); + if (!model) { + Vector variances = Vector_(3, m(0, 0), m(1, 1), m(2, 2)); + model = noiseModel::Diagonal::Variances(variances, smart); + } - 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; - } + if (addNoise) + l1Xl2 = l1Xl2.retract((*model)->sample()); - stream.close(); -} + // Insert vertices if pure odometry file + if (!poses->exists(id1)) + poses->insert(id1, Pose2()); + if (!poses->exists(id2)) + poses->insert(id2, poses->at(id1) * l1Xl2); -/* ************************************************************************* */ -bool load3D(const string& filename) { - ifstream is(filename.c_str()); - if (!is) return false; - - while (is) { - char buf[LINESIZE]; - is.getline(buf, LINESIZE); - istringstream ls(buf); - string tag; - ls >> tag; - - if (tag == "VERTEX3") { - int id; - double x, y, z, roll, pitch, yaw; - ls >> id >> x >> y >> z >> roll >> pitch >> yaw; + pose2SLAM::Graph::sharedFactor factor( + new pose2SLAM::Odometry(id1, id2, l1Xl2, *model)); + graph->push_back(factor); + } + is.ignore(LINESIZE, '\n'); } + + cout << "load2D read a graph file with " << poses->size() + << " vertices and " << graph->nrFactors() << " factors" << endl; + + return make_pair(graph, poses); } - is.clear(); /* clears the end-of-file and error flags */ - is.seekg(0, ios::beg); - while (is) { - char buf[LINESIZE]; - is.getline(buf, LINESIZE); - istringstream ls(buf); - string tag; - ls >> tag; + /* ************************************************************************* */ + void save2D(const pose2SLAM::Graph& graph, const Values& config, + const SharedDiagonal model, const string& filename) { - if (tag == "EDGE3") { - int id1, id2; - double x, y, z, roll, pitch, yaw; - ls >> id1 >> id2 >> x >> y >> z >> roll >> pitch >> yaw; - Matrix m = eye(6); - for (int i = 0; i < 6; i++) - for (int j = i; j < 6; j++) - ls >> m(i, j); + fstream stream(filename.c_str(), fstream::out); + + // save poses + BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, config) + { + const Pose2& pose = dynamic_cast(key_value.value); + 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 factor_, graph) + { + boost::shared_ptr factor = + boost::dynamic_pointer_cast(factor_); + if (!factor) + continue; + + 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; + } + + stream.close(); + } + + /* ************************************************************************* */ + bool load3D(const string& filename) { + ifstream is(filename.c_str()); + if (!is) + return false; + + while (is) { + char buf[LINESIZE]; + is.getline(buf, LINESIZE); + istringstream ls(buf); + string tag; + ls >> tag; + + if (tag == "VERTEX3") { + int id; + double x, y, z, roll, pitch, yaw; + ls >> id >> x >> y >> z >> roll >> pitch >> yaw; + } } + is.clear(); /* clears the end-of-file and error flags */ + is.seekg(0, ios::beg); + + while (is) { + char buf[LINESIZE]; + is.getline(buf, LINESIZE); + istringstream ls(buf); + string tag; + ls >> tag; + + if (tag == "EDGE3") { + int id1, id2; + double x, y, z, roll, pitch, yaw; + ls >> id1 >> id2 >> x >> y >> z >> roll >> pitch >> yaw; + Matrix m = eye(6); + for (int i = 0; i < 6; i++) + for (int j = i; j < 6; j++) + ls >> m(i, j); + } + } + return true; } - return true; -} } diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index 5e06e583d..a099fb3d5 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -18,43 +18,52 @@ #pragma once - -#include #include +#include -namespace gtsam -{ -/** - * Construct dataset filename from short name - * Currently has "Killian" "intel.gfs", "10K", etc... - * @param filename - * @param optional dataset, if empty will try to getenv $DATASET - * @param optional path, if empty will try to getenv $HOME - */ -std::pair > +namespace gtsam { + /** + * Construct dataset filename from short name + * Currently has "Killian" "intel.gfs", "10K", etc... + * @param filename + * @param optional dataset, if empty will try to getenv $DATASET + * @param optional path, if empty will try to getenv $HOME + */ + std::pair > dataset(const std::string& dataset = "", const std::string& path = ""); -/** - * Load TORO 2D Graph - * @param filename - * @param maxID, if non-zero cut out vertices >= maxID - * @param smart: try to reduce complexity of covariance to cheapest model - */ -std::pair, boost::shared_ptr > load2D( - std::pair > dataset, - int maxID = 0, bool addNoise=false, bool smart=true); -std::pair, boost::shared_ptr > load2D( - const std::string& filename, - boost::optional model = boost::optional(), - 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 + */ + std::pair load2D( + std::pair > dataset, + int maxID = 0, bool addNoise = false, bool smart = true); -/** save 2d graph */ -void save2D(const pose2SLAM::Graph& graph, const Values& config, const gtsam::SharedDiagonal model, - const std::string& filename); + /** + * 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 + */ + std::pair load2D( + const std::string& filename, + boost::optional model = boost::optional< + gtsam::SharedDiagonal>(), int maxID = 0, bool addNoise = false, + bool smart = true); -/** - * Load TORO 3D Graph - */ -bool load3D(const std::string& filename); + /** save 2d graph */ + void save2D(const pose2SLAM::Graph& graph, const Values& config, + const gtsam::SharedDiagonal model, const std::string& filename); + + /** + * Load TORO 3D Graph + */ + bool load3D(const std::string& filename); } // namespace gtsam diff --git a/gtsam/slam/pose2SLAM.h b/gtsam/slam/pose2SLAM.h index f438ce24d..5668f8ad0 100644 --- a/gtsam/slam/pose2SLAM.h +++ b/gtsam/slam/pose2SLAM.h @@ -33,7 +33,9 @@ namespace pose2SLAM { /// Values class, inherited from Values, mainly used as a convenience for MATLAB wrapper struct Values: public gtsam::Values { - /// Default constructor + typedef boost::shared_ptr shared_ptr; + + /// Default constructor Values() {} /// Copy constructor @@ -75,7 +77,9 @@ namespace pose2SLAM { /// Graph struct Graph: public NonlinearFactorGraph { - /// Default constructor for a NonlinearFactorGraph + typedef boost::shared_ptr shared_ptr; + + /// Default constructor for a NonlinearFactorGraph Graph(){} /// Creates a NonlinearFactorGraph based on another NonlinearFactorGraph