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

Using Eigen to combine HessianFactors

release/4.3a0
Richard Roberts 2011-02-07 02:49:58 +00:00
parent ab3dd665a5
commit 0339a33de0
5 changed files with 37 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
gtsam/base/blockMatrices.h
View File

@ -227,7 +227,7 @@ public:
assertInvariants();
size_t actualBlock = block + blockStart_;
checkBlock(actualBlock);
return variableColOffsets_[actualBlock] - variableColOffsets_[blockStart_];
return variableColOffsets_[actualBlock];
}
size_t& rowStart() { return rowStart_; }
@ -484,7 +484,7 @@ public:
assertInvariants();
size_t actualBlock = block + blockStart_;
checkBlock(actualBlock);
return variableColOffsets_[actualBlock] - variableColOffsets_[blockStart_];
return variableColOffsets_[actualBlock];
}
size_t& blockStart() { return blockStart_; }

34
gtsam/linear/HessianFactor.cpp
View File

@ -41,6 +41,9 @@
#include <boost/numeric/ublas/vector_proxy.hpp>
#include <boost/numeric/ublas/blas.hpp>
#include <gtsam/3rdparty/Eigen/Core>
#include <gtsam/3rdparty/Eigen/Dense>
#include <sstream>
using namespace std;
@ -236,6 +239,11 @@ void HessianFactor::updateATA(const HessianFactor& update, const Scatter& scatte
}
toc(1, "slots");
if(debug) {
this->print("Updating this: ");
update.print("with: ");
}
// Apply updates to the upper triangle
tic(2, "update");
assert(this->info_.nBlocks() - 1 == scatter.size());
@ -246,19 +254,33 @@ void HessianFactor::updateATA(const HessianFactor& update, const Scatter& scatte
if(slot2 > slot1) {
if(debug)
cout << "Updating (" << slot1 << "," << slot2 << ") from (" << j1 << "," << j2 << ")" << endl;
ublas::noalias(this->info_(slot1, slot2)) += update.info_(j1,j2);
// ublas::noalias(this->info_(slot1, slot2)) += update.info_(j1,j2);
Eigen::Map<Eigen::MatrixXd>(&matrix_(0,0), matrix_.size1(), matrix_.size2()).block(
info_.offset(slot1), info_.offset(slot2), info_(slot1,slot2).size1(), info_(slot1,slot2).size2()) +=
Eigen::Map<Eigen::MatrixXd>(&update.matrix_(0,0), update.matrix_.size1(), update.matrix_.size2()).block(
update.info_.offset(j1), update.info_.offset(j2), update.info_(j1,j2).size1(), update.info_(j1,j2).size2());
} else if(slot1 > slot2) {
if(debug)
cout << "Updating (" << slot2 << "," << slot1 << ") from (" << j1 << "," << j2 << ")" << endl;
ublas::noalias(this->info_(slot2, slot1)) += ublas::trans(update.info_(j1,j2));
// ublas::noalias(this->info_(slot2, slot1)) += ublas::trans(update.info_(j1,j2));
Eigen::Map<Eigen::MatrixXd>(&matrix_(0,0), matrix_.size1(), matrix_.size2()).block(
info_.offset(slot2), info_.offset(slot1), info_(slot2,slot1).size1(), info_(slot2,slot1).size2()) +=
Eigen::Map<Eigen::MatrixXd>(&update.matrix_(0,0), update.matrix_.size1(), update.matrix_.size2()).block(
update.info_.offset(j1), update.info_.offset(j2), update.info_(j1,j2).size1(), update.info_(j1,j2).size2()).transpose();
} else {
if(debug)
cout << "Updating (" << slot1 << "," << slot2 << ") from (" << j1 << "," << j2 << ")" << endl;
Block thisBlock(this->info_(slot1, slot2));
constBlock updateBlock(update.info_(j1,j2));
ublas::noalias(ublas::symmetric_adaptor<Block,ublas::upper>(thisBlock)) +=
ublas::symmetric_adaptor<constBlock,ublas::upper>(updateBlock);
// Block thisBlock(this->info_(slot1, slot2));
// constBlock updateBlock(update.info_(j1,j2));
// ublas::noalias(ublas::symmetric_adaptor<Block,ublas::upper>(thisBlock)) +=
// ublas::symmetric_adaptor<constBlock,ublas::upper>(updateBlock);
Eigen::Map<Eigen::MatrixXd>(&matrix_(0,0), matrix_.size1(), matrix_.size2()).block(
info_.offset(slot1), info_.offset(slot2), info_(slot1,slot2).size1(), info_(slot1,slot2).size2()).triangularView<Eigen::Upper>() +=
Eigen::Map<Eigen::MatrixXd>(&update.matrix_(0,0), update.matrix_.size1(), update.matrix_.size2()).block(
update.info_.offset(j1), update.info_.offset(j2), update.info_(j1,j2).size1(), update.info_(j1,j2).size2());
}
if(debug) cout << "Updating block " << slot1 << "," << slot2 << " from block " << j1 << "," << j2 << "\n";
if(debug) this->print();
}
}
toc(2, "update");

2
gtsam/linear/JacobianFactor.cpp
View File

@ -478,7 +478,7 @@ namespace gtsam {
firstNonzeroBlocks_.resize(this->size1());
for(size_t row=0; row<size1(); ++row) {
if(debug) cout << "row " << row << " varpos " << varpos << " Ab_.offset(varpos)=" << Ab_.offset(varpos) << " Ab_.offset(varpos+1)=" << Ab_.offset(varpos+1) << endl;
while(varpos < this->keys_.size() && Ab_.offset(varpos+1) <= row)
while(varpos < this->keys_.size() && Ab_.offset(varpos+1)-Ab_.offset(0) <= row)
++ varpos;
firstNonzeroBlocks_[row] = varpos;
if(debug) cout << "firstNonzeroVars_[" << row << "] = " << firstNonzeroBlocks_[row] << endl;

4
gtsam/linear/tests/testGaussianJunctionTree.cpp
View File

@ -24,6 +24,7 @@
#include <boost/assign/std/set.hpp> // for operator +=
using namespace boost::assign;
#include <gtsam/base/debug.h>
#include <gtsam/linear/GaussianJunctionTree.h>
#include <gtsam/inference/BayesTree-inl.h>
@ -66,6 +67,9 @@ GaussianFactorGraph createChain() {
*/
TEST( GaussianJunctionTree, eliminate )
{
SETDEBUG("updateATA",true);
GaussianFactorGraph fg = createChain();
GaussianJunctionTree junctionTree(fg);
BayesTree<GaussianConditional>::sharedClique rootClique = junctionTree.eliminate();

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

@ -16,6 +16,7 @@
* @created Dec 15, 2010
*/
#include <gtsam/base/debug.h>
#include <gtsam/linear/HessianFactor.h>
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/linear/GaussianFactorGraph.h>
@ -207,6 +208,7 @@ TEST(GaussianFactor, eliminate2 )
/* ************************************************************************* */
TEST_UNSAFE(GaussianFactor, eliminateUnsorted) {
JacobianFactor::shared_ptr factor1(
new JacobianFactor(0,
Matrix_(3,3,