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

Copied back-substitution pointer caching scheme from ISAM to batch

release/4.3a0
Richard Roberts 2013-08-15 17:39:15 +00:00
parent 4e8fe533c8
commit 7767f813e8
1 changed files with 39 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

60
gtsam/linear/linearAlgorithms-inst.h
View File

@ -30,7 +30,7 @@ namespace gtsam
/* ************************************************************************* */ /* ************************************************************************* */
struct OptimizeData { struct OptimizeData {
boost::optional<OptimizeData&> parentData; boost::optional<OptimizeData&> parentData;
//FastMap<Key, VectorValues::const_iterator> cliqueResults; FastMap<Key, VectorValues::const_iterator> cliqueResults;
//VectorValues ancestorResults; //VectorValues ancestorResults;
//VectorValues results; //VectorValues results;
}; };
@ -54,29 +54,47 @@ namespace gtsam
OptimizeData myData; OptimizeData myData;
myData.parentData = parentData; myData.parentData = parentData;
// Take any ancestor results we'll need // Take any ancestor results we'll need
//BOOST_FOREACH(Key parent, clique->conditional_->parents()) BOOST_FOREACH(Key parent, clique->conditional_->parents())
// myData.cliqueResults.insert(std::make_pair(parent, myData.parentData->cliqueResults.at(parent))); myData.cliqueResults.insert(std::make_pair(parent, myData.parentData->cliqueResults.at(parent)));
// Solve and store in our results // Solve and store in our results
collectedResult.insert(clique->conditional()->solve(collectedResult/*myData.ancestorResults*/)); //collectedResult.insert(clique->conditional()->solve(collectedResult/*myData.ancestorResults*/));
//{ {
// GaussianConditional& c = *clique->conditional(); GaussianConditional& c = *clique->conditional();
// // Solve matrix // Solve matrix
// Vector xS = x.vector(vector<Key>(c.beginParents(), c.endParents())); Vector xS;
// xS = c.getb() - c.get_S() * xS; {
// Vector soln = c.get_R().triangularView<Eigen::Upper>().solve(xS); // Count dimensions of vector
DenseIndex dim = 0;
FastVector<VectorValues::const_iterator> parentPointers;
parentPointers.reserve(clique->conditional()->nrParents());
BOOST_FOREACH(Key parent, clique->conditional()->parents()) {
parentPointers.push_back(myData.cliqueResults.at(parent));
dim += parentPointers.back()->second.size();
}
// // Check for indeterminant solution // Fill parent vector
// if(soln.hasNaN()) throw IndeterminantLinearSystemException(c.keys().front()); xS.resize(dim);
DenseIndex vectorPos = 0;
BOOST_FOREACH(const VectorValues::const_iterator& parentPointer, parentPointers) {
xS.segment(vectorPos, parentPointer->second.size()) = parentPointer->second;
vectorPos += parentPointer->second.size();
}
}
xS = c.getb() - c.get_S() * xS;
Vector soln = c.get_R().triangularView<Eigen::Upper>().solve(xS);
// // Insert solution into a VectorValues // Check for indeterminant solution
// DenseIndex vectorPosition = 0; if(soln.hasNaN()) throw IndeterminantLinearSystemException(c.keys().front());
// for(GaussianConditional::const_iterator frontal = c.beginFrontals(); frontal != c.endFrontals(); ++frontal) {
// VectorValues::const_iterator r = // Insert solution into a VectorValues
// collectedResult.insert(*frontal, soln.segment(vectorPosition, c.getDim(frontal))); DenseIndex vectorPosition = 0;
// myData.cliqueResults.insert(r->first, r); for(GaussianConditional::const_iterator frontal = c.beginFrontals(); frontal != c.endFrontals(); ++frontal) {
// vectorPosition += c.getDim(frontal); VectorValues::const_iterator r =
// } collectedResult.insert(*frontal, soln.segment(vectorPosition, c.getDim(frontal)));
//} myData.cliqueResults.insert(make_pair(r->first, r));
vectorPosition += c.getDim(frontal);
}
}
return myData; return myData;
} }
}; };