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

To simplify FactorGraph, removed convertCastFactors and dynamicCastFactors from FactorGraph - replaced their calls with in-place code to do the needed conversions

release/4.3a0
Richard Roberts 2012-06-18 14:55:30 +00:00
parent 4ec1cc9e5a
commit d57fc32e74
14 changed files with 104 additions and 80 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

33
gtsam/inference/FactorGraph.h
View File

@ -200,39 +200,6 @@ template<class CONDITIONAL, class CLIQUE> class BayesTree;
/** return the number valid factors */
size_t nrFactors() const;
/** dynamic_cast the factor pointers down or up the class hierarchy */
template<class RELATED>
typename RELATED::shared_ptr dynamicCastFactors() const {
typename RELATED::shared_ptr ret(new RELATED);
ret->reserve(this->size());
BOOST_FOREACH(const sharedFactor& factor, *this) {
typename RELATED::FactorType::shared_ptr castedFactor(boost::dynamic_pointer_cast<typename RELATED::FactorType>(factor));
if(castedFactor)
ret->push_back(castedFactor);
else
throw std::invalid_argument("In FactorGraph<FACTOR>::dynamic_factor_cast(), dynamic_cast failed, meaning an invalid cast was requested.");
}
return ret;
}
/**
* dynamic_cast factor pointers if possible, otherwise convert with a
* constructor of the target type.
*/
template<class TARGET>
typename TARGET::shared_ptr convertCastFactors() const {
typename TARGET::shared_ptr ret(new TARGET);
ret->reserve(this->size());
BOOST_FOREACH(const sharedFactor& factor, *this) {
typename TARGET::FactorType::shared_ptr castedFactor(boost::dynamic_pointer_cast<typename TARGET::FactorType>(factor));
if(castedFactor)
ret->push_back(castedFactor);
else
ret->push_back(typename TARGET::FactorType::shared_ptr(new typename TARGET::FactorType(*factor)));
}
return ret;
}
private:
/** Serialization function */

6
gtsam/inference/tests/testFactorGraph.cpp
View File

@ -91,12 +91,6 @@ typedef boost::shared_ptr<SymbolicFactorGraph> shared;
// CHECK(singletonGraph_excepted.equals(singletonGraph));
//}
/* ************************************************************************* */
TEST(FactorGraph, dynamic_factor_cast) {
FactorGraph<IndexFactor> fg;
fg.dynamicCastFactors<FactorGraph<IndexFactor> >();
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */

2
gtsam/linear/GaussianConditional.h
View File

@ -80,7 +80,7 @@ public:
GaussianConditional();
/** constructor */
GaussianConditional(Index key);
explicit GaussianConditional(Index key);
/** constructor with no parents
* |Rx-d|

13
gtsam/linear/GaussianFactorGraph.cpp
View File

@ -160,9 +160,18 @@ namespace gtsam {
/* ************************************************************************* */
Matrix GaussianFactorGraph::denseJacobian() const {
// Convert to Jacobians
FactorGraph<JacobianFactor> jfg;
jfg.reserve(this->size());
BOOST_FOREACH(const sharedFactor& factor, *this) {
if(boost::shared_ptr<JacobianFactor> jf =
boost::dynamic_pointer_cast<JacobianFactor>(factor))
jfg.push_back(jf);
else
jfg.push_back(boost::make_shared<JacobianFactor>(*factor));
}
// combine all factors
JacobianFactor combined(*CombineJacobians(*convertCastFactors<FactorGraph<
JacobianFactor> > (), VariableSlots(*this)));
JacobianFactor combined(*CombineJacobians(jfg, VariableSlots(*this)));
return combined.matrix_augmented();
}

4
gtsam/linear/HessianFactor.h
View File

@ -197,10 +197,10 @@ namespace gtsam {
const std::vector<Vector>& gs, double f);
/** Construct from Conditional Gaussian */
HessianFactor(const GaussianConditional& cg);
explicit HessianFactor(const GaussianConditional& cg);
/** Convert from a JacobianFactor (computes A^T * A) or HessianFactor */
HessianFactor(const GaussianFactor& factor);
explicit HessianFactor(const GaussianFactor& factor);
/** Special constructor used in EliminateCholesky which combines the given factors */
HessianFactor(const FactorGraph<GaussianFactor>& factors,

12
gtsam/linear/JacobianFactor.cpp
View File

@ -66,6 +66,18 @@ namespace gtsam {
assertInvariants();
}
/* ************************************************************************* */
JacobianFactor::JacobianFactor(const GaussianFactor& gf) : Ab_(matrix_) {
// Copy the matrix data depending on what type of factor we're copying from
if(const JacobianFactor* rhs = dynamic_cast<const JacobianFactor*>(&gf))
*this = JacobianFactor(*rhs);
else if(const HessianFactor* rhs = dynamic_cast<const HessianFactor*>(&gf))
*this = JacobianFactor(*rhs);
else
throw std::invalid_argument("In JacobianFactor(const GaussianFactor& rhs), rhs is neither a JacobianFactor nor a HessianFactor");
assertInvariants();
}
/* ************************************************************************* */
JacobianFactor::JacobianFactor() : Ab_(matrix_) { assertInvariants(); }

3
gtsam/linear/JacobianFactor.h
View File

@ -102,6 +102,9 @@ namespace gtsam {
/** Copy constructor */
JacobianFactor(const JacobianFactor& gf);
/** Convert from other GaussianFactor */
JacobianFactor(const GaussianFactor& gf);
/** default constructor for I/O */
JacobianFactor();

27
gtsam/linear/SubgraphSolver.cpp
View File

@ -52,10 +52,29 @@ SubgraphSolver::SubgraphSolver(const GaussianFactorGraph &gfg, const Parameters
GaussianBayesNet::shared_ptr Rc1 = EliminationTree<GaussianFactor>::Create(*Ab1)->eliminate(&EliminateQR);
VectorValues::shared_ptr xbar(new VectorValues(gtsam::optimize(*Rc1)));
pc_ = boost::make_shared<SubgraphPreconditioner>(
Ab1->dynamicCastFactors<FactorGraph<JacobianFactor> >(),
Ab2->dynamicCastFactors<FactorGraph<JacobianFactor> >(),
Rc1, xbar);
// Convert or cast Ab1 to JacobianFactors
boost::shared_ptr<FactorGraph<JacobianFactor> > Ab1Jacobians = boost::make_shared<FactorGraph<JacobianFactor> >();
Ab1Jacobians->reserve(Ab1->size());
BOOST_FOREACH(const boost::shared_ptr<GaussianFactor>& factor, *Ab1) {
if(boost::shared_ptr<JacobianFactor> jf =
boost::dynamic_pointer_cast<JacobianFactor>(factor))
Ab1Jacobians->push_back(jf);
else
Ab1Jacobians->push_back(boost::make_shared<JacobianFactor>(*factor));
}
// Convert or cast Ab2 to JacobianFactors
boost::shared_ptr<FactorGraph<JacobianFactor> > Ab2Jacobians = boost::make_shared<FactorGraph<JacobianFactor> >();
Ab1Jacobians->reserve(Ab2->size());
BOOST_FOREACH(const boost::shared_ptr<GaussianFactor>& factor, *Ab2) {
if(boost::shared_ptr<JacobianFactor> jf =
boost::dynamic_pointer_cast<JacobianFactor>(factor))
Ab2Jacobians->push_back(jf);
else
Ab2Jacobians->push_back(boost::make_shared<JacobianFactor>(*factor));
}
pc_ = boost::make_shared<SubgraphPreconditioner>(Ab1Jacobians, Ab2Jacobians, Rc1, xbar);
}
VectorValues SubgraphSolver::optimize() {

28
gtsam/linear/tests/testGaussianFactorGraph.cpp
View File

@ -50,10 +50,8 @@ TEST(GaussianFactorGraph, initialization) {
fg.add(0, -5*eye(2), 2, 5*eye(2), Vector_(2, 0.0, 1.0), unit2);
fg.add(1, -5*eye(2), 2, 5*eye(2), Vector_(2, -1.0, 1.5), unit2);
FactorGraph<JacobianFactor> graph = *fg.dynamicCastFactors<FactorGraph<JacobianFactor> >();
EXPECT_LONGS_EQUAL(4, graph.size());
JacobianFactor factor = *graph[0];
EXPECT_LONGS_EQUAL(4, fg.size());
JacobianFactor factor = *boost::dynamic_pointer_cast<JacobianFactor>(fg[0]);
// Test sparse, which takes a vector and returns a matrix, used in MATLAB
// Note that this the augmented vector and the RHS is in column 7
@ -155,8 +153,14 @@ TEST(GaussianFactorGraph, Combine2)
gfg.add(0, A10, 1, A11, b1, noiseModel::Diagonal::Sigmas(s1, true));
gfg.add(1, A21, b2, noiseModel::Diagonal::Sigmas(s2, true));
JacobianFactor actual = *CombineJacobians(*gfg.dynamicCastFactors<FactorGraph<
JacobianFactor> > (), VariableSlots(gfg));
// Convert to Jacobians, inefficient copy of all factors instead of selectively converting only Hessians
FactorGraph<JacobianFactor> jacobians;
BOOST_FOREACH(const GaussianFactorGraph::sharedFactor& factor, gfg) {
jacobians.push_back(boost::make_shared<JacobianFactor>(*factor));
}
// Combine Jacobians into a single dense factor
JacobianFactor actual = *CombineJacobians(jacobians, VariableSlots(gfg));
Matrix zero3x3 = zeros(3,3);
Matrix A0 = gtsam::stack(3, &A10, &zero3x3, &zero3x3);
@ -213,8 +217,7 @@ TEST(GaussianFactor, CombineAndEliminate)
GaussianConditional::shared_ptr actualBN;
GaussianFactor::shared_ptr actualFactor;
boost::tie(actualBN, actualFactor) = //
EliminateQR(*gfg.dynamicCastFactors<FactorGraph<JacobianFactor> > (), 1);
boost::tie(actualBN, actualFactor) = EliminateQR(gfg, 1);
JacobianFactor::shared_ptr actualJacobian = boost::dynamic_pointer_cast<
JacobianFactor>(actualFactor);
@ -410,9 +413,14 @@ TEST(GaussianFactor, eliminateFrontals)
Matrix actualDense = factors.denseJacobian();
EXPECT(assert_equal(2.0 * Ab, actualDense));
// Convert to Jacobians, inefficient copy of all factors instead of selectively converting only Hessians
FactorGraph<JacobianFactor> jacobians;
BOOST_FOREACH(const GaussianFactorGraph::sharedFactor& factor, factors) {
jacobians.push_back(boost::make_shared<JacobianFactor>(*factor));
}
// Create combined factor
JacobianFactor combined(*CombineJacobians(*factors.dynamicCastFactors<FactorGraph<
JacobianFactor> > (), VariableSlots(factors)));
JacobianFactor combined(*CombineJacobians(jacobians, VariableSlots(factors)));
// Copies factors as they will be eliminated in place
JacobianFactor actualFactor_QR = combined;

18
gtsam/linear/tests/testHessianFactor.cpp
View File

@ -414,10 +414,20 @@ TEST_UNSAFE(HessianFactor, CombineAndEliminate)
// create expected Hessian after elimination
HessianFactor expectedCholeskyFactor(expectedFactor);
GaussianFactorGraph::EliminationResult actualCholesky = EliminateCholesky(
*gfg.convertCastFactors<FactorGraph<HessianFactor> > (), 1);
HessianFactor::shared_ptr actualFactor = boost::dynamic_pointer_cast<
HessianFactor>(actualCholesky.second);
// Convert all factors to hessians
FactorGraph<HessianFactor> hessians;
BOOST_FOREACH(const GaussianFactorGraph::sharedFactor& factor, gfg) {
if(boost::shared_ptr<HessianFactor> hf = boost::dynamic_pointer_cast<HessianFactor>(factor))
hessians.push_back(hf);
else if(boost::shared_ptr<JacobianFactor> jf = boost::dynamic_pointer_cast<JacobianFactor>(factor))
hessians.push_back(boost::make_shared<HessianFactor>(*jf));
else
CHECK(false);
}
// Eliminate
GaussianFactorGraph::EliminationResult actualCholesky = EliminateCholesky(gfg, 1);
HessianFactor::shared_ptr actualFactor = boost::dynamic_pointer_cast<HessianFactor>(actualCholesky.second);
EXPECT(assert_equal(*expectedBN, *actualCholesky.first, 1e-6));
EXPECT(assert_equal(expectedCholeskyFactor, *actualFactor, 1e-6));

10
gtsam/nonlinear/GradientDescentOptimizer.cpp
View File

@ -25,10 +25,16 @@ void gradientInPlace(const NonlinearFactorGraph &nfg, const Values &values, cons
// Linearize graph
GaussianFactorGraph::shared_ptr linear = nfg.linearize(values, ordering);
const FactorGraph<JacobianFactor>::shared_ptr jfg = linear->dynamicCastFactors<FactorGraph<JacobianFactor> >();
FactorGraph<JacobianFactor> jfg; jfg.reserve(linear->size());
BOOST_FOREACH(const GaussianFactorGraph::sharedFactor& factor, *linear) {
if(boost::shared_ptr<JacobianFactor> jf = boost::dynamic_pointer_cast<JacobianFactor>(factor))
jfg.push_back((jf));
else
jfg.push_back(boost::make_shared<JacobianFactor>(*factor));
}
// compute the gradient direction
gradientAtZero(*jfg, g);
gradientAtZero(jfg, g);
}

6
gtsam/nonlinear/NonlinearISAM.cpp
View File

@ -58,8 +58,7 @@ void NonlinearISAM::update(const NonlinearFactorGraph& newFactors,
BOOST_FOREACH(Key key, factor->keys())
ordering_.tryInsert(key, ordering_.nVars()); // will do nothing if already present
boost::shared_ptr<GaussianFactorGraph> linearizedNewFactors(
newFactors.linearize(linPoint_, ordering_)->dynamicCastFactors<GaussianFactorGraph>());
boost::shared_ptr<GaussianFactorGraph> linearizedNewFactors = newFactors.linearize(linPoint_, ordering_);
// Update ISAM
isam_.update(*linearizedNewFactors);
@ -81,8 +80,7 @@ void NonlinearISAM::reorder_relinearize() {
ordering_ = *factors_.orderingCOLAMD(newLinPoint);
// Create a linear factor graph at the new linearization point
boost::shared_ptr<GaussianFactorGraph> gfg(
factors_.linearize(newLinPoint, ordering_)->dynamicCastFactors<GaussianFactorGraph>());
boost::shared_ptr<GaussianFactorGraph> gfg = factors_.linearize(newLinPoint, ordering_);
// Just recreate the whole BayesTree
isam_.update(*gfg);

10
gtsam/slam/tests/testPlanarSLAM.cpp
View File

@ -174,12 +174,10 @@ TEST( planarSLAM, constructor )
Vector expected2 = Vector_(1, -0.1);
Vector expected3 = Vector_(1, 0.22);
// Get NoiseModelFactors
FactorGraph<NoiseModelFactor > GNM =
*G.dynamicCastFactors<FactorGraph<NoiseModelFactor > >();
EXPECT(assert_equal(expected0, GNM[0]->unwhitenedError(c)));
EXPECT(assert_equal(expected1, GNM[1]->unwhitenedError(c)));
EXPECT(assert_equal(expected2, GNM[2]->unwhitenedError(c)));
EXPECT(assert_equal(expected3, GNM[3]->unwhitenedError(c)));
EXPECT(assert_equal(expected0, boost::dynamic_pointer_cast<NoiseModelFactor>(G[0])->unwhitenedError(c)));
EXPECT(assert_equal(expected1, boost::dynamic_pointer_cast<NoiseModelFactor>(G[1])->unwhitenedError(c)));
EXPECT(assert_equal(expected2, boost::dynamic_pointer_cast<NoiseModelFactor>(G[2])->unwhitenedError(c)));
EXPECT(assert_equal(expected3, boost::dynamic_pointer_cast<NoiseModelFactor>(G[3])->unwhitenedError(c)));
}
/* ************************************************************************* */

12
tests/smallExample.cpp
View File

@ -138,23 +138,23 @@ namespace example {
/* ************************************************************************* */
JacobianFactorGraph createGaussianFactorGraph(const Ordering& ordering) {
// Create empty graph
GaussianFactorGraph fg;
JacobianFactorGraph fg;
SharedDiagonal unit2 = noiseModel::Unit::Create(2);
// linearized prior on x1: c[_x1_]+x1=0 i.e. x1=-c[_x1_]
fg.add(ordering[X(1)], 10*eye(2), -1.0*ones(2), unit2);
fg.push_back(boost::make_shared<JacobianFactor>(ordering[X(1)], 10*eye(2), -1.0*ones(2), unit2));
// odometry between x1 and x2: x2-x1=[0.2;-0.1]
fg.add(ordering[X(1)], -10*eye(2),ordering[X(2)], 10*eye(2), Vector_(2, 2.0, -1.0), unit2);
fg.push_back(boost::make_shared<JacobianFactor>(ordering[X(1)], -10*eye(2),ordering[X(2)], 10*eye(2), Vector_(2, 2.0, -1.0), unit2));
// measurement between x1 and l1: l1-x1=[0.0;0.2]
fg.add(ordering[X(1)], -5*eye(2), ordering[L(1)], 5*eye(2), Vector_(2, 0.0, 1.0), unit2);
fg.push_back(boost::make_shared<JacobianFactor>(ordering[X(1)], -5*eye(2), ordering[L(1)], 5*eye(2), Vector_(2, 0.0, 1.0), unit2));
// measurement between x2 and l1: l1-x2=[-0.2;0.3]
fg.add(ordering[X(2)], -5*eye(2), ordering[L(1)], 5*eye(2), Vector_(2, -1.0, 1.5), unit2);
fg.push_back(boost::make_shared<JacobianFactor>(ordering[X(2)], -5*eye(2), ordering[L(1)], 5*eye(2), Vector_(2, -1.0, 1.5), unit2));
return *fg.dynamicCastFactors<JacobianFactorGraph >();
return fg;
}
/* ************************************************************************* */