diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index b241ff24a..0fa3bd7f5 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -396,6 +396,73 @@ namespace gtsam { return this->eliminate(1); } + /* ************************************************************************* */ + GaussianConditional::shared_ptr JacobianFactor::splitConditional(size_t nrFrontals) { + assert(Ab_.rowStart() == 0 && Ab_.rowEnd() == (size_t) matrix_.rows() && Ab_.firstBlock() == 0); + assert(size() >= nrFrontals); + assertInvariants(); + + const bool debug = ISDEBUG("JacobianFactor::splitConditional"); + + if(debug) cout << "Eliminating " << nrFrontals << " frontal variables" << endl; + if(debug) this->print("Splitting JacobianFactor: "); + + size_t frontalDim = Ab_.range(0,nrFrontals).cols(); + + // Check for singular factor + if(model_->dim() < frontalDim) { + throw domain_error((boost::format( + "JacobianFactor is singular in variable %1%, discovered while attempting\n" + "to eliminate this variable.") % front()).str()); + } + + // Extract conditional + tic(3, "cond Rd"); + + // Restrict the matrix to be in the first nrFrontals variables + Ab_.rowEnd() = Ab_.rowStart() + frontalDim; + const Eigen::VectorBlock sigmas = model_->sigmas().segment(Ab_.rowStart(), Ab_.rowEnd()-Ab_.rowStart()); + GaussianConditional::shared_ptr conditional(new GaussianConditional(begin(), end(), nrFrontals, Ab_, sigmas)); + if(debug) conditional->print("Extracted conditional: "); + Ab_.rowStart() += frontalDim; + Ab_.firstBlock() += nrFrontals; + toc(3, "cond Rd"); + + if(debug) conditional->print("Extracted conditional: "); + + tic(4, "remaining factor"); + // Take lower-right block of Ab to get the new factor + Ab_.rowEnd() = model_->dim(); + keys_.erase(begin(), begin() + nrFrontals); + // Set sigmas with the right model + if (model_->isConstrained()) + model_ = noiseModel::Constrained::MixedSigmas(sub(model_->sigmas(), frontalDim, model_->dim())); + else + model_ = noiseModel::Diagonal::Sigmas(sub(model_->sigmas(), frontalDim, model_->dim())); + if(debug) this->print("Eliminated factor: "); + 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("QR result noise model: "); - // Check for singular factor - if(noiseModel->dim() < frontalDim) { - throw domain_error((boost::format( - "JacobianFactor is singular in variable %1%, discovered while attempting\n" - "to eliminate this variable.") % front()).str()); - } - - // Extract conditionals - tic(3, "cond Rd"); - GaussianConditional::shared_ptr conditionals(new GaussianConditional()); - - // Restrict the matrix to be in the first nrFrontals variables - Ab_.rowEnd() = Ab_.rowStart() + frontalDim; - const Eigen::VectorBlock sigmas = noiseModel->sigmas().segment(Ab_.rowStart(), Ab_.rowEnd()-Ab_.rowStart()); - conditionals = boost::make_shared(begin(), end(), nrFrontals, Ab_, sigmas); - if(debug) conditionals->print("Extracted conditional: "); - Ab_.rowStart() += frontalDim; - Ab_.firstBlock() += nrFrontals; - toc(3, "cond Rd"); - - if(debug) conditionals->print("Extracted conditionals: "); - - tic(4, "remaining factor"); - // Take lower-right block of Ab to get the new factor - Ab_.rowEnd() = noiseModel->dim(); - keys_.erase(begin(), begin() + nrFrontals); - // Set sigmas with the right model - if (noiseModel->isConstrained()) - model_ = noiseModel::Constrained::MixedSigmas(sub(noiseModel->sigmas(), frontalDim, noiseModel->dim())); - else - model_ = noiseModel::Diagonal::Sigmas(sub(noiseModel->sigmas(), frontalDim, noiseModel->dim())); - if(debug) this->print("Eliminated factor: "); - 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; rowdim() < frontalDim) { +// throw domain_error((boost::format( +// "JacobianFactor is singular in variable %1%, discovered while attempting\n" +// "to eliminate this variable.") % front()).str()); +// } +// +// // Extract conditional +// tic(3, "cond Rd"); +// +// // Restrict the matrix to be in the first nrFrontals variables +// Ab_.rowEnd() = Ab_.rowStart() + frontalDim; +// const Eigen::VectorBlock sigmas = noiseModel->sigmas().segment(Ab_.rowStart(), Ab_.rowEnd()-Ab_.rowStart()); +// GaussianConditional::shared_ptr conditional(new GaussianConditional(begin(), end(), nrFrontals, Ab_, sigmas)); +// if(debug) conditional->print("Extracted conditional: "); +// Ab_.rowStart() += frontalDim; +// Ab_.firstBlock() += nrFrontals; +// toc(3, "cond Rd"); +// +// if(debug) conditional->print("Extracted conditionals: "); +// +// tic(4, "remaining factor"); +// // Take lower-right block of Ab to get the new factor +// Ab_.rowEnd() = noiseModel->dim(); +// keys_.erase(begin(), begin() + nrFrontals); +// // Set sigmas with the right model +// if (noiseModel->isConstrained()) +// model_ = noiseModel::Constrained::MixedSigmas(sub(noiseModel->sigmas(), frontalDim, noiseModel->dim())); +// else +// model_ = noiseModel::Diagonal::Sigmas(sub(noiseModel->sigmas(), frontalDim, noiseModel->dim())); +// if(debug) this->print("Eliminated factor: "); +// 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; row eliminate(size_t nrFrontals = 1); + /** + * splits a pre-factorized factor into a conditional, and changes the current + * factor to be the remaining component. Performs same operation as eliminate(), + * but without running QR. + */ + boost::shared_ptr splitConditional(size_t nrFrontals = 1); + /* Used by ::CombineJacobians for sorting */ struct _RowSource { size_t firstNonzeroVar;