12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merged in fix/iSAM-334 (pull request #310) 

Fix/iSAM-334
release/4.3a0
Frank Dellaert 2018-05-22 16:34:59 +00:00
parent 5df4af063d ebf047b4aa
commit 250a039435
5 changed files with 245 additions and 73 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

75
gtsam/inference/ISAM-inst.h
View File

@ -22,43 +22,48 @@
namespace gtsam {
/* ************************************************************************* */
template<class BAYESTREE>
void ISAM<BAYESTREE>::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<Key>(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<BayesTreeOrphanWrapper<Clique> >(orphan);
// eliminate into a Bayes net
const VariableIndex varIndex(factors);
const KeySet newFactorKeys = newFactors.keys();
const Ordering constrainedOrdering =
Ordering::ColamdConstrainedLast(varIndex, std::vector<Key>(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<class BAYESTREE>
void ISAM<BAYESTREE>::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<Key> keyVector(newFactorKeys.begin(), newFactorKeys.end());
this->removeTop(keyVector, bn, orphans);
}
/* ************************************************************************* */
template<class BAYESTREE>
void ISAM<BAYESTREE>::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<BayesTreeOrphanWrapper<Clique> >(orphan);
// Get an ordering where the new keys are eliminated last
const VariableIndex index(factors);
const Ordering ordering = Ordering::ColamdConstrainedLast(index,
std::vector<Key>(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<class BAYESTREE>
void ISAM<BAYESTREE>::update(const FactorGraphType& newFactors,
const Eliminate& function) {
Cliques orphans;
this->update_internal(newFactors, orphans, function);
}
}
/// namespace gtsam

97
gtsam/slam/dataset.cpp
View File

@ -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<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
// 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<string> 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<IndexedPose> 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<IndexedEdge> 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<Key, Key>(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,

21
gtsam/slam/dataset.h
View File

@ -33,6 +33,7 @@
#include <string>
#include <utility> // for pair
#include <vector>
#include <iosfwd>
namespace gtsam {
@ -71,6 +72,26 @@ enum KernelFunctionType {
KernelFunctionTypeNONE, KernelFunctionTypeHUBER, KernelFunctionTypeTUKEY
};
/// Return type for auxiliary functions
typedef std::pair<Key, Pose2> IndexedPose;
typedef std::pair<std::pair<Key, Key>, 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<IndexedPose> 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<IndexedEdge> parseEdge(std::istream& is,
const std::string& tag);
/// Return type for load functions
typedef std::pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> GraphAndValues;

34
gtsam/slam/tests/testDataset.cpp
View File

@ -26,6 +26,9 @@
#include <CppUnitLite/TestHarness.h>
#include <iostream>
#include <sstream>
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<Key, Key> expected(0, 1);
EXPECT(expected == actual->first);
EXPECT(assert_equal(Pose2(2, 3, 4), actual->second));
}
}
/* ************************************************************************* */
TEST( dataSet, load2D)
{

91
tests/testNonlinearISAM.cpp
View File

@ -7,6 +7,7 @@
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/sam/BearingRangeFactor.h>
#include <gtsam/slam/dataset.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/nonlinear/NonlinearEquality.h>
#include <gtsam/nonlinear/NonlinearISAM.h>
@ -16,7 +17,10 @@
#include <gtsam/linear/Sampler.h>
#include <gtsam/geometry/Pose2.h>
#include <iostream>
#include <sstream>
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<Point2>(lm3), actualQR.at<Point2>(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<string> 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<PriorFactor<Pose2> >(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<BetweenFactor<Pose2> >(Symbol('x', id2),
Symbol('x', id1), betweenPose->second, model);
}
}
EXPECT_LONGS_EQUAL(16, isam.estimate().size())
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */