diff --git a/examples/Pose2SLAMExample_g2o.cpp b/examples/Pose2SLAMExample_g2o.cpp index 565a26965..3a4ee9cff 100644 --- a/examples/Pose2SLAMExample_g2o.cpp +++ b/examples/Pose2SLAMExample_g2o.cpp @@ -35,18 +35,18 @@ int main(const int argc, const char *argv[]) { else g2oFile = argv[1]; - NonlinearFactorGraph graph; - Values initial; - readG2o(g2oFile, graph, initial); + NonlinearFactorGraph::shared_ptr graph; + Values::shared_ptr initial; + boost::tie(graph, initial) = readG2o(g2oFile); // Add prior on the pose having index (key) = 0 - NonlinearFactorGraph graphWithPrior = graph; + NonlinearFactorGraph graphWithPrior = *graph; noiseModel::Diagonal::shared_ptr priorModel = // noiseModel::Diagonal::Variances((Vector(3) << 1e-6, 1e-6, 1e-8)); graphWithPrior.add(PriorFactor(0, Pose2(), priorModel)); std::cout << "Optimizing the factor graph" << std::endl; - GaussNewtonOptimizer optimizer(graphWithPrior, initial); // , parameters); + GaussNewtonOptimizer optimizer(graphWithPrior, *initial); Values result = optimizer.optimize(); std::cout << "Optimization complete" << std::endl; @@ -55,7 +55,7 @@ int main(const int argc, const char *argv[]) { } else { const string outputFile = argv[2]; std::cout << "Writing results to file: " << outputFile << std::endl; - writeG2o(outputFile, graph, result); + writeG2o(*graph, result, outputFile); std::cout << "done! " << std::endl; } return 0; diff --git a/examples/Pose2SLAMExample_lago.cpp b/examples/Pose2SLAMExample_lago.cpp index a6340caf7..cf82a05ca 100644 --- a/examples/Pose2SLAMExample_lago.cpp +++ b/examples/Pose2SLAMExample_lago.cpp @@ -36,12 +36,12 @@ int main(const int argc, const char *argv[]) { else g2oFile = argv[1]; - NonlinearFactorGraph graph; - Values initial; - readG2o(g2oFile, graph, initial); + NonlinearFactorGraph::shared_ptr graph; + Values::shared_ptr initial; + boost::tie(graph, initial) = readG2o(g2oFile); // Add prior on the pose having index (key) = 0 - NonlinearFactorGraph graphWithPrior = graph; + NonlinearFactorGraph graphWithPrior = *graph; noiseModel::Diagonal::shared_ptr priorModel = // noiseModel::Diagonal::Variances((Vector(3) << 1e-6, 1e-6, 1e-8)); graphWithPrior.add(PriorFactor(0, Pose2(), priorModel)); @@ -56,7 +56,7 @@ int main(const int argc, const char *argv[]) { } else { const string outputFile = argv[2]; std::cout << "Writing results to file: " << outputFile << std::endl; - writeG2o(outputFile, graph, estimateLago); + writeG2o(*graph, estimateLago, outputFile); std::cout << "done! " << std::endl; } diff --git a/gtsam.h b/gtsam.h index 96a778acf..b7178d534 100644 --- a/gtsam.h +++ b/gtsam.h @@ -2249,6 +2249,13 @@ pair load2D(string filename, pair load2D(string filename); pair load2D_robust(string filename, gtsam::noiseModel::Base* model); +void save2D(const gtsam::NonlinearFactorGraph& graph, + const gtsam::Values& config, gtsam::noiseModel::Diagonal* model, + string filename); + +pair readG2o(string filename); +void writeG2o(const gtsam::NonlinearFactorGraph& graph, + const gtsam::Values& estimate, string filename); //************************************************************************* // Navigation diff --git a/gtsam/nonlinear/tests/testLago.cpp b/gtsam/nonlinear/tests/testLago.cpp index f2cab4506..6a94588b4 100644 --- a/gtsam/nonlinear/tests/testLago.cpp +++ b/gtsam/nonlinear/tests/testLago.cpp @@ -258,13 +258,13 @@ TEST( Lago, smallGraph2 ) { /* *************************************************************************** */ TEST( Lago, largeGraphNoisy_orientations ) { - NonlinearFactorGraph g; - Values initial; string inputFile = findExampleDataFile("noisyToyGraph"); - readG2o(inputFile, g, initial); + NonlinearFactorGraph::shared_ptr g; + Values::shared_ptr initial; + boost::tie(g, initial) = readG2o(inputFile); // Add prior on the pose having index (key) = 0 - NonlinearFactorGraph graphWithPrior = g; + NonlinearFactorGraph graphWithPrior = *g; noiseModel::Diagonal::shared_ptr priorModel = noiseModel::Diagonal::Variances((Vector(3) << 1e-2, 1e-2, 1e-4)); graphWithPrior.add(PriorFactor(0, Pose2(), priorModel)); @@ -279,40 +279,40 @@ TEST( Lago, largeGraphNoisy_orientations ) { actual.insert(key, poseLago); } } - NonlinearFactorGraph gmatlab; - Values expected; string matlabFile = findExampleDataFile("orientationsNoisyToyGraph"); - readG2o(matlabFile, gmatlab, expected); + NonlinearFactorGraph::shared_ptr gmatlab; + Values::shared_ptr expected; + boost::tie(gmatlab, expected) = readG2o(matlabFile); - BOOST_FOREACH(const Values::KeyValuePair& key_val, expected){ + BOOST_FOREACH(const Values::KeyValuePair& key_val, *expected){ Key k = key_val.key; - EXPECT(assert_equal(expected.at(k), actual.at(k), 1e-5)); + EXPECT(assert_equal(expected->at(k), actual.at(k), 1e-5)); } } /* *************************************************************************** */ TEST( Lago, largeGraphNoisy ) { - NonlinearFactorGraph g; - Values initial; string inputFile = findExampleDataFile("noisyToyGraph"); - readG2o(inputFile, g, initial); + NonlinearFactorGraph::shared_ptr g; + Values::shared_ptr initial; + boost::tie(g, initial) = readG2o(inputFile); // Add prior on the pose having index (key) = 0 - NonlinearFactorGraph graphWithPrior = g; + NonlinearFactorGraph graphWithPrior = *g; noiseModel::Diagonal::shared_ptr priorModel = noiseModel::Diagonal::Variances((Vector(3) << 1e-2, 1e-2, 1e-4)); graphWithPrior.add(PriorFactor(0, Pose2(), priorModel)); Values actual = lago::initialize(graphWithPrior); - NonlinearFactorGraph gmatlab; - Values expected; string matlabFile = findExampleDataFile("optimizedNoisyToyGraph"); - readG2o(matlabFile, gmatlab, expected); + NonlinearFactorGraph::shared_ptr gmatlab; + Values::shared_ptr expected; + boost::tie(gmatlab, expected) = readG2o(matlabFile); - BOOST_FOREACH(const Values::KeyValuePair& key_val, expected){ + BOOST_FOREACH(const Values::KeyValuePair& key_val, *expected){ Key k = key_val.key; - EXPECT(assert_equal(expected.at(k), actual.at(k), 1e-2)); + EXPECT(assert_equal(expected->at(k), actual.at(k), 1e-2)); } } diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index b397ab60d..b3c9b9557 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -62,7 +62,7 @@ string findExampleDataFile(const string& name) { // If we did not return already, then we did not find the file throw -std::invalid_argument( +invalid_argument( "gtsam::findExampleDataFile could not find a matching file in\n" SOURCE_TREE_DATASET_DIR " or\n" INSTALLED_DATASET_DIR " named\n" + @@ -73,7 +73,7 @@ std::invalid_argument( string createRewrittenFileName(const string& name) { // Search source tree and installed location if (!exists(fs::path(name))) { - throw std::invalid_argument( + throw invalid_argument( "gtsam::createRewrittenFileName could not find a matching file in\n" + name); } @@ -89,9 +89,8 @@ string createRewrittenFileName(const string& name) { #endif /* ************************************************************************* */ -pair load2D( - pair dataset, int maxID, bool addNoise, - bool smart, NoiseFormat noiseFormat, +GraphAndValues load2D(pair dataset, int maxID, + bool addNoise, bool smart, NoiseFormat noiseFormat, KernelFunctionType kernelFunctionType) { return load2D(dataset.first, dataset.second, maxID, addNoise, smart, noiseFormat, kernelFunctionType); @@ -111,7 +110,7 @@ static SharedNoiseModel readNoiseModel(ifstream& is, bool smart, case NoiseFormatCOV: // i.e., [ v1 v2 v3; v2' v4 v5; v3' v5' v6 ] if (v1 == 0.0 || v4 == 0.0 || v6 == 0.0) - throw std::runtime_error( + throw runtime_error( "load2D::readNoiseModel looks like this is not G2O matrix order"); M << v1, v2, v3, v2, v4, v5, v3, v5, v6; break; @@ -121,12 +120,12 @@ static SharedNoiseModel readNoiseModel(ifstream& is, bool smart, // inf_ff inf_fs inf_ss inf_rr inf_fr inf_sr // i.e., [ v1 v2 v5; v2' v3 v6; v5' v6' v4 ] if (v1 == 0.0 || v3 == 0.0 || v4 == 0.0) - throw std::invalid_argument( + throw invalid_argument( "load2D::readNoiseModel looks like this is not TORO matrix order"); M << v1, v2, v5, v2, v3, v6, v5, v6, v4; break; default: - throw std::runtime_error("load2D: invalid noise format"); + throw runtime_error("load2D: invalid noise format"); } // Now, create a Gaussian noise model @@ -144,11 +143,11 @@ static SharedNoiseModel readNoiseModel(ifstream& is, bool smart, model = noiseModel::Gaussian::Covariance(M, smart); break; default: - throw std::invalid_argument("load2D: invalid noise format"); + throw invalid_argument("load2D: invalid noise format"); } switch (kernelFunctionType) { - case KernelFunctionTypeQUADRATIC: + case KernelFunctionTypeNONE: return model; break; case KernelFunctionTypeHUBER: @@ -160,20 +159,18 @@ static SharedNoiseModel readNoiseModel(ifstream& is, bool smart, noiseModel::mEstimator::Tukey::Create(4.6851), model); break; default: - throw std::invalid_argument("load2D: invalid kernel function type"); + throw invalid_argument("load2D: invalid kernel function type"); } } /* ************************************************************************* */ -pair load2D( - const string& filename, SharedNoiseModel model, int maxID, bool addNoise, - bool smart, NoiseFormat noiseFormat, +GraphAndValues load2D(const string& filename, SharedNoiseModel model, int maxID, + bool addNoise, bool smart, NoiseFormat noiseFormat, KernelFunctionType kernelFunctionType) { - cout << "Will try to read " << filename << endl; ifstream is(filename.c_str()); if (!is) - throw std::invalid_argument("load2D: can not find file " + filename); + throw invalid_argument("load2D: can not find file " + filename); Values::shared_ptr initial(new Values); NonlinearFactorGraph::shared_ptr graph(new NonlinearFactorGraph); @@ -270,8 +267,8 @@ pair load2D( is >> id1 >> id2 >> lmx >> lmy >> v1 >> v2 >> v3; // Convert x,y to bearing,range - bearing = std::atan2(lmy, lmx); - range = std::sqrt(lmx * lmx + lmy * lmy); + bearing = atan2(lmy, lmx); + range = 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. @@ -319,12 +316,15 @@ pair load2D( is.ignore(LINESIZE, '\n'); } - cout << "load2D read a graph file with " << initial->size() - << " vertices and " << graph->nrFactors() << " factors" << endl; - return make_pair(graph, initial); } +/* ************************************************************************* */ +GraphAndValues load2D_robust(const string& filename, + noiseModel::Base::shared_ptr& model, int maxID) { + return load2D(filename, model, maxID); +} + /* ************************************************************************* */ void save2D(const NonlinearFactorGraph& graph, const Values& config, const noiseModel::Diagonal::shared_ptr model, const string& filename) { @@ -357,6 +357,54 @@ void save2D(const NonlinearFactorGraph& graph, const Values& config, stream.close(); } +/* ************************************************************************* */ +GraphAndValues readG2o(const string& g2oFile, + KernelFunctionType kernelFunctionType) { + // just call load2D + int maxID = 0; + bool addNoise = false; + bool smart = true; + return load2D(g2oFile, SharedNoiseModel(), maxID, addNoise, smart, + NoiseFormatG2O, kernelFunctionType); +} + +/* ************************************************************************* */ +void writeG2o(const NonlinearFactorGraph& graph, const Values& estimate, + const string& filename) { + + fstream stream(filename.c_str(), fstream::out); + + // save poses + BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, estimate) { + const Pose2& pose = dynamic_cast(key_value.value); + stream << "VERTEX_SE2 " << key_value.key << " " << pose.x() << " " + << pose.y() << " " << pose.theta() << endl; + } + + // save edges + BOOST_FOREACH(boost::shared_ptr factor_, graph) { + boost::shared_ptr > factor = + boost::dynamic_pointer_cast >(factor_); + if (!factor) + continue; + + SharedNoiseModel model = factor->get_noiseModel(); + boost::shared_ptr diagonalModel = + boost::dynamic_pointer_cast(model); + if (!diagonalModel) + throw 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; + } + stream.close(); +} + /* ************************************************************************* */ bool load3D(const string& filename) { ifstream is(filename.c_str()); @@ -399,105 +447,6 @@ bool load3D(const string& filename) { return true; } -/* ************************************************************************* */ -pair load2D_robust( - const string& filename, noiseModel::Base::shared_ptr& model, int maxID) { - cout << "Will try to read " << filename << endl; - ifstream is(filename.c_str()); - if (!is) - throw std::invalid_argument("load2D: can not find the file!"); - - Values::shared_ptr initial(new Values); - NonlinearFactorGraph::shared_ptr graph(new NonlinearFactorGraph); - - string tag; - - // 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; - initial->insert(id, Pose2(x, y, yaw)); - } - is.ignore(LINESIZE, '\n'); - } - is.clear(); /* clears the end-of-file and error flags */ - is.seekg(0, ios::beg); - - // Create a sampler with random number generator - Sampler sampler(42u); - - // load the factors - while (is) { - is >> tag; - - if ((tag == "EDGE2") || (tag == "EDGE") || (tag == "ODOMETRY")) { - int id1, id2; - double x, y, yaw; - - 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); - - // optional filter - if (maxID && (id1 >= maxID || id2 >= maxID)) - continue; - - Pose2 l1Xl2(x, y, yaw); - - // Insert vertices if pure odometry file - if (!initial->exists(id1)) - initial->insert(id1, Pose2()); - if (!initial->exists(id2)) - initial->insert(id2, initial->at(id1) * l1Xl2); - - NonlinearFactor::shared_ptr factor( - new BetweenFactor(id1, id2, l1Xl2, model)); - graph->push_back(factor); - } - if (tag == "BR") { - int id1, id2; - double bearing, range, bearing_std, range_std; - - is >> id1 >> id2 >> bearing >> range >> bearing_std >> range_std; - - // optional filter - if (maxID && (id1 >= maxID || id2 >= maxID)) - continue; - - noiseModel::Diagonal::shared_ptr measurementNoise = - noiseModel::Diagonal::Sigmas((Vector(2) << bearing_std, range_std)); - *graph += BearingRangeFactor(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(id1); - Point2 local(cos(bearing) * range, sin(bearing) * range); - Point2 global = pose.transform_from(local); - initial->insert(id2, global); - } - } - is.ignore(LINESIZE, '\n'); - } - - cout << "load2D read a graph file with " << initial->size() - << " 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 /* R = [ 1 0 0 @@ -617,61 +566,6 @@ bool readBundler(const string& filename, SfM_data &data) { return true; } -/* ************************************************************************* */ -bool readG2o(const std::string& g2oFile, NonlinearFactorGraph& graph, - Values& initial, KernelFunctionType kernelFunctionType) { - - // just call load2D - NonlinearFactorGraph::shared_ptr graph_ptr; - Values::shared_ptr initial_ptr; - int maxID = 0; - bool addNoise = false; - bool smart = true; - boost::tie(graph_ptr, initial_ptr) = load2D(g2oFile, SharedNoiseModel(), - maxID, addNoise, smart, NoiseFormatG2O, kernelFunctionType); - graph = *graph_ptr; - initial = *initial_ptr; - return true; -} - -/* ************************************************************************* */ -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) { - const Pose2& pose = dynamic_cast(key_value.value); - stream << "VERTEX_SE2 " << key_value.key << " " << pose.x() << " " - << pose.y() << " " << pose.theta() << endl; - } - - // save edges - BOOST_FOREACH(boost::shared_ptr factor_, graph) { - boost::shared_ptr > factor = - boost::dynamic_pointer_cast >(factor_); - if (!factor) - continue; - - SharedNoiseModel model = factor->get_noiseModel(); - boost::shared_ptr diagonalModel = - boost::dynamic_pointer_cast(model); - if (!diagonalModel) - 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; - } - stream.close(); - return true; -} - /* ************************************************************************* */ bool readBAL(const string& filename, SfM_data &data) { // Load the data file diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index 771328d6d..8fd75269c 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -54,16 +54,20 @@ GTSAM_EXPORT std::string createRewrittenFileName(const std::string& name); /// Indicates how noise parameters are stored in file enum NoiseFormat { - NoiseFormatG2O, ///< Information matrix I11, I12, I13, I22, I23, I33 - NoiseFormatTORO, ///< Information matrix, but inf_ff inf_fs inf_ss inf_rr inf_fr inf_sr + NoiseFormatG2O, ///< Information matrix I11, I12, I13, I22, I23, I33 + NoiseFormatTORO, ///< Information matrix, but inf_ff inf_fs inf_ss inf_rr inf_fr inf_sr NoiseFormatGRAPH, ///< default: toro-style order, but covariance matrix ! - NoiseFormatCOV ///< Covariance matrix C11, C12, C13, C22, C23, C33 + NoiseFormatCOV ///< Covariance matrix C11, C12, C13, C22, C23, C33 }; +/// Robust kernel type to wrap around quadratic noise model enum KernelFunctionType { - KernelFunctionTypeQUADRATIC, KernelFunctionTypeHUBER, KernelFunctionTypeTUKEY + KernelFunctionTypeNONE, KernelFunctionTypeHUBER, KernelFunctionTypeTUKEY }; +/// Return type for load functions +typedef std::pair GraphAndValues; + /** * Load TORO 2D Graph * @param dataset/model pair as constructed by [dataset] @@ -71,36 +75,58 @@ enum KernelFunctionType { * @param addNoise add noise to the edges * @param smart try to reduce complexity of covariance to cheapest model */ -GTSAM_EXPORT std::pair load2D( +GTSAM_EXPORT GraphAndValues load2D( std::pair dataset, int maxID = 0, bool addNoise = false, bool smart = true, // NoiseFormat noiseFormat = NoiseFormatGRAPH, - KernelFunctionType kernelFunctionType = KernelFunctionTypeQUADRATIC); + KernelFunctionType kernelFunctionType = KernelFunctionTypeNONE); /** - * Load TORO 2D Graph + * Load TORO/G2O style graph files * @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 + * @param noiseFormat how noise parameters are stored + * @param kernelFunctionType whether to wrap the noise model in a robust kernel + * @return graph and initial values */ -GTSAM_EXPORT std::pair load2D( - const std::string& filename, SharedNoiseModel model = SharedNoiseModel(), - int maxID = 0, bool addNoise = false, bool smart = true, - NoiseFormat noiseFormat = NoiseFormatGRAPH, // - KernelFunctionType kernelFunctionType = KernelFunctionTypeQUADRATIC); +GTSAM_EXPORT GraphAndValues load2D(const std::string& filename, + SharedNoiseModel model = SharedNoiseModel(), int maxID = 0, bool addNoise = + false, bool smart = true, NoiseFormat noiseFormat = NoiseFormatGRAPH, // + KernelFunctionType kernelFunctionType = KernelFunctionTypeNONE); -GTSAM_EXPORT std::pair load2D_robust( - const std::string& filename, noiseModel::Base::shared_ptr& model, - int maxID = 0); +/// @deprecated load2D now allows for arbitrary models and wrapping a robust kernel +GTSAM_EXPORT GraphAndValues load2D_robust(const std::string& filename, + 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); +/** + * @brief This function parses a g2o file and stores the measurements into a + * NonlinearFactorGraph and the initial guess in a Values structure + * @param filename The name of the g2o file + * @param kernelFunctionType whether to wrap the noise model in a robust kernel + * @return graph and initial values + */ +GTSAM_EXPORT GraphAndValues readG2o(const std::string& g2oFile, + KernelFunctionType kernelFunctionType = KernelFunctionTypeNONE); + +/** + * @brief This function writes a g2o file from + * NonlinearFactorGraph and a Values structure + * @param filename The name of the g2o file to write + * @param graph NonlinearFactor graph storing the measurements + * @param estimate Values + */ +GTSAM_EXPORT void writeG2o(const NonlinearFactorGraph& graph, + const Values& estimate, const std::string& filename); + /** * Load TORO 3D Graph */ @@ -143,27 +169,6 @@ struct SfM_data { */ GTSAM_EXPORT bool readBundler(const std::string& filename, SfM_data &data); -/** - * @brief This function parses a g2o file and stores the measurements into a - * NonlinearFactorGraph and the initial guess in a Values structure - * @param filename The name of the g2o file - * @param graph NonlinearFactor graph storing the measurements (EDGE_SE2). NOTE: information matrix is assumed diagonal. - * @return initial Values containing the initial guess (VERTEX_SE2) - */ -GTSAM_EXPORT bool readG2o(const std::string& g2oFile, - NonlinearFactorGraph& graph, Values& initial, - KernelFunctionType kernelFunctionType = KernelFunctionTypeQUADRATIC); - -/** - * @brief This function writes a g2o file from - * NonlinearFactorGraph and a Values structure - * @param filename The name of the g2o file to write - * @param graph NonlinearFactor graph storing the measurements (EDGE_SE2) - * @return estimate Values containing the values (VERTEX_SE2) - */ -GTSAM_EXPORT bool writeG2o(const std::string& filename, - const NonlinearFactorGraph& graph, const Values& estimate); - /** * @brief This function parses a "Bundle Adjustment in the Large" (BAL) file and stores the data into a * SfM_data structure diff --git a/gtsam/slam/tests/testDataset.cpp b/gtsam/slam/tests/testDataset.cpp index 8789c4085..d7adf9b51 100644 --- a/gtsam/slam/tests/testDataset.cpp +++ b/gtsam/slam/tests/testDataset.cpp @@ -81,9 +81,9 @@ TEST( dataSet, Balbianello) TEST( dataSet, readG2o) { const string g2oFile = findExampleDataFile("pose2example"); - NonlinearFactorGraph actualGraph; - Values actualValues; - readG2o(g2oFile, actualGraph, actualValues); + NonlinearFactorGraph::shared_ptr actualGraph; + Values::shared_ptr actualValues; + boost::tie(actualGraph, actualValues) = readG2o(g2oFile); Values expectedValues; expectedValues.insert(0, Pose2(0.000000, 0.000000, 0.000000)); @@ -97,7 +97,7 @@ TEST( dataSet, readG2o) expectedValues.insert(8, Pose2(4.128877, 2.321481, -1.825391)); expectedValues.insert(9, Pose2(3.884653, 1.327509, -1.953016)); expectedValues.insert(10, Pose2(3.531067, 0.388263, -2.148934)); - EXPECT(assert_equal(expectedValues,actualValues,1e-5)); + EXPECT(assert_equal(expectedValues,*actualValues,1e-5)); noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Precisions((Vector(3) << 44.721360, 44.721360, 30.901699)); NonlinearFactorGraph expectedGraph; @@ -113,16 +113,16 @@ TEST( dataSet, readG2o) expectedGraph.add(BetweenFactor(9,10, Pose2(1.003350, 0.022250, -0.195918), model)); expectedGraph.add(BetweenFactor(5, 9, Pose2(0.033943, 0.032439, 3.073637), model)); expectedGraph.add(BetweenFactor(3,10, Pose2(0.044020, 0.988477, -1.553511), model)); - EXPECT(assert_equal(actualGraph,expectedGraph,1e-5)); + EXPECT(assert_equal(expectedGraph,*actualGraph,1e-5)); } /* ************************************************************************* */ TEST( dataSet, readG2oHuber) { const string g2oFile = findExampleDataFile("pose2example"); - NonlinearFactorGraph actualGraph; - Values actualValues; - readG2o(g2oFile, actualGraph, actualValues, KernelFunctionTypeHUBER); + NonlinearFactorGraph::shared_ptr actualGraph; + Values::shared_ptr actualValues; + boost::tie(actualGraph, actualValues) = readG2o(g2oFile, KernelFunctionTypeHUBER); noiseModel::Diagonal::shared_ptr baseModel = noiseModel::Diagonal::Precisions((Vector(3) << 44.721360, 44.721360, 30.901699)); SharedNoiseModel model = noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(1.345), baseModel); @@ -140,16 +140,16 @@ TEST( dataSet, readG2oHuber) expectedGraph.add(BetweenFactor(9,10, Pose2(1.003350, 0.022250, -0.195918), model)); expectedGraph.add(BetweenFactor(5, 9, Pose2(0.033943, 0.032439, 3.073637), model)); expectedGraph.add(BetweenFactor(3,10, Pose2(0.044020, 0.988477, -1.553511), model)); - EXPECT(assert_equal(actualGraph,expectedGraph,1e-5)); + EXPECT(assert_equal(expectedGraph,*actualGraph,1e-5)); } /* ************************************************************************* */ TEST( dataSet, readG2oTukey) { const string g2oFile = findExampleDataFile("pose2example"); - NonlinearFactorGraph actualGraph; - Values actualValues; - readG2o(g2oFile, actualGraph, actualValues, KernelFunctionTypeTUKEY); + NonlinearFactorGraph::shared_ptr actualGraph; + Values::shared_ptr actualValues; + boost::tie(actualGraph, actualValues) = readG2o(g2oFile, KernelFunctionTypeTUKEY); noiseModel::Diagonal::shared_ptr baseModel = noiseModel::Diagonal::Precisions((Vector(3) << 44.721360, 44.721360, 30.901699)); SharedNoiseModel model = noiseModel::Robust::Create(noiseModel::mEstimator::Tukey::Create(4.6851), baseModel); @@ -167,25 +167,25 @@ TEST( dataSet, readG2oTukey) expectedGraph.add(BetweenFactor(9,10, Pose2(1.003350, 0.022250, -0.195918), model)); expectedGraph.add(BetweenFactor(5, 9, Pose2(0.033943, 0.032439, 3.073637), model)); expectedGraph.add(BetweenFactor(3,10, Pose2(0.044020, 0.988477, -1.553511), model)); - EXPECT(assert_equal(actualGraph,expectedGraph,1e-5)); + EXPECT(assert_equal(expectedGraph,*actualGraph,1e-5)); } /* ************************************************************************* */ TEST( dataSet, writeG2o) { const string g2oFile = findExampleDataFile("pose2example"); - NonlinearFactorGraph expectedGraph; - Values expectedValues; - readG2o(g2oFile, expectedGraph, expectedValues); + NonlinearFactorGraph::shared_ptr expectedGraph; + Values::shared_ptr expectedValues; + boost::tie(expectedGraph, expectedValues) = readG2o(g2oFile); const string filenameToWrite = createRewrittenFileName(g2oFile); - writeG2o(filenameToWrite, expectedGraph, expectedValues); + writeG2o(*expectedGraph, *expectedValues, filenameToWrite); - NonlinearFactorGraph actualGraph; - Values actualValues; - readG2o(filenameToWrite, actualGraph, actualValues); - EXPECT(assert_equal(expectedValues,actualValues,1e-5)); - EXPECT(assert_equal(actualGraph,expectedGraph,1e-5)); + NonlinearFactorGraph::shared_ptr actualGraph; + Values::shared_ptr actualValues; + boost::tie(actualGraph, actualValues) = readG2o(filenameToWrite); + EXPECT(assert_equal(*expectedValues,*actualValues,1e-5)); + EXPECT(assert_equal(*expectedGraph,*actualGraph,1e-5)); } /* ************************************************************************* */