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

remove column singletons in the factor graph

release/4.3a0
Kai Ni 2010-04-03 03:36:10 +00:00
parent 94395249fb
commit 41c77ef421
7 changed files with 97 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

34
cpp/BearingRangeFactor.h
View File

@ -21,20 +21,18 @@ namespace gtsam {
PointKey, Point2> {
private:
// the bearing factor
BearingFactor<Config, PoseKey, PointKey> bearing_;
// the range factor
RangeFactor<Config, PoseKey, PointKey> range_;
// the measurement
Rot2 bearing_;
double range_;
typedef NonlinearFactor2<Config, PoseKey, Pose2, PointKey, Point2> Base;
public:
BearingRangeFactor(); /* Default constructor */
BearingRangeFactor(const PoseKey& i, const PointKey& j, const std::pair<Rot2, double>& z,
BearingRangeFactor(const PoseKey& i, const PointKey& j, const Rot2& bearing, const double range,
const SharedGaussian& model) :
Base(model, i, j), bearing_(i, j, z.first, model), range_(i, j, z.second, model) {
Base(model, i, j), bearing_(bearing), range_(range) {
}
/** h(x)-z -> between(z,h(x)) for Rot2 manifold */
@ -45,24 +43,22 @@ namespace gtsam {
boost::optional<Matrix&> H21_ = H1 ? boost::optional<Matrix&>(H21) : boost::optional<Matrix&>();
boost::optional<Matrix&> H12_ = H2 ? boost::optional<Matrix&>(H12) : boost::optional<Matrix&>();
boost::optional<Matrix&> H22_ = H2 ? boost::optional<Matrix&>(H22) : boost::optional<Matrix&>();
Vector e1 = bearing_.evaluateError(pose, point, H11_, H12_);
Vector e2 = range_.evaluateError(pose, point, H21_, H22_);
if (H1) *H1 = stack_matrices(H11, H21);
if (H2) *H2 = stack_matrices(H12, H22);
Rot2 y1 = gtsam::bearing(pose, point, H11_, H12_);
Vector e1 = logmap(between(bearing_, y1));
double y2 = gtsam::range(pose, point, H21_, H22_);
Vector e2 = Vector_(1, y2 - range_);
if (H1) *H1 = gtsam::stack(2, &H11, &H21);
if (H2) *H2 = gtsam::stack(2, &H12, &H22);
return concatVectors(2, &e1, &e2);
}
/** return the measured */
inline const std::pair<Rot2, double> measured() const {
return concatVectors(2, bearing_.measured(), range_.measured());
return std::make_pair(bearing_, range_);
}
/** return the bearing factor */
const BearingFactor<Config, PoseKey, PointKey>& bearing() const { return bearing_; }
/** return the range factor */
const RangeFactor<Config, PoseKey, PointKey>& range() const { return range_; }
}; // BearingRangeFactor
} // namespace gtsam

38
cpp/FactorGraph-inl.h
View File

@ -134,6 +134,41 @@ Ordering FactorGraph<Factor>::keys() const {
return keys;
}
/* ************************************************************************* */
template<class Factor>
std::pair<FactorGraph<Factor>, set<Symbol> > FactorGraph<Factor>::removeSingletons() {
FactorGraph<Factor> singletonGraph;
set<Symbol> singletons;
while(true) {
// find all the singleton variables
Ordering new_singletons;
Symbol key;
list<int> indices;
BOOST_FOREACH(boost::tie(key, indices), indices_) {
// find out the number of factors associated with the current key
int numValidFactors = 0;
BOOST_FOREACH(const int& i, indices)
if (factors_[i]!=NULL) numValidFactors++;
if (numValidFactors == 1) {
new_singletons.push_back(key);
BOOST_FOREACH(const int& i, indices)
if (factors_[i]!=NULL) singletonGraph.push_back(factors_[i]);
}
}
singletons.insert(new_singletons.begin(), new_singletons.end());
BOOST_FOREACH(const Symbol& singleton, new_singletons)
findAndRemoveFactors(singleton);
// exit when there are no more singletons
if (new_singletons.empty()) break;
}
return make_pair(singletonGraph, singletons);
}
/* ************************************************************************* */
/**
* Call colamd given a column-major symbolic matrix A
@ -248,6 +283,9 @@ FactorGraph<Factor>::findAndRemoveFactors(const Symbol& key) {
found.push_back(fi); // add to found
fi.reset(); // set factor to NULL == remove(i)
}
indices_.erase(key);
return found;
}

5
cpp/FactorGraph.h
View File

@ -97,6 +97,11 @@ namespace gtsam {
return !(indices_.find(key)==indices_.end());
}
/** check whether a variable is a singleton, i.e. it only involve*/
/** remove singleton variables and the related factors */
std::pair<FactorGraph<Factor>, std::set<Symbol> > removeSingletons();
/**
* Compute colamd ordering, including I/O and shared pointer version
*/

5
cpp/Matrix.cpp
View File

@ -742,11 +742,6 @@ Matrix stack(size_t nrMatrices, ...)
return A;
}
/* ************************************************************************* */
Matrix stack_matrices(const Matrix& A, const Matrix& B) {
return stack(2, &A, &B);
}
/* ************************************************************************* */
Matrix collect(const std::vector<const Matrix *>& matrices, size_t m, size_t n)
{

3
cpp/Matrix.h
View File

@ -287,9 +287,6 @@ Vector backSubstituteLower(const Matrix& L, const Vector& d, bool unit=false);
*/
Matrix stack(size_t nrMatrices, ...);
/** a shortcut to prevent the name confliction with STL stack */
Matrix stack_matrices(const Matrix& A, const Matrix& B);
/**
* create a matrix by concatenating
* Given a set of matrices: A1, A2, A3...

38
cpp/testFactorgraph.cpp
View File

@ -9,6 +9,9 @@
#include <iostream>
#include <boost/shared_ptr.hpp>
#include <boost/tuple/tuple.hpp>
#include <boost/assign/std/set.hpp> // for operator +=
using namespace boost::assign;
#include <CppUnitLite/TestHarness.h>
#define GTSAM_MAGIC_KEY
@ -44,6 +47,41 @@ TEST( FactorGraph, splitMinimumSpanningTree )
CHECK(assert_equal(expectedC,C));
}
/* ************************************************************************* */
/**
* x1 - x2 - x3 - x4 - x5
* | | / |
* l1 l2 l3
*/
TEST( FactorGraph, removeSingletons )
{
SymbolicFactorGraph G;
G.push_factor("x1", "x2");
G.push_factor("x2", "x3");
G.push_factor("x3", "x4");
G.push_factor("x4", "x5");
G.push_factor("x2", "l1");
G.push_factor("x3", "l2");
G.push_factor("x4", "l2");
G.push_factor("x4", "l3");
SymbolicFactorGraph singletonGraph;
set<Symbol> singletons;
boost::tie(singletonGraph, singletons) = G.removeSingletons();
set<Symbol> singletons_excepted; singletons_excepted += "x1", "x2", "x5", "l1", "l3";
CHECK(singletons_excepted == singletons);
SymbolicFactorGraph singletonGraph_excepted;
singletonGraph_excepted.push_factor("x2", "l1");
singletonGraph_excepted.push_factor("x4", "l3");
singletonGraph_excepted.push_factor("x1", "x2");
singletonGraph_excepted.push_factor("x4", "x5");
singletonGraph_excepted.push_factor("x2", "x3");
CHECK(singletonGraph_excepted.equals(singletonGraph));
}
/* ************************************************************************* */
int main() {
TestResult tr;

2
cpp/testPlanarSLAM.cpp
View File

@ -61,7 +61,7 @@ TEST( planarSLAM, BearingRangeFactor )
// Create factor
Rot2 r = Rot2::fromAngle(M_PI_4 + 0.1); // h(x) - z = -0.1
double b(sqrt(2) - 0.22); // h(x) - z = 0.22
planarSLAM::BearingRange factor(2, 3, make_pair(r,b), sigma2);
planarSLAM::BearingRange factor(2, 3, r, b, sigma2);
// create config
planarSLAM::Config c;