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

clean the gfg to jfg conversion code

release/4.3a0
Yong-Dian Jian 2012-09-05 17:04:48 +00:00
parent 6c2f213091
commit 4443752a18
5 changed files with 36 additions and 60 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

73
gtsam/linear/GaussianFactorGraph.cpp
View File

@ -521,15 +521,14 @@ break;
} // \EliminatePreferCholesky
/* ************************************************************************* */
Errors operator*(const GaussianFactorGraph& fg, const VectorValues& x) {
Errors e;
BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) {
JacobianFactor::shared_ptr Ai;
if(JacobianFactor::shared_ptr Ai_J = boost::dynamic_pointer_cast<JacobianFactor>(Ai_G))
Ai = Ai_J;
else
Ai = boost::make_shared<JacobianFactor>(*Ai_G); // Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with Cholesky)
JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G);
e.push_back((*Ai)*x);
}
return e;
@ -544,12 +543,8 @@ break;
void multiplyInPlace(const GaussianFactorGraph& fg, const VectorValues& x, const Errors::iterator& e) {
Errors::iterator ei = e;
BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) {
JacobianFactor::shared_ptr Ai;
if(JacobianFactor::shared_ptr Ai_J = boost::dynamic_pointer_cast<JacobianFactor>(Ai_G))
Ai = Ai_J;
else
Ai = boost::make_shared<JacobianFactor>(*Ai_G); // Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with Cholesky)
*ei = (*Ai)*x;
JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G);
*ei = (*Ai)*x;
ei++;
}
}
@ -560,12 +555,8 @@ break;
// For each factor add the gradient contribution
Errors::const_iterator ei = e.begin();
BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) {
JacobianFactor::shared_ptr Ai;
if(JacobianFactor::shared_ptr Ai_J = boost::dynamic_pointer_cast<JacobianFactor>(Ai_G))
Ai = Ai_J;
else
Ai = boost::make_shared<JacobianFactor>(*Ai_G); // Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with Cholesky)
Ai->transposeMultiplyAdd(alpha,*(ei++),x);
JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G);
Ai->transposeMultiplyAdd(alpha,*(ei++),x);
}
}
@ -575,12 +566,8 @@ break;
VectorValues g = VectorValues::Zero(x0);
Errors e;
BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) {
JacobianFactor::shared_ptr Ai;
if(JacobianFactor::shared_ptr Ai_J = boost::dynamic_pointer_cast<JacobianFactor>(Ai_G))
Ai = Ai_J;
else
Ai = boost::make_shared<JacobianFactor>(*Ai_G); // Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with Cholesky)
e.push_back(Ai->error_vector(x0));
JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G);
e.push_back(Ai->error_vector(x0));
}
transposeMultiplyAdd(fg, 1.0, e, g);
return g;
@ -592,12 +579,8 @@ break;
g.setZero();
Errors e;
BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) {
JacobianFactor::shared_ptr Ai;
if(JacobianFactor::shared_ptr Ai_J = boost::dynamic_pointer_cast<JacobianFactor>(Ai_G))
Ai = Ai_J;
else
Ai = boost::make_shared<JacobianFactor>(*Ai_G); // Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with Cholesky)
e.push_back(-Ai->getb());
JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G);
e.push_back(-Ai->getb());
}
transposeMultiplyAdd(fg, 1.0, e, g);
}
@ -606,12 +589,8 @@ break;
void residual(const GaussianFactorGraph& fg, const VectorValues &x, VectorValues &r) {
Index i = 0 ;
BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) {
JacobianFactor::shared_ptr Ai;
if(JacobianFactor::shared_ptr Ai_J = boost::dynamic_pointer_cast<JacobianFactor>(Ai_G))
Ai = Ai_J;
else
Ai = boost::make_shared<JacobianFactor>(*Ai_G); // Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with Cholesky)
r[i] = Ai->getb();
JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G);
r[i] = Ai->getb();
i++;
}
VectorValues Ax = VectorValues::SameStructure(r);
@ -624,12 +603,8 @@ break;
r.vector() = Vector::Zero(r.dim());
Index i = 0;
BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) {
JacobianFactor::shared_ptr Ai;
if(JacobianFactor::shared_ptr Ai_J = boost::dynamic_pointer_cast<JacobianFactor>(Ai_G))
Ai = Ai_J;
else
Ai = boost::make_shared<JacobianFactor>(*Ai_G); // Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with Cholesky)
SubVector &y = r[i];
JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G);
SubVector &y = r[i];
for(JacobianFactor::const_iterator j = Ai->begin(); j != Ai->end(); ++j) {
y += Ai->getA(j) * x[*j];
}
@ -642,12 +617,8 @@ break;
x.vector() = Vector::Zero(x.dim());
Index i = 0;
BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) {
JacobianFactor::shared_ptr Ai;
if(JacobianFactor::shared_ptr Ai_J = boost::dynamic_pointer_cast<JacobianFactor>(Ai_G))
Ai = Ai_J;
else
Ai = boost::make_shared<JacobianFactor>(*Ai_G); // Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with Cholesky)
for(JacobianFactor::const_iterator j = Ai->begin(); j != Ai->end(); ++j) {
JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G);
for(JacobianFactor::const_iterator j = Ai->begin(); j != Ai->end(); ++j) {
x[*j] += Ai->getA(j).transpose() * r[i];
}
++i;
@ -658,12 +629,8 @@ break;
boost::shared_ptr<Errors> gaussianErrors_(const GaussianFactorGraph& fg, const VectorValues& x) {
boost::shared_ptr<Errors> e(new Errors);
BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) {
JacobianFactor::shared_ptr Ai;
if(JacobianFactor::shared_ptr Ai_J = boost::dynamic_pointer_cast<JacobianFactor>(Ai_G))
Ai = Ai_J;
else
Ai = boost::make_shared<JacobianFactor>(*Ai_G); // Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with Cholesky)
e->push_back(Ai->error_vector(x));
JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G);
e->push_back(Ai->error_vector(x));
}
return e;
}

4
gtsam/linear/GaussianFactorGraph.h
View File

@ -303,7 +303,9 @@ namespace gtsam {
*/
GaussianFactorGraph::EliminationResult EliminateCholesky(const FactorGraph<
GaussianFactor>& factors, size_t nrFrontals = 1);
/****** Linear Algebra Opeations ******/
/** return A*x */
Errors operator*(const GaussianFactorGraph& fg, const VectorValues& x);

9
gtsam/linear/JacobianFactor.cpp
View File

@ -489,4 +489,13 @@ namespace gtsam {
return description_.c_str();
}
/* ************************************************************************* */
JacobianFactor::shared_ptr convertToJacobianFactorPtr(const GaussianFactor::shared_ptr &gf) {
JacobianFactor::shared_ptr result = boost::dynamic_pointer_cast<JacobianFactor>(gf);
if( !result ) {
result = boost::make_shared<JacobianFactor>(*gf); // Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with Cholesky)
}
return result;
}
}

4
gtsam/linear/JacobianFactor.h
View File

@ -322,8 +322,10 @@ namespace gtsam {
ar & BOOST_SERIALIZATION_NVP(model_);
ar & BOOST_SERIALIZATION_NVP(matrix_);
}
}; // JacobianFactor
/** cast from GaussianFactor::shared_ptr to JacobianFactor::shared_ptr */
JacobianFactor::shared_ptr convertToJacobianFactorPtr(const GaussianFactor::shared_ptr &gf);
} // gtsam

6
gtsam/linear/SubgraphSolver.cpp
View File

@ -111,11 +111,7 @@ SubgraphSolver::splitGraph(const GaussianFactorGraph &jfg) {
size_t t = 0;
BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, jfg ) {
JacobianFactor::shared_ptr jf;
if(JacobianFactor::shared_ptr Ai_J = boost::dynamic_pointer_cast<JacobianFactor>(gf))
jf = Ai_J;
else
jf = boost::make_shared<JacobianFactor>(*gf); // Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with Cholesky)
JacobianFactor::shared_ptr jf = convertToJacobianFactorPtr(gf);
if ( jf->keys().size() > 2 ) {
throw runtime_error("SubgraphSolver::splitGraph the graph is not simple, sanity check failed ");