Added separate splitConditional() to remove the top part of a JacobianFactor after performing QR

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<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
+    // Start of next part
-    if(noiseModel->dim() < frontalDim) {
+    model_ = noiseModel;
-      throw domain_error((boost::format(
+    return splitConditional(nrFrontals);
-          "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;
 
+//    // 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;
   }
 
   /* ************************************************************************* */

gtsam/linear/JacobianFactor.h

@@ -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;