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

Made HessianFactor::updateATA public to allow low-rank updates

release/4.3a0
Richard Roberts 2012-09-04 15:05:47 +00:00
parent 0d5cb9a7e0
commit 8e4f605122
2 changed files with 18 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
gtsam/linear/HessianFactor.cpp
View File

@ -261,12 +261,10 @@ HessianFactor::HessianFactor(const FactorGraph<GaussianFactor>& factors,
if (debug) cout << "Combining " << factors.size() << " factors" << endl;
BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factors)
{
shared_ptr hessian(boost::dynamic_pointer_cast<HessianFactor>(factor));
if (hessian)
updateATA(*hessian, scatter);
else {
JacobianFactor::shared_ptr jacobianFactor(boost::dynamic_pointer_cast<JacobianFactor>(factor));
if (jacobianFactor)
if(factor) {
if(shared_ptr hessian = boost::dynamic_pointer_cast<HessianFactor>(factor))
updateATA(*hessian, scatter);
else if(JacobianFactor::shared_ptr jacobianFactor = boost::dynamic_pointer_cast<JacobianFactor>(factor))
updateATA(*jacobianFactor, scatter);
else
throw invalid_argument("GaussianFactor is neither Hessian nor Jacobian");
@ -360,7 +358,6 @@ void HessianFactor::updateATA(const HessianFactor& update, const Scatter& scatte
// Apply updates to the upper triangle
tic(3, "update");
assert(this->info_.nBlocks() - 1 == scatter.size());
for(size_t j2=0; j2<update.info_.nBlocks(); ++j2) {
size_t slot2 = (j2 == update.size()) ? this->info_.nBlocks()-1 : slots[j2];
for(size_t j1=0; j1<=j2; ++j1) {
@ -437,7 +434,6 @@ void HessianFactor::updateATA(const JacobianFactor& update, const Scatter& scatt
// Apply updates to the upper triangle
tic(3, "update");
assert(this->info_.nBlocks() - 1 == scatter.size());
for(size_t j2=0; j2<update.Ab_.nBlocks(); ++j2) {
size_t slot2 = (j2 == update.size()) ? this->info_.nBlocks()-1 : slots[j2];
for(size_t j1=0; j1<=j2; ++j1) {

28
gtsam/linear/HessianFactor.h
View File

@ -120,20 +120,6 @@ namespace gtsam {
InfoMatrix matrix_; ///< The full augmented information matrix, s.t. the quadratic error is 0.5*[x -1]'*H*[x -1]
BlockInfo info_; ///< The block view of the full information matrix.
/** Update the factor by adding the information from the JacobianFactor
* (used internally during elimination).
* @param update The JacobianFactor containing the new information to add
* @param scatter A mapping from variable index to slot index in this HessianFactor
*/
void updateATA(const JacobianFactor& update, const Scatter& scatter);
/** Update the factor by adding the information from the HessianFactor
* (used internally during elimination).
* @param update The HessianFactor containing the new information to add
* @param scatter A mapping from variable index to slot index in this HessianFactor
*/
void updateATA(const HessianFactor& update, const Scatter& scatter);
public:
typedef boost::shared_ptr<HessianFactor> shared_ptr; ///< A shared_ptr to this
@ -315,6 +301,20 @@ namespace gtsam {
/** split partially eliminated factor */
boost::shared_ptr<GaussianConditional> splitEliminatedFactor(size_t nrFrontals);
/** Update the factor by adding the information from the JacobianFactor
* (used internally during elimination).
* @param update The JacobianFactor containing the new information to add
* @param scatter A mapping from variable index to slot index in this HessianFactor
*/
void updateATA(const JacobianFactor& update, const Scatter& scatter);
/** Update the factor by adding the information from the HessianFactor
* (used internally during elimination).
* @param update The HessianFactor containing the new information to add
* @param scatter A mapping from variable index to slot index in this HessianFactor
*/
void updateATA(const HessianFactor& update, const Scatter& scatter);
/** assert invariants */
void assertInvariants() const;