Added separate splitConditional() to remove the top part of a JacobianFactor after performing QR
Alex Cunningham
parent
355141f985
commit
9ee098b1c1
|
|
@ -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<const Vector> 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; row<rows(); ++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->size() && Ab_.offset(varpos+1)-Ab_.offset(0) <= row)
|
||||
++ varpos;
|
||||
firstNonzeroBlocks_[row] = varpos;
|
||||
if(debug) cout << "firstNonzeroVars_[" << row << "] = " << firstNonzeroBlocks_[row] << endl;
|
||||
}
|
||||
toc(5, "rowstarts");
|
||||
|
||||
if(debug) print("Eliminated factor: ");
|
||||
|
||||
assertInvariants();
|
||||
|
||||
return conditional;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
GaussianConditional::shared_ptr JacobianFactor::eliminate(size_t nrFrontals) {
|
||||
|
||||
|
|
@ -457,60 +524,62 @@ namespace gtsam {
|
|||
if(debug) gtsam::print(matrix_, "QR result: ");
|
||||
if(debug) noiseModel->print("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<const Vector> sigmas = noiseModel->sigmas().segment(Ab_.rowStart(), Ab_.rowEnd()-Ab_.rowStart());
|
||||
conditionals = boost::make_shared<ConditionalType>(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; row<rows(); ++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->size() && Ab_.offset(varpos+1)-Ab_.offset(0) <= row)
|
||||
++ varpos;
|
||||
firstNonzeroBlocks_[row] = varpos;
|
||||
if(debug) cout << "firstNonzeroVars_[" << row << "] = " << firstNonzeroBlocks_[row] << endl;
|
||||
}
|
||||
toc(5, "rowstarts");
|
||||
|
||||
if(debug) print("Eliminated factor: ");
|
||||
|
||||
assertInvariants();
|
||||
|
||||
return conditionals;
|
||||
// Start of next part
|
||||
model_ = noiseModel;
|
||||
return splitConditional(nrFrontals);
|
||||
|
||||
// // 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 conditional
|
||||
// tic(3, "cond Rd");
|
||||
//
|
||||
// // Restrict the matrix to be in the first nrFrontals variables
|
||||
// Ab_.rowEnd() = Ab_.rowStart() + frontalDim;
|
||||
// const Eigen::VectorBlock<const Vector> 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<rows(); ++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->size() && Ab_.offset(varpos+1)-Ab_.offset(0) <= row)
|
||||
// ++ varpos;
|
||||
// firstNonzeroBlocks_[row] = varpos;
|
||||
// if(debug) cout << "firstNonzeroVars_[" << row << "] = " << firstNonzeroBlocks_[row] << endl;
|
||||
// }
|
||||
// toc(5, "rowstarts");
|
||||
//
|
||||
// if(debug) print("Eliminated factor: ");
|
||||
//
|
||||
// assertInvariants();
|
||||
//
|
||||
// return conditional;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -241,7 +241,6 @@ namespace gtsam {
|
|||
/**
|
||||
* Return (dense) matrix associated with factor
|
||||
* The returned system is an augmented matrix: [A b]
|
||||
* @param ordering of variables needed for matrix column order
|
||||
* @param set weight to use whitening to bake in weights
|
||||
*/
|
||||
Matrix matrix_augmented(bool weight = true) const;
|
||||
|
|
@ -268,6 +267,13 @@ namespace gtsam {
|
|||
/** return a multi-frontal conditional. It's actually a chordal Bayesnet */
|
||||
boost::shared_ptr<GaussianConditional> 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<GaussianConditional> splitConditional(size_t nrFrontals = 1);
|
||||
|
||||
/* Used by ::CombineJacobians for sorting */
|
||||
struct _RowSource {
|
||||
size_t firstNonzeroVar;
|
||||
|
|
|
|||
Loading…
Reference in New Issue