From 3e93c488e539bfaadb04c68f835bd6022e06e620 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Mon, 27 Aug 2012 22:29:56 +0000 Subject: [PATCH] Removed extra code that was sometimes maintaining a staircase pattern in JacobianFactor by sorting rows - a holdover from the staircase optimization that is no longer done now that we are using Eigen's QR --- gtsam/inference/VariableSlots.h | 4 - gtsam/linear/GaussianFactorGraph.cpp | 61 +++++----- gtsam/linear/JacobianFactor.cpp | 111 ++---------------- gtsam/linear/JacobianFactor.h | 24 ---- .../linear/tests/testGaussianFactorGraph.cpp | 8 +- tests/testDoglegOptimizer.cpp | 2 +- 6 files changed, 45 insertions(+), 165 deletions(-) diff --git a/gtsam/inference/VariableSlots.h b/gtsam/inference/VariableSlots.h index 551993db4..2533a7411 100644 --- a/gtsam/inference/VariableSlots.h +++ b/gtsam/inference/VariableSlots.h @@ -48,10 +48,6 @@ namespace gtsam { * variable index 3), row-block 2 (also meaning component factor 2), comes from * column-block 0 of component factor 2. * - * Note that in the case of GaussianFactor, the rows of the combined factor are - * further sorted so that the column-block position of the first structural - * non-zero increases monotonically through the rows. This additional process - * is not performed by this class. * \nosubgrouping */ diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp index dd86ffca6..e2598b1ac 100644 --- a/gtsam/linear/GaussianFactorGraph.cpp +++ b/gtsam/linear/GaussianFactorGraph.cpp @@ -240,20 +240,6 @@ break; } toc(1, "countDims"); - if (debug) cout << "Sort rows" << endl; - tic(2, "sort rows"); - vector rowSources; - rowSources.reserve(m); - bool anyConstrained = false; - for (size_t sourceFactorI = 0; sourceFactorI < factors.size(); ++sourceFactorI) { - const JacobianFactor& sourceFactor(*factors[sourceFactorI]); - sourceFactor.collectInfo(sourceFactorI, rowSources); - if (sourceFactor.isConstrained()) anyConstrained = true; - } - assert(rowSources.size() == m); - std::sort(rowSources.begin(), rowSources.end()); - toc(2, "sort rows"); - if (debug) cout << "Allocate new factor" << endl; tic(3, "allocate"); JacobianFactor::shared_ptr combined(new JacobianFactor()); @@ -261,36 +247,47 @@ break; Vector sigmas(m); toc(3, "allocate"); - if (debug) cout << "Copy rows" << endl; - tic(4, "copy rows"); + if (debug) cout << "Copy blocks" << endl; + tic(4, "copy blocks"); + // Loop over slots in combined factor Index combinedSlot = 0; BOOST_FOREACH(const VariableSlots::value_type& varslot, variableSlots) { - for (size_t row = 0; row < m; ++row) { - const JacobianFactor::_RowSource& info(rowSources[row]); - const JacobianFactor& source(*factors[info.factorI]); - size_t sourceRow = info.factorRowI; - Index sourceSlot = varslot.second[info.factorI]; - combined->copyRow(source, sourceRow, sourceSlot, row, combinedSlot); + JacobianFactor::ABlock destSlot(combined->getA(combined->begin()+combinedSlot)); + // Loop over source factors + size_t nextRow = 0; + for(size_t factorI = 0; factorI < factors.size(); ++factorI) { + // Slot in source factor + const Index sourceSlot = varslot.second[factorI]; + const size_t sourceRows = factors[factorI]->rows(); + JacobianFactor::ABlock::RowsBlockXpr destBlock(destSlot.middleRows(nextRow, sourceRows)); + // Copy if exists in source factor, otherwise set zero + if(sourceSlot != numeric_limits::max()) + destBlock = factors[factorI]->getA(factors[factorI]->begin()+sourceSlot); + else + destBlock.setZero(); + nextRow += sourceRows; } ++combinedSlot; } - toc(4, "copy rows"); + toc(4, "copy blocks"); - if (debug) cout << "Copy rhs (b), sigma, and firstNonzeroBlocks" << endl; + if (debug) cout << "Copy rhs (b) and sigma" << endl; tic(5, "copy vectors"); - for (size_t row = 0; row < m; ++row) { - const JacobianFactor::_RowSource& info(rowSources[row]); - const JacobianFactor& source(*factors[info.factorI]); - const size_t sourceRow = info.factorRowI; - combined->getb()(row) = source.getb()(sourceRow); - sigmas(row) = source.get_model()->sigmas()(sourceRow); + bool anyConstrained = false; + // Loop over source factors + size_t nextRow = 0; + for(size_t factorI = 0; factorI < factors.size(); ++factorI) { + const size_t sourceRows = factors[factorI]->rows(); + combined->getb().segment(nextRow, sourceRows) = factors[factorI]->getb(); + sigmas.segment(nextRow, sourceRows) = factors[factorI]->get_model()->sigmas(); + if (factors[factorI]->isConstrained()) anyConstrained = true; + nextRow += sourceRows; } - combined->copyFNZ(m, variableSlots.size(),rowSources); toc(5, "copy vectors"); if (debug) cout << "Create noise model from sigmas" << endl; tic(6, "noise model"); - combined->setModel( anyConstrained,sigmas); + combined->setModel(anyConstrained, sigmas); toc(6, "noise model"); if (debug) cout << "Assert Invariants" << endl; diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index d0023b766..759be4948 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -48,15 +48,12 @@ namespace gtsam { #ifndef NDEBUG GaussianFactor::assertInvariants(); // The base class checks for unique keys assert((size() == 0 && Ab_.rows() == 0 && Ab_.nBlocks() == 0) || size()+1 == Ab_.nBlocks()); - assert(firstNonzeroBlocks_.size() == Ab_.rows()); - for(size_t i=0; idim() != (size_t) b.size()) throw InvalidNoiseModel(b.size(), model->dim()); @@ -103,7 +100,7 @@ namespace gtsam { /* ************************************************************************* */ JacobianFactor::JacobianFactor(Index i1, const Matrix& A1, Index i2, const Matrix& A2, const Vector& b, const SharedDiagonal& model) : - GaussianFactor(i1,i2), model_(model), firstNonzeroBlocks_(b.size(), 0), Ab_(matrix_) { + GaussianFactor(i1,i2), model_(model), Ab_(matrix_) { if(model->dim() != (size_t) b.size()) throw InvalidNoiseModel(b.size(), model->dim()); @@ -119,7 +116,7 @@ namespace gtsam { /* ************************************************************************* */ JacobianFactor::JacobianFactor(Index i1, const Matrix& A1, Index i2, const Matrix& A2, Index i3, const Matrix& A3, const Vector& b, const SharedDiagonal& model) : - GaussianFactor(i1,i2,i3), model_(model), firstNonzeroBlocks_(b.size(), 0), Ab_(matrix_) { + GaussianFactor(i1,i2,i3), model_(model), Ab_(matrix_) { if(model->dim() != (size_t) b.size()) throw InvalidNoiseModel(b.size(), model->dim()); @@ -137,7 +134,7 @@ namespace gtsam { JacobianFactor::JacobianFactor(const std::vector > &terms, const Vector &b, const SharedDiagonal& model) : GaussianFactor(GetKeys(terms.size(), terms.begin(), terms.end())), - model_(model), firstNonzeroBlocks_(b.size(), 0), Ab_(matrix_) + model_(model), Ab_(matrix_) { if(model->dim() != (size_t) b.size()) @@ -158,7 +155,7 @@ namespace gtsam { JacobianFactor::JacobianFactor(const std::list > &terms, const Vector &b, const SharedDiagonal& model) : GaussianFactor(GetKeys(terms.size(), terms.begin(), terms.end())), - model_(model), firstNonzeroBlocks_(b.size(), 0), Ab_(matrix_) + model_(model), Ab_(matrix_) { if(model->dim() != (size_t) b.size()) @@ -179,9 +176,11 @@ namespace gtsam { } /* ************************************************************************* */ - JacobianFactor::JacobianFactor(const GaussianConditional& cg) : GaussianFactor(cg), model_(noiseModel::Diagonal::Sigmas(cg.get_sigmas(), true)), Ab_(matrix_) { + JacobianFactor::JacobianFactor(const GaussianConditional& cg) : + GaussianFactor(cg), + model_(noiseModel::Diagonal::Sigmas(cg.get_sigmas(), true)), + Ab_(matrix_) { Ab_.assignNoalias(cg.rsd_); - firstNonzeroBlocks_.resize(cg.get_d().size(), 0); // set sigmas from precisions assertInvariants(); } @@ -206,8 +205,6 @@ namespace gtsam { Ab_.rowEnd() = maxrank; model_ = noiseModel::Unit::Create(maxrank); - firstNonzeroBlocks_.resize(this->rows(), 0); - assertInvariants(); } @@ -215,7 +212,6 @@ namespace gtsam { JacobianFactor& JacobianFactor::operator=(const JacobianFactor& rhs) { this->Base::operator=(rhs); // Copy keys model_ = rhs.model_; // Copy noise model - firstNonzeroBlocks_ = rhs.firstNonzeroBlocks_; // Copy staircase pattern Ab_.assignNoalias(rhs.Ab_); // Copy matrix and block structure assertInvariants(); return *this; @@ -417,19 +413,6 @@ namespace gtsam { assert(Ab_.rows() <= Ab_.cols()-1); toc(4, "remaining factor"); - // todo SL: deal with "dead" pivot columns!!! - tic(5, "rowstarts"); - size_t varpos = 0; - firstNonzeroBlocks_.resize(this->rows()); - for(size_t row=0; rowprint("Eliminating JacobianFactor: "); - - // NOTE: stairs are not currently used in the Eigen QR implementation - // add this back if DenseQR is ever reimplemented -// tic(1, "stairs"); -// // Translate the left-most nonzero column indices into top-most zero row indices -// vector firstZeroRows(Ab_.cols()); -// { -// size_t lastNonzeroRow = 0; -// vector::iterator firstZeroRowsIt = firstZeroRows.begin(); -// for(size_t var=0; varrows() && firstNonzeroBlocks_[lastNonzeroRow] <= var) -// ++ lastNonzeroRow; -// fill(firstZeroRowsIt, firstZeroRowsIt+Ab_(var).cols(), lastNonzeroRow); -// firstZeroRowsIt += Ab_(var).cols(); -// } -// assert(firstZeroRowsIt+1 == firstZeroRows.end()); -// *firstZeroRowsIt = this->rows(); -// } -// toc(1, "stairs"); - -// #ifndef NDEBUG -// for(size_t col=0; col& rowSources) const { - assertInvariants(); - for (size_t row = 0; row < rows(); ++row) { - Index firstNonzeroVar; - if (firstNonzeroBlocks_[row] < size()) { - firstNonzeroVar = keys_[firstNonzeroBlocks_[row]]; - } else if (firstNonzeroBlocks_[row] == size()) { - firstNonzeroVar = back() + 1; - } else { - assert(false); - } - rowSources.push_back(_RowSource(firstNonzeroVar, index, row)); - } - } - /* ************************************************************************* */ void JacobianFactor::allocate(const VariableSlots& variableSlots, vector< size_t>& varDims, size_t m) { @@ -527,34 +466,6 @@ namespace gtsam { boost::bind(&VariableSlots::const_iterator::value_type::first, _1)); varDims.push_back(1); Ab_.copyStructureFrom(BlockAb(matrix_, varDims.begin(), varDims.end(), m)); - firstNonzeroBlocks_.resize(m); - } - - /* ************************************************************************* */ - void JacobianFactor::copyRow(const JacobianFactor& source, Index sourceRow, - size_t sourceSlot, size_t row, Index slot) { - ABlock combinedBlock = Ab_(slot); - if (sourceSlot != numeric_limits::max()) { - if (source.firstNonzeroBlocks_[sourceRow] <= sourceSlot) { - const constABlock sourceBlock(source.Ab_(sourceSlot)); - combinedBlock.row(row).noalias() = sourceBlock.row(sourceRow); - } else { - combinedBlock.row(row).setZero(); - } - } else { - combinedBlock.row(row).setZero(); - } - } - - /* ************************************************************************* */ - void JacobianFactor::copyFNZ(size_t m, size_t n, - vector<_RowSource>& rowSources) { - Index i = 0; - for (size_t row = 0; row < m; ++row) { - while (i < n && rowSources[row].firstNonzeroVar > keys_[i]) - ++i; - firstNonzeroBlocks_[row] = i; - } } /* ************************************************************************* */ diff --git a/gtsam/linear/JacobianFactor.h b/gtsam/linear/JacobianFactor.h index 7cf1aa960..def36b086 100644 --- a/gtsam/linear/JacobianFactor.h +++ b/gtsam/linear/JacobianFactor.h @@ -83,7 +83,6 @@ namespace gtsam { typedef VerticalBlockView BlockAb; noiseModel::Diagonal::shared_ptr model_; // Gaussian noise model with diagonal covariance matrix - std::vector firstNonzeroBlocks_; AbMatrix matrix_; // the full matrix corresponding to the factor BlockAb Ab_; // the block view of the full matrix typedef GaussianFactor Base; // typedef to base @@ -274,35 +273,13 @@ namespace gtsam { */ boost::shared_ptr splitConditional(size_t nrFrontals = 1); - /* Used by ::CombineJacobians for sorting */ - struct _RowSource { - size_t firstNonzeroVar; - size_t factorI; - size_t factorRowI; - _RowSource(size_t _firstNonzeroVar, size_t _factorI, size_t _factorRowI) : - firstNonzeroVar(_firstNonzeroVar), factorI(_factorI), factorRowI(_factorRowI) {} - bool operator<(const _RowSource& o) const { - return firstNonzeroVar < o.firstNonzeroVar; - } - }; - // following methods all used in CombineJacobians: // Many imperative, perhaps all need to be combined in constructor - /** Collect information on Jacobian rows */ - void collectInfo(size_t index, std::vector<_RowSource>& rowSources) const; - /** allocate space */ void allocate(const VariableSlots& variableSlots, std::vector& varDims, size_t m); - /** copy a slot from source */ - void copyRow(const JacobianFactor& source, - Index sourceRow, size_t sourceSlot, size_t row, Index slot); - - /** copy firstNonzeroBlocks structure */ - void copyFNZ(size_t m, size_t n, std::vector<_RowSource>& rowSources); - /** set noiseModel correctly */ void setModel(bool anyConstrained, const Vector& sigmas); @@ -341,7 +318,6 @@ namespace gtsam { template void serialize(ARCHIVE & ar, const unsigned int version) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(GaussianFactor); - ar & BOOST_SERIALIZATION_NVP(firstNonzeroBlocks_); ar & BOOST_SERIALIZATION_NVP(Ab_); ar & BOOST_SERIALIZATION_NVP(model_); ar & BOOST_SERIALIZATION_NVP(matrix_); diff --git a/gtsam/linear/tests/testGaussianFactorGraph.cpp b/gtsam/linear/tests/testGaussianFactorGraph.cpp index 37aa440ff..4c0aa3871 100644 --- a/gtsam/linear/tests/testGaussianFactorGraph.cpp +++ b/gtsam/linear/tests/testGaussianFactorGraph.cpp @@ -163,10 +163,10 @@ TEST(GaussianFactorGraph, Combine2) JacobianFactor actual = *CombineJacobians(jacobians, VariableSlots(gfg)); Matrix zero3x3 = zeros(3,3); - Matrix A0 = gtsam::stack(3, &A10, &zero3x3, &zero3x3); - Matrix A1 = gtsam::stack(3, &A11, &A01, &A21); - Vector b = gtsam::concatVectors(3, &b1, &b0, &b2); - Vector sigmas = gtsam::concatVectors(3, &s1, &s0, &s2); + Matrix A0 = gtsam::stack(3, &zero3x3, &A10, &zero3x3); + Matrix A1 = gtsam::stack(3, &A01, &A11, &A21); + Vector b = gtsam::concatVectors(3, &b0, &b1, &b2); + Vector sigmas = gtsam::concatVectors(3, &s0, &s1, &s2); JacobianFactor expected(0, A0, 1, A1, b, noiseModel::Diagonal::Sigmas(sigmas, true)); diff --git a/tests/testDoglegOptimizer.cpp b/tests/testDoglegOptimizer.cpp index 3c5409af7..781685a23 100644 --- a/tests/testDoglegOptimizer.cpp +++ b/tests/testDoglegOptimizer.cpp @@ -200,7 +200,7 @@ TEST(DoglegOptimizer, BT_BN_equivalency) { GaussianFactorGraph expected(gbn); GaussianFactorGraph actual(bt); - EXPECT(assert_equal(expected.denseJacobian(), actual.denseJacobian())); + EXPECT(assert_equal(expected.denseHessian(), actual.denseHessian())); } /* ************************************************************************* */