diff --git a/gtsam/inference/ISAM-inst.h b/gtsam/inference/ISAM-inst.h index bf2cacc84..4471567fc 100644 --- a/gtsam/inference/ISAM-inst.h +++ b/gtsam/inference/ISAM-inst.h @@ -22,43 +22,48 @@ namespace gtsam { - /* ************************************************************************* */ - template - void ISAM::update_internal(const FactorGraphType& newFactors, Cliques& orphans, const Eliminate& function) - { - // Remove the contaminated part of the Bayes tree - BayesNetType bn; - if (!this->empty()) { - const KeySet newFactorKeys = newFactors.keys(); - this->removeTop(std::vector(newFactorKeys.begin(), newFactorKeys.end()), bn, orphans); - } - - // Add the removed top and the new factors - FactorGraphType factors; - factors += bn; - factors += newFactors; - - // Add the orphaned subtrees - for(const sharedClique& orphan: orphans) - factors += boost::make_shared >(orphan); - - // eliminate into a Bayes net - const VariableIndex varIndex(factors); - const KeySet newFactorKeys = newFactors.keys(); - const Ordering constrainedOrdering = - Ordering::ColamdConstrainedLast(varIndex, std::vector(newFactorKeys.begin(), newFactorKeys.end())); - Base bayesTree = *factors.eliminateMultifrontal(constrainedOrdering, function, varIndex); - this->roots_.insert(this->roots_.end(), bayesTree.roots().begin(), bayesTree.roots().end()); - this->nodes_.insert(bayesTree.nodes().begin(), bayesTree.nodes().end()); +/* ************************************************************************* */ +template +void ISAM::update_internal(const FactorGraphType& newFactors, + Cliques& orphans, const Eliminate& function) { + // Remove the contaminated part of the Bayes tree + BayesNetType bn; + const KeySet newFactorKeys = newFactors.keys(); + if (!this->empty()) { + std::vector keyVector(newFactorKeys.begin(), newFactorKeys.end()); + this->removeTop(keyVector, bn, orphans); } - /* ************************************************************************* */ - template - void ISAM::update(const FactorGraphType& newFactors, const Eliminate& function) - { - Cliques orphans; - this->update_internal(newFactors, orphans, function); - } + // Add the removed top and the new factors + FactorGraphType factors; + factors += bn; + factors += newFactors; + + // Add the orphaned subtrees + for (const sharedClique& orphan : orphans) + factors += boost::make_shared >(orphan); + + // Get an ordering where the new keys are eliminated last + const VariableIndex index(factors); + const Ordering ordering = Ordering::ColamdConstrainedLast(index, + std::vector(newFactorKeys.begin(), newFactorKeys.end())); + + // eliminate all factors (top, added, orphans) into a new Bayes tree + auto bayesTree = factors.eliminateMultifrontal(ordering, function, index); + + // Re-add into Bayes tree data structures + this->roots_.insert(this->roots_.end(), bayesTree->roots().begin(), + bayesTree->roots().end()); + this->nodes_.insert(bayesTree->nodes().begin(), bayesTree->nodes().end()); +} + +/* ************************************************************************* */ +template +void ISAM::update(const FactorGraphType& newFactors, + const Eliminate& function) { + Cliques orphans; + this->update_internal(newFactors, orphans, function); +} } /// namespace gtsam diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index dc25026d2..ba51864f1 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -11,7 +11,7 @@ /** * @file dataset.cpp * @date Jan 22, 2010 - * @author nikai, Luca Carlone + * @author Kai Ni, Luca Carlone, Frank Dellaert * @brief utility functions for loading datasets */ @@ -56,8 +56,10 @@ namespace gtsam { string findExampleDataFile(const string& name) { // Search source tree and installed location vector 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 + + // Constants below are defined by CMake, see gtsam/gtsam/CMakeLists.txt + rootsToSearch.push_back(GTSAM_SOURCE_TREE_DATASET_DIR); + rootsToSearch.push_back(GTSAM_INSTALLED_DATASET_DIR); // Search for filename as given, and with .graph and .txt extensions vector namesToSearch; @@ -75,12 +77,11 @@ string findExampleDataFile(const string& name) { } // If we did not return already, then we did not find the file - throw -invalid_argument( - "gtsam::findExampleDataFile could not find a matching file in\n" - GTSAM_SOURCE_TREE_DATASET_DIR " or\n" - GTSAM_INSTALLED_DATASET_DIR " named\n" + - name + ", " + name + ".graph, or " + name + ".txt"); + throw invalid_argument( + "gtsam::findExampleDataFile could not find a matching file in\n" + GTSAM_SOURCE_TREE_DATASET_DIR " or\n" + GTSAM_INSTALLED_DATASET_DIR " named\n" + name + ", " + name + + ".graph, or " + name + ".txt"); } /* ************************************************************************* */ @@ -98,6 +99,7 @@ string createRewrittenFileName(const string& name) { return newpath.string(); } + /* ************************************************************************* */ #endif @@ -116,23 +118,20 @@ static SharedNoiseModel readNoiseModel(ifstream& is, bool smart, double v1, v2, v3, v4, v5, v6; is >> v1 >> v2 >> v3 >> v4 >> v5 >> v6; - if (noiseFormat == NoiseFormatAUTO) - { - // Try to guess covariance matrix layout - if(v1 != 0.0 && v2 == 0.0 && v3 != 0.0 && v4 != 0.0 && v5 == 0.0 && v6 == 0.0) - { - // NoiseFormatGRAPH - noiseFormat = NoiseFormatGRAPH; - } - else if(v1 != 0.0 && v2 == 0.0 && v3 == 0.0 && v4 != 0.0 && v5 == 0.0 && v6 != 0.0) - { - // NoiseFormatCOV - noiseFormat = NoiseFormatCOV; - } - else - { - throw std::invalid_argument("load2D: unrecognized covariance matrix format in dataset file. Please specify the noise format."); - } + if (noiseFormat == NoiseFormatAUTO) { + // Try to guess covariance matrix layout + if (v1 != 0.0 && v2 == 0.0 && v3 != 0.0 && v4 != 0.0 && v5 == 0.0 + && v6 == 0.0) { + // NoiseFormatGRAPH + noiseFormat = NoiseFormatGRAPH; + } else if (v1 != 0.0 && v2 == 0.0 && v3 == 0.0 && v4 != 0.0 && v5 == 0.0 + && v6 != 0.0) { + // NoiseFormatCOV + noiseFormat = NoiseFormatCOV; + } else { + throw std::invalid_argument( + "load2D: unrecognized covariance matrix format in dataset file. Please specify the noise format."); + } } // Read matrix and check that diagonal entries are non-zero @@ -195,6 +194,32 @@ static SharedNoiseModel readNoiseModel(ifstream& is, bool smart, } } +/* ************************************************************************* */ +boost::optional parseVertex(istream& is, const string& tag) { + if ((tag == "VERTEX2") || (tag == "VERTEX_SE2") || (tag == "VERTEX")) { + Key id; + double x, y, yaw; + is >> id >> x >> y >> yaw; + return IndexedPose(id, Pose2(x, y, yaw)); + } else { + return boost::none; + } +} + +/* ************************************************************************* */ +boost::optional parseEdge(istream& is, const string& tag) { + if ((tag == "EDGE2") || (tag == "EDGE") || (tag == "EDGE_SE2") + || (tag == "ODOMETRY")) { + + Key id1, id2; + double x, y, yaw; + is >> id1 >> id2 >> x >> y >> yaw; + return IndexedEdge(pair(id1, id2), Pose2(x, y, yaw)); + } else { + return boost::none; + } +} + /* ************************************************************************* */ GraphAndValues load2D(const string& filename, SharedNoiseModel model, Key maxID, bool addNoise, bool smart, NoiseFormat noiseFormat, @@ -214,16 +239,15 @@ GraphAndValues load2D(const string& filename, SharedNoiseModel model, Key maxID, if (!(is >> tag)) break; - if ((tag == "VERTEX2") || (tag == "VERTEX_SE2") || (tag == "VERTEX")) { - Key id; - double x, y, yaw; - is >> id >> x >> y >> yaw; + const auto indexed_pose = parseVertex(is, tag); + if (indexed_pose) { + Key id = indexed_pose->first; // optional filter if (maxID && id >= maxID) continue; - initial->insert(id, Pose2(x, y, yaw)); + initial->insert(id, indexed_pose->second); } is.ignore(LINESIZE, '\n'); } @@ -251,13 +275,10 @@ GraphAndValues load2D(const string& filename, SharedNoiseModel model, Key maxID, if (!(is >> tag)) break; - if ((tag == "EDGE2") || (tag == "EDGE") || (tag == "EDGE_SE2") - || (tag == "ODOMETRY")) { - - // Read transform - double x, y, yaw; - is >> id1 >> id2 >> x >> y >> yaw; - Pose2 l1Xl2(x, y, yaw); + auto between_pose = parseEdge(is, tag); + if (between_pose) { + std::tie(id1, id2) = between_pose->first; + Pose2& l1Xl2 = between_pose->second; // read noise model SharedNoiseModel modelInFile = readNoiseModel(is, smart, noiseFormat, diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index dc7804c2d..929ada2c1 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -33,6 +33,7 @@ #include #include // for pair #include +#include namespace gtsam { @@ -71,6 +72,26 @@ enum KernelFunctionType { KernelFunctionTypeNONE, KernelFunctionTypeHUBER, KernelFunctionTypeTUKEY }; +/// Return type for auxiliary functions +typedef std::pair IndexedPose; +typedef std::pair, Pose2> IndexedEdge; + +/** + * Parse TORO/G2O vertex "id x y yaw" + * @param is input stream + * @param tag string parsed from input stream, will only parse if vertex type + */ +GTSAM_EXPORT boost::optional parseVertex(std::istream& is, + const std::string& tag); + +/** + * Parse TORO/G2O edge "id1 id2 x y yaw" + * @param is input stream + * @param tag string parsed from input stream, will only parse if edge type + */ +GTSAM_EXPORT boost::optional parseEdge(std::istream& is, + const std::string& tag); + /// Return type for load functions typedef std::pair GraphAndValues; diff --git a/gtsam/slam/tests/testDataset.cpp b/gtsam/slam/tests/testDataset.cpp index b3e208b91..39c2d3722 100644 --- a/gtsam/slam/tests/testDataset.cpp +++ b/gtsam/slam/tests/testDataset.cpp @@ -26,6 +26,9 @@ #include +#include +#include + using namespace gtsam::symbol_shorthand; using namespace std; using namespace gtsam; @@ -39,6 +42,37 @@ TEST(dataSet, findExampleDataFile) { EXPECT(assert_equal(expected_end, actual_end)); } +/* ************************************************************************* */ +TEST( dataSet, parseVertex) +{ + const string str = "VERTEX2 1 2.000000 3.000000 4.000000"; + istringstream is(str); + string tag; + EXPECT(is >> tag); + const auto actual = parseVertex(is, tag); + EXPECT(actual); + if (actual) { + EXPECT_LONGS_EQUAL(1, actual->first); + EXPECT(assert_equal(Pose2(2, 3, 4), actual->second)); + } +} + +/* ************************************************************************* */ +TEST( dataSet, parseEdge) +{ + const string str = "EDGE2 0 1 2.000000 3.000000 4.000000"; + istringstream is(str); + string tag; + EXPECT(is >> tag); + const auto actual = parseEdge(is, tag); + EXPECT(actual); + if (actual) { + pair expected(0, 1); + EXPECT(expected == actual->first); + EXPECT(assert_equal(Pose2(2, 3, 4), actual->second)); + } +} + /* ************************************************************************* */ TEST( dataSet, load2D) { diff --git a/tests/testNonlinearISAM.cpp b/tests/testNonlinearISAM.cpp index 617a8cc1c..c06d10beb 100644 --- a/tests/testNonlinearISAM.cpp +++ b/tests/testNonlinearISAM.cpp @@ -7,6 +7,7 @@ #include #include +#include #include #include #include @@ -16,7 +17,10 @@ #include #include +#include +#include +using namespace std; using namespace gtsam; const double tol=1e-5; @@ -228,6 +232,93 @@ TEST(testNonlinearISAM, markov_chain_with_reconnect ) { EXPECT(assert_equal(expected.at(lm3), actualQR.at(lm3), tol)); } +/* ************************************************************************* */ +TEST(testNonlinearISAM, loop_closures ) { + int relinearizeInterval = 100; + NonlinearISAM isam(relinearizeInterval); + + // Create a Factor Graph and Values to hold the new data + NonlinearFactorGraph graph; + Values initialEstimate; + + vector lines; + lines.emplace_back("VERTEX2 0 0.000000 0.000000 0.000000"); + lines.emplace_back("EDGE2 1 0 1.030390 0.011350 -0.012958"); + lines.emplace_back("VERTEX2 1 1.030390 0.011350 -0.012958"); + lines.emplace_back("EDGE2 2 1 1.013900 -0.058639 -0.013225"); + lines.emplace_back("VERTEX2 2 2.043445 -0.060422 -0.026183"); + lines.emplace_back("EDGE2 3 2 1.027650 -0.007456 0.004833"); + lines.emplace_back("VERTEX2 3 3.070548 -0.094779 -0.021350"); + lines.emplace_back("EDGE2 4 3 -0.012016 1.004360 1.566790"); + lines.emplace_back("VERTEX2 4 3.079976 0.909609 1.545440"); + lines.emplace_back("EDGE2 5 4 1.016030 0.014565 -0.016304"); + lines.emplace_back("VERTEX2 5 3.091176 1.925681 1.529136"); + lines.emplace_back("EDGE2 6 5 1.023890 0.006808 0.010981"); + lines.emplace_back("VERTEX2 6 3.127018 2.948966 1.540117"); + lines.emplace_back("EDGE2 7 6 0.957734 0.003159 0.010901"); + lines.emplace_back("VERTEX2 7 3.153237 3.906347 1.551018"); + lines.emplace_back("EDGE2 8 7 -1.023820 -0.013668 -3.093240"); + lines.emplace_back("VERTEX2 8 3.146655 2.882457 -1.542222"); + lines.emplace_back("EDGE2 9 8 1.023440 0.013984 -0.007802"); + lines.emplace_back("EDGE2 9 5 0.033943 0.032439 -3.127400"); + lines.emplace_back("VERTEX2 9 3.189873 1.859834 -1.550024"); + lines.emplace_back("EDGE2 10 9 1.003350 0.022250 0.023491"); + lines.emplace_back("EDGE2 10 3 0.044020 0.988477 -1.563530"); + lines.emplace_back("VERTEX2 10 3.232959 0.857162 -1.526533"); + lines.emplace_back("EDGE2 11 10 0.977245 0.019042 -0.028623"); + lines.emplace_back("VERTEX2 11 3.295225 -0.118283 -1.555156"); + lines.emplace_back("EDGE2 12 11 -0.996880 -0.025512 -3.126915"); + lines.emplace_back("VERTEX2 12 3.254125 0.878076 1.601114"); + lines.emplace_back("EDGE2 13 12 0.990646 0.018396 -0.016519"); + lines.emplace_back("VERTEX2 13 3.205708 1.867709 1.584594"); + lines.emplace_back("EDGE2 14 13 0.945873 0.008893 -0.002602"); + lines.emplace_back("EDGE2 14 8 0.015808 0.021059 3.128310"); + lines.emplace_back("VERTEX2 14 3.183765 2.813370 1.581993"); + lines.emplace_back("EDGE2 15 14 1.000010 0.006428 0.028234"); + lines.emplace_back("EDGE2 15 7 -0.014728 -0.001595 -0.019579"); + lines.emplace_back("VERTEX2 15 3.166141 3.813245 1.610227"); + + auto model = noiseModel::Diagonal::Sigmas(Vector3(3.0, 3.0, 0.5)); + + // Loop over the different poses, adding the observations to iSAM incrementally + for (const string& str : lines) { + // scan the tag + string tag; + istringstream is(str); + if (!(is >> tag)) + break; + + // Check if vertex + const auto indexedPose = parseVertex(is, tag); + if (indexedPose) { + Key id = indexedPose->first; + initialEstimate.insert(Symbol('x', id), indexedPose->second); + if (id == 0) { + noiseModel::Diagonal::shared_ptr priorNoise = + noiseModel::Diagonal::Sigmas(Vector3(0.001, 0.001, 0.001)); + graph.emplace_shared >(Symbol('x', id), + Pose2(0, 0, 0), priorNoise); + } else { + isam.update(graph, initialEstimate); + + // Clear the factor graph and values for the next iteration + graph.resize(0); + initialEstimate.clear(); + } + } + + // check if edge + const auto betweenPose = parseEdge(is, tag); + if (betweenPose) { + Key id1, id2; + tie(id1, id2) = betweenPose->first; + graph.emplace_shared >(Symbol('x', id2), + Symbol('x', id1), betweenPose->second, model); + } + } + EXPECT_LONGS_EQUAL(16, isam.estimate().size()) +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */