diff --git a/base/blockMatrices.h b/base/blockMatrices.h index f62c493e1..5823b1365 100644 --- a/base/blockMatrices.h +++ b/base/blockMatrices.h @@ -86,14 +86,14 @@ public: template class VerticalBlockView { public: - typedef MATRIX matrix_type; - typedef typename boost::numeric::ublas::matrix_range block_type; - typedef typename boost::numeric::ublas::matrix_range const_block_type; - typedef BlockColumn column_type; - typedef BlockColumn const_column_type; + typedef MATRIX FullMatrix; + typedef typename boost::numeric::ublas::matrix_range Block; + typedef typename boost::numeric::ublas::matrix_range constBlock; + typedef BlockColumn Column; + typedef BlockColumn constColumn; protected: - matrix_type& matrix_; + FullMatrix& matrix_; std::vector variableColOffsets_; size_t rowStart_; size_t rowEnd_; @@ -101,18 +101,18 @@ protected: public: /** Construct from an empty matrix (asserts that the matrix is empty) */ - VerticalBlockView(matrix_type& matrix); + VerticalBlockView(FullMatrix& matrix); /** Construct from iterators over the sizes of each vertical block */ template - VerticalBlockView(matrix_type& matrix, ITERATOR firstBlockDim, ITERATOR lastBlockDim); + VerticalBlockView(FullMatrix& matrix, ITERATOR firstBlockDim, ITERATOR lastBlockDim); /** Construct from a vector of the sizes of each vertical block, resize the * matrix so that its height is matrixNewHeight and its width fits the given * block dimensions. */ template - VerticalBlockView(matrix_type& matrix, ITERATOR firstBlockDim, ITERATOR lastBlockDim, size_t matrixNewHeight); + VerticalBlockView(FullMatrix& matrix, ITERATOR firstBlockDim, ITERATOR lastBlockDim, size_t matrixNewHeight); /** Row size */ @@ -128,52 +128,52 @@ public: size_t nBlocks() const { assertInvariants(); return variableColOffsets_.size() - 1 - blockStart_; } - block_type operator()(size_t block) { + Block operator()(size_t block) { return range(block, block+1); } - const_block_type operator()(size_t block) const { + constBlock operator()(size_t block) const { return range(block, block+1); } - block_type range(size_t startBlock, size_t endBlock) { + Block range(size_t startBlock, size_t endBlock) { assertInvariants(); size_t actualStartBlock = startBlock + blockStart_; size_t actualEndBlock = endBlock + blockStart_; checkBlock(actualStartBlock); assert(actualEndBlock < variableColOffsets_.size()); - return block_type(matrix_, + return Block(matrix_, boost::numeric::ublas::range(rowStart_, rowEnd_), boost::numeric::ublas::range(variableColOffsets_[actualStartBlock], variableColOffsets_[actualEndBlock])); } - const_block_type range(size_t startBlock, size_t endBlock) const { + constBlock range(size_t startBlock, size_t endBlock) const { assertInvariants(); size_t actualStartBlock = startBlock + blockStart_; size_t actualEndBlock = endBlock + blockStart_; checkBlock(actualStartBlock); assert(actualEndBlock < variableColOffsets_.size()); - return const_block_type(matrix_, + return constBlock(matrix_, boost::numeric::ublas::range(rowStart_, rowEnd_), boost::numeric::ublas::range(variableColOffsets_[actualStartBlock], variableColOffsets_[actualEndBlock])); } - column_type column(size_t block, size_t columnOffset) { + Column column(size_t block, size_t columnOffset) { assertInvariants(); size_t actualBlock = block + blockStart_; checkBlock(actualBlock); assert(variableColOffsets_[actualBlock] + columnOffset < matrix_.size2()); - block_type blockMat(operator()(block)); - return column_type(blockMat, columnOffset); + Block blockMat(operator()(block)); + return Column(blockMat, columnOffset); } - const_column_type column(size_t block, size_t columnOffset) const { + constColumn column(size_t block, size_t columnOffset) const { assertInvariants(); size_t actualBlock = block + blockStart_; checkBlock(actualBlock); assert(variableColOffsets_[actualBlock] + columnOffset < matrix_.size2()); - const_block_type blockMat(operator()(block)); - return const_column_type(blockMat, columnOffset); + constBlock blockMat(operator()(block)); + return constColumn(blockMat, columnOffset); } size_t offset(size_t block) const { @@ -242,7 +242,7 @@ protected: /* ************************************************************************* */ template -VerticalBlockView::VerticalBlockView(matrix_type& matrix) : +VerticalBlockView::VerticalBlockView(FullMatrix& matrix) : matrix_(matrix), rowStart_(0), rowEnd_(matrix_.size1()), blockStart_(0) { fillOffsets((size_t*)0, (size_t*)0); assertInvariants(); @@ -251,7 +251,7 @@ matrix_(matrix), rowStart_(0), rowEnd_(matrix_.size1()), blockStart_(0) { /* ************************************************************************* */ template template -VerticalBlockView::VerticalBlockView(matrix_type& matrix, ITERATOR firstBlockDim, ITERATOR lastBlockDim) : +VerticalBlockView::VerticalBlockView(FullMatrix& matrix, ITERATOR firstBlockDim, ITERATOR lastBlockDim) : matrix_(matrix), rowStart_(0), rowEnd_(matrix_.size1()), blockStart_(0) { fillOffsets(firstBlockDim, lastBlockDim); assertInvariants(); @@ -260,7 +260,7 @@ matrix_(matrix), rowStart_(0), rowEnd_(matrix_.size1()), blockStart_(0) { /* ************************************************************************* */ template template -VerticalBlockView::VerticalBlockView(matrix_type& matrix, ITERATOR firstBlockDim, ITERATOR lastBlockDim, size_t matrixNewHeight) : +VerticalBlockView::VerticalBlockView(FullMatrix& matrix, ITERATOR firstBlockDim, ITERATOR lastBlockDim, size_t matrixNewHeight) : matrix_(matrix), rowStart_(0), rowEnd_(matrixNewHeight), blockStart_(0) { fillOffsets(firstBlockDim, lastBlockDim); matrix_.resize(matrixNewHeight, variableColOffsets_.back(), false); diff --git a/inference/JunctionTree-inl.h b/inference/JunctionTree-inl.h index 77ddb7f16..76f760509 100644 --- a/inference/JunctionTree-inl.h +++ b/inference/JunctionTree-inl.h @@ -176,7 +176,7 @@ namespace gtsam { typename FG::sharedFactor jointFactor = FG::Factor::Combine(fg, variableSlots); toc("JT 2.2 Combine"); tic("JT 2.3 Eliminate"); - typename FG::bayesnet_type::shared_ptr fragment = jointFactor->eliminate(current->frontal.size()); + typename BayesNet::shared_ptr fragment = jointFactor->eliminate(current->frontal.size()); toc("JT 2.3 Eliminate"); assert(std::equal(jointFactor->begin(), jointFactor->end(), current->separator.begin())); diff --git a/inference/SymbolicFactorGraph.h b/inference/SymbolicFactorGraph.h index 5cab6885c..308b9515b 100644 --- a/inference/SymbolicFactorGraph.h +++ b/inference/SymbolicFactorGraph.h @@ -34,8 +34,8 @@ typedef EliminationTree SymbolicEliminationTree; /** Symbolic IndexFactor Graph */ class SymbolicFactorGraph: public FactorGraph { + public: - typedef SymbolicBayesNet bayesnet_type; /** Construct empty factor graph */ SymbolicFactorGraph() {} diff --git a/linear/GaussianConditional.h b/linear/GaussianConditional.h index a0daf2d0d..454c56933 100644 --- a/linear/GaussianConditional.h +++ b/linear/GaussianConditional.h @@ -46,19 +46,19 @@ public: typedef boost::shared_ptr shared_ptr; /** Store the conditional matrix as upper-triangular column-major */ - typedef boost::numeric::ublas::triangular_matrix matrix_type; - typedef VerticalBlockView rsd_type; + typedef boost::numeric::ublas::triangular_matrix AbMatrix; + typedef VerticalBlockView rsd_type; - typedef rsd_type::block_type r_type; - typedef rsd_type::const_block_type const_r_type; - typedef rsd_type::column_type d_type; - typedef rsd_type::const_column_type const_d_type; + typedef rsd_type::Block r_type; + typedef rsd_type::constBlock const_r_type; + typedef rsd_type::Column d_type; + typedef rsd_type::constColumn const_d_type; protected: /** Store the conditional as one big upper-triangular wide matrix, arranged * as [ R S1 S2 ... d ]. Access these blocks using a VerticalBlockView. */ - matrix_type matrix_; + AbMatrix matrix_; rsd_type rsd_; /** vector of standard deviations */ @@ -113,9 +113,9 @@ public: size_t dim() const { return rsd_.size1(); } /** return stuff contained in GaussianConditional */ - rsd_type::const_column_type get_d() const { return rsd_.column(1+nrParents(), 0); } - rsd_type::const_block_type get_R() const { return rsd_(0); } - rsd_type::const_block_type get_S(const_iterator variable) const { return rsd_(variable - this->begin()); } + rsd_type::constColumn get_d() const { return rsd_.column(1+nrParents(), 0); } + rsd_type::constBlock get_R() const { return rsd_(0); } + rsd_type::constBlock get_S(const_iterator variable) const { return rsd_(variable - this->begin()); } const Vector& get_sigmas() const {return sigmas_;} /** @@ -133,9 +133,9 @@ public: Vector solve(const Permuted& x) const; protected: - rsd_type::column_type get_d_() { return rsd_.column(1+nrParents(), 0); } - rsd_type::block_type get_R_() { return rsd_(0); } - rsd_type::block_type get_S_(iterator variable) { return rsd_(variable - this->begin()); } + rsd_type::Column get_d_() { return rsd_.column(1+nrParents(), 0); } + rsd_type::Block get_R_() { return rsd_(0); } + rsd_type::Block get_S_(iterator variable) { return rsd_(variable - this->begin()); } friend class GaussianFactor; diff --git a/linear/GaussianFactor.cpp b/linear/GaussianFactor.cpp index 4e3cc4f90..2e3855c2e 100644 --- a/linear/GaussianFactor.cpp +++ b/linear/GaussianFactor.cpp @@ -63,7 +63,7 @@ GaussianFactor::GaussianFactor() : Ab_(matrix_) { assertInvariants(); } /* ************************************************************************* */ GaussianFactor::GaussianFactor(const Vector& b_in) : firstNonzeroBlocks_(b_in.size(), 0), Ab_(matrix_) { size_t dims[] = { 1 }; - Ab_.copyStructureFrom(ab_type(matrix_, dims, dims+1, b_in.size())); + Ab_.copyStructureFrom(BlockAb(matrix_, dims, dims+1, b_in.size())); getb() = b_in; assertInvariants(); } @@ -73,7 +73,7 @@ GaussianFactor::GaussianFactor(Index i1, const Matrix& A1, const Vector& b, const SharedDiagonal& model) : IndexFactor(i1), model_(model), firstNonzeroBlocks_(b.size(), 0), Ab_(matrix_) { size_t dims[] = { A1.size2(), 1}; - Ab_.copyStructureFrom(ab_type(matrix_, dims, dims+2, b.size())); + Ab_.copyStructureFrom(BlockAb(matrix_, dims, dims+2, b.size())); Ab_(0) = A1; getb() = b; assertInvariants(); @@ -84,7 +84,7 @@ GaussianFactor::GaussianFactor(Index i1, const Matrix& A1, Index i2, const Matri const Vector& b, const SharedDiagonal& model) : IndexFactor(i1,i2), model_(model), firstNonzeroBlocks_(b.size(), 0), Ab_(matrix_) { size_t dims[] = { A1.size2(), A2.size2(), 1}; - Ab_.copyStructureFrom(ab_type(matrix_, dims, dims+3, b.size())); + Ab_.copyStructureFrom(BlockAb(matrix_, dims, dims+3, b.size())); Ab_(0) = A1; Ab_(1) = A2; getb() = b; @@ -96,7 +96,7 @@ GaussianFactor::GaussianFactor(Index i1, const Matrix& A1, Index i2, const Matri Index i3, const Matrix& A3, const Vector& b, const SharedDiagonal& model) : IndexFactor(i1,i2,i3), model_(model), firstNonzeroBlocks_(b.size(), 0), Ab_(matrix_) { size_t dims[] = { A1.size2(), A2.size2(), A3.size2(), 1}; - Ab_.copyStructureFrom(ab_type(matrix_, dims, dims+4, b.size())); + Ab_.copyStructureFrom(BlockAb(matrix_, dims, dims+4, b.size())); Ab_(0) = A1; Ab_(1) = A2; Ab_(2) = A3; @@ -115,7 +115,7 @@ GaussianFactor::GaussianFactor(const std::vector > &ter dims[j] = terms[j].second.size2(); } dims[terms.size()] = 1; - Ab_.copyStructureFrom(ab_type(matrix_, dims, dims+terms.size()+1, b.size())); + Ab_.copyStructureFrom(BlockAb(matrix_, dims, dims+terms.size()+1, b.size())); for(size_t j=0; j > &terms } dims[j] = 1; firstNonzeroBlocks_.resize(b.size(), 0); - Ab_.copyStructureFrom(ab_type(matrix_, dims, dims+terms.size()+1, b.size())); + Ab_.copyStructureFrom(BlockAb(matrix_, dims, dims+terms.size()+1, b.size())); j = 0; for(std::list >::const_iterator term=terms.begin(); term!=terms.end(); ++term) { Ab_(j) = term->second; @@ -178,8 +178,8 @@ bool GaussianFactor::equals(const GaussianFactor& f, double tol) const { assert(Ab_.size1() == f.Ab_.size1() && Ab_.size2() == f.Ab_.size2()); - ab_type::const_block_type Ab1(Ab_.range(0, Ab_.nBlocks())); - ab_type::const_block_type Ab2(f.Ab_.range(0, f.Ab_.nBlocks())); + constABlock Ab1(Ab_.range(0, Ab_.nBlocks())); + constABlock Ab2(f.Ab_.range(0, f.Ab_.nBlocks())); for(size_t row=0; row oldKeys(keys_.size()); keys_.swap(oldKeys); - matrix_type oldMatrix; - ab_type oldAb(oldMatrix, dimensions.begin(), dimensions.end(), Ab_.size1()); + AbMatrix oldMatrix; + BlockAb oldAb(oldMatrix, dimensions.begin(), dimensions.end(), Ab_.size1()); Ab_.swap(oldAb); j = 0; BOOST_FOREACH(const SourceSlots::value_type& sourceSlot, sourceSlots) { @@ -295,7 +295,7 @@ GaussianFactor::sparse(const Dimensions& columnIndices) const { // iterate over all matrices in the factor for(size_t pos=0; pos&, const GaussianVariableIndex&, const vector&, const vector&, const std::vector >&); -//template GaussianFactor::shared_ptr GaussianFactor::Combine(const FactorGraph&, const GaussianVariableIndex&, const vector&, const vector&, const std::vector >&); -// -//// Utility function to determine row count and check if any noise models are constrained -//// TODO: would be nicer if this function was split and are factorgraph methods -//static std::pair rowCount(const FactorGraph& factorGraph, -// const vector& factorIndices) -//{ -// static const bool debug = false; -// tic("Combine: count sizes"); -// size_t m = 0; -// bool constrained = false; -// BOOST_FOREACH(const size_t i, factorIndices) -// { -// assert(factorGraph[i] != NULL); -// assert(!factorGraph[i]->keys().empty()); -// m += factorGraph[i]->numberOfRows(); -// if (debug) cout << "Combining factor " << i << endl; -// if (debug) factorGraph[i]->print(" :"); -// if (!constrained && factorGraph[i]->isConstrained()) { -// constrained = true; -// if (debug) std::cout << "Found a constraint!" << std::endl; -// } -// } -// toc("Combine: count sizes"); -// return make_pair(m,constrained); -//} -// -//// Determine row and column counts and check if any noise models are constrained -//template -//static vector columnDimensions( -// const GaussianVariableIndex& variableIndex, -// const vector& variables) -//{ -// tic("Combine: count dims"); -// static const bool debug = false; -// vector dims(variables.size() + 1); -// size_t n = 0; -// { -// size_t j = 0; -// BOOST_FOREACH(const Index& var, variables) -// { -// if (debug) cout << "Have variable " << var << endl; -// dims[j] = variableIndex.dim(var); -// n += dims[j]; -// ++j; -// } -// dims[j] = 1; -// } -// toc("Combine: count dims"); -// return dims; -//} -// -//// To do this, we merge-sort the rows so that the column indices of the first structural -//// non-zero in each row increase monotonically. -//vector<_RowSource> computeRowPermutation(size_t m, const vector& factorIndices, -// const FactorGraph& factorGraph) { -// vector<_RowSource> rowSources; -// rowSources.reserve(m); -// size_t i1 = 0; -// BOOST_FOREACH(const size_t i2, factorIndices) { -// const GaussianFactor& factor(*factorGraph[i2]); -// size_t factorRowI = 0; -// assert(factor.get_firstNonzeroBlocks().size() == factor.numberOfRows()); -// BOOST_FOREACH(const size_t factorFirstNonzeroVarpos, factor.get_firstNonzeroBlocks()) { -// Index firstNonzeroVar; -// firstNonzeroVar = factor.keys()[factorFirstNonzeroVarpos]; -// rowSources.push_back(_RowSource(firstNonzeroVar, i1, factorRowI)); -// ++ factorRowI; -// } -// assert(factorRowI == factor.numberOfRows()); -// ++ i1; -// } -// assert(rowSources.size() == m); -// assert(i1 == factorIndices.size()); -// sort(rowSources.begin(), rowSources.end()); -// return rowSources; -//} -// -//void copyMatrices(boost::shared_ptr combinedFactor, size_t row, -// const GaussianFactor& factor, const std::vector >& variablePositions, -// const size_t factorRow, const size_t factorI, const vector& variables) { -// const static bool debug = false; -// const size_t factorFirstNonzeroVarpos = factor.get_firstNonzeroBlocks()[factorRow]; -// std::vector::const_iterator keyit = factor.keys().begin() + factorFirstNonzeroVarpos; -// std::vector::const_iterator varposIt = variablePositions[factorI].begin() + factorFirstNonzeroVarpos; -// combinedFactor->set_firstNonzeroBlocks(row, *varposIt); -// if(debug) cout << " copying starting at varpos " << *varposIt << " (variable " << variables[*varposIt] << ")" << endl; -// std::vector::const_iterator keyitend = factor.keys().end(); -// while(keyit != keyitend) { -// const size_t varpos = *varposIt; -// assert(variables[varpos] == *keyit); -// GaussianFactor::ab_type::block_type retBlock(combinedFactor->getAb(varpos)); -// const GaussianFactor::ab_type::const_block_type factorBlock(factor.getA(keyit)); -// ublas::noalias(ublas::row(retBlock, row)) = ublas::row(factorBlock, factorRow); -// ++ keyit; -// ++ varposIt; -// } -//} -// -//template -//GaussianFactor::shared_ptr GaussianFactor::Combine(const FactorGraph& factorGraph, -// const GaussianVariableIndex& variableIndex, const vector& factorIndices, -// const vector& variables, const std::vector >& variablePositions) { -// -// // Debugging flags -// static const bool verbose = false; -// static const bool debug = false; -// if (verbose) std::cout << "GaussianFactor::GaussianFactor (factors)" << std::endl; -// assert(factorIndices.size() == variablePositions.size()); -// -// // Determine row count and check if any noise models are constrained -// size_t m; bool constrained; -// boost::tie(m,constrained) = rowCount(factorGraph,factorIndices); -// -// // Determine column dimensions -// vector dims = columnDimensions(variableIndex,variables); -// -// // Allocate return value, the combined factor, the augmented Ab matrix and other arrays -// tic("Combine: set up empty"); -// shared_ptr combinedFactor(boost::make_shared()); -// combinedFactor->Ab_.copyStructureFrom(ab_type(combinedFactor->matrix_, dims.begin(), dims.end(), m)); -// ublas::noalias(combinedFactor->matrix_) = ublas::zero_matrix(combinedFactor->matrix_.size1(), combinedFactor->matrix_.size2()); -// combinedFactor->firstNonzeroBlocks_.resize(m); -// Vector sigmas(m); -// -// // Copy keys -// combinedFactor->keys_.reserve(variables.size()); -// combinedFactor->keys_.insert(combinedFactor->keys_.end(), variables.begin(), variables.end()); -// toc("Combine: set up empty"); -// -// // Compute a row permutation that maintains a staircase pattern in the new combined factor. -// tic("Combine: sort rows"); -// vector<_RowSource> rowSources = computeRowPermutation(m, factorIndices, factorGraph); -// toc("Combine: sort rows"); -// -// // Fill in the rows of the new factor in sorted order. Fill in the array of -// // the left-most nonzero for each row and the first structural zero in each column. -// // todo SL: smarter ignoring of zero factor variables (store first possible like above) -// -// if(debug) gtsam::print(combinedFactor->matrix_, "matrix_ before copying rows: "); -// -// tic("Combine: copy rows"); -//#ifndef NDEBUG -// size_t lastRowFirstVarpos = 0; -//#endif -// for(size_t row=0; rowgetb()(row) = factor.getb()(factorRow); -// sigmas(row) = factor.get_sigmas()(factorRow); -// -// // Copy the row of A variable by variable, starting at the first nonzero variable. -// copyMatrices(combinedFactor, row, factor, variablePositions, factorRow, factorI, variables); -// -//#ifndef NDEBUG -// // Debug check, make sure the first column of nonzeros increases monotonically -// if(row != 0) -// assert(combinedFactor->firstNonzeroBlocks_[row] >= lastRowFirstVarpos); -// lastRowFirstVarpos = combinedFactor->firstNonzeroBlocks_[row]; -//#endif -// } -// toc("Combine: copy rows"); -// -// if (verbose) std::cout << "GaussianFactor::GaussianFactor done" << std::endl; -// -// if (constrained) { -// combinedFactor->model_ = noiseModel::Constrained::MixedSigmas(sigmas); -// if (verbose) combinedFactor->model_->print("Just created Constraint ^"); -// } else { -// combinedFactor->model_ = noiseModel::Diagonal::Sigmas(sigmas); -// if (verbose) combinedFactor->model_->print("Just created Diagonal"); -// } -// -// if(debug) combinedFactor->print("Combined factor: "); -// -// combinedFactor->assertInvariants(); -// -// return combinedFactor; -//} - /* ************************************************************************* */ // Helper functions for Combine -boost::tuple, size_t, size_t> countDims(const std::vector& factors, const VariableSlots& variableSlots) { +static boost::tuple, size_t, size_t> countDims(const std::vector& factors, const VariableSlots& variableSlots) { #ifndef NDEBUG vector varDims(variableSlots.size(), numeric_limits::max()); #else @@ -791,6 +600,7 @@ boost::tuple, size_t, size_t> countDims(const std::vector& factors, const VariableSlots& variableSlots) { static const bool verbose = false; @@ -835,7 +645,7 @@ GaussianFactor::shared_ptr GaussianFactor::Combine(const FactorGraphkeys_.resize(variableSlots.size()); std::transform(variableSlots.begin(), variableSlots.end(), combined->keys_.begin(), bind(&VariableSlots::const_iterator::value_type::first, boost::lambda::_1)); varDims.push_back(1); - combined->Ab_.copyStructureFrom(ab_type(combined->matrix_, varDims.begin(), varDims.end(), m)); + combined->Ab_.copyStructureFrom(BlockAb(combined->matrix_, varDims.begin(), varDims.end(), m)); combined->firstNonzeroBlocks_.resize(m); Vector sigmas(m); toc("Combine 3: allocate"); @@ -846,12 +656,12 @@ GaussianFactor::shared_ptr GaussianFactor::Combine(const FactorGraphAb_(combinedSlot)); + ABlock combinedBlock(combined->Ab_(combinedSlot)); if(sourceSlot != numeric_limits::max()) { const GaussianFactor& source(*factors[rowSources[row].factorI]); const size_t sourceRow = rowSources[row].factorRowI; if(source.firstNonzeroBlocks_[sourceRow] <= sourceSlot) { - const ab_type::const_block_type sourceBlock(source.Ab_(sourceSlot)); + const constABlock sourceBlock(source.Ab_(sourceSlot)); ublas::noalias(ublas::row(combinedBlock, row)) = ublas::row(sourceBlock, sourceRow); } else ublas::noalias(ublas::row(combinedBlock, row)) = ublas::zero_vector(combinedBlock.size2()); diff --git a/linear/GaussianFactor.h b/linear/GaussianFactor.h index 55cacacac..fef2d4e94 100644 --- a/linear/GaussianFactor.h +++ b/linear/GaussianFactor.h @@ -59,19 +59,24 @@ typedef std::map Dimensions; */ class GaussianFactor: public IndexFactor { +protected: + + typedef boost::numeric::ublas::matrix AbMatrix; + typedef VerticalBlockView BlockAb; + public: typedef GaussianConditional Conditional; typedef boost::shared_ptr shared_ptr; - typedef boost::numeric::ublas::matrix matrix_type; - typedef VerticalBlockView ab_type; - -protected: + typedef BlockAb::Block ABlock; + typedef BlockAb::constBlock constABlock; + typedef BlockAb::Column BVector; + typedef BlockAb::constColumn constBVector; SharedDiagonal model_; // Gaussian noise model with diagonal covariance matrix std::vector firstNonzeroBlocks_; - matrix_type matrix_; - ab_type Ab_; + AbMatrix matrix_; + BlockAb Ab_; public: @@ -124,14 +129,15 @@ public: bool empty() const { return Ab_.size1() == 0;} /** Get a view of the r.h.s. vector b */ - ab_type::const_column_type getb() const { return Ab_.column(size(), 0); } - ab_type::column_type getb() { return Ab_.column(size(), 0); } + constBVector getb() const { return Ab_.column(size(), 0); } /** Get a view of the A matrix for the variable pointed to be the given key iterator */ - ab_type::const_block_type getA(const_iterator variable) const { return Ab_(variable - keys_.begin()); } - ab_type::block_type getA(iterator variable) { return Ab_(variable - keys_.begin()); } - ab_type::block_type getAb(size_t block) { return Ab_(block); } + constABlock getA(const_iterator variable) const { return Ab_(variable - keys_.begin()); } + BVector getb() { return Ab_.column(size(), 0); } + + ABlock getA(iterator variable) { return Ab_(variable - keys_.begin()); } + ABlock getAb(size_t block) { return Ab_(block); } /** Return the dimension of the variable pointed to by the given key iterator * todo: Remove this in favor of keeping track of dimensions with variables? @@ -160,8 +166,6 @@ public: protected: - /** Protected mutable accessor for the r.h.s. b. */ - /** Internal debug check to make sure variables are sorted */ void assertInvariants() const; @@ -176,6 +180,7 @@ public: /** get the indices list */ const std::vector& get_firstNonzeroBlocks() const { return firstNonzeroBlocks_; } + /** whether the noise model of this factor is constrained (i.e. contains any sigmas of 0.0) */ bool isConstrained() const {return model_->isConstrained();} /** diff --git a/linear/GaussianFactorGraph.h b/linear/GaussianFactorGraph.h index 0112c907a..2800d1c67 100644 --- a/linear/GaussianFactorGraph.h +++ b/linear/GaussianFactorGraph.h @@ -38,7 +38,6 @@ namespace gtsam { public: typedef boost::shared_ptr shared_ptr; - typedef GaussianBayesNet bayesnet_type; /** * Default constructor diff --git a/linear/tests/testGaussianFactor.cpp b/linear/tests/testGaussianFactor.cpp index b34494c0c..e4d35b8ee 100644 --- a/linear/tests/testGaussianFactor.cpp +++ b/linear/tests/testGaussianFactor.cpp @@ -68,7 +68,7 @@ TEST( GaussianFactor, constructor2) std::list > terms; terms.push_back(make_pair(_x0_, eye(3))); terms.push_back(make_pair(_x1_, 2.*eye(3))); - GaussianFactor actual(terms, b, noise); + const GaussianFactor actual(terms, b, noise); GaussianFactor::const_iterator key0 = actual.begin(); GaussianFactor::const_iterator key1 = key0 + 1;