diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp index 6184c9563..71d111149 100644 --- a/gtsam/linear/GaussianFactorGraph.cpp +++ b/gtsam/linear/GaussianFactorGraph.cpp @@ -43,7 +43,7 @@ namespace gtsam { GaussianFactorGraph::Keys GaussianFactorGraph::keys() const { FastSet keys; BOOST_FOREACH(const sharedFactor& factor, *this) - if (factor) keys.insert(factor->begin(), factor->end()); + if (factor) keys.insert(factor->begin(), factor->end()); return keys; } @@ -51,15 +51,15 @@ namespace gtsam { void GaussianFactorGraph::permuteWithInverse( const Permutation& inversePermutation) { BOOST_FOREACH(const sharedFactor& factor, factors_) - { - factor->permuteWithInverse(inversePermutation); - } + { + factor->permuteWithInverse(inversePermutation); + } } /* ************************************************************************* */ void GaussianFactorGraph::combine(const GaussianFactorGraph &lfg) { for (const_iterator factor = lfg.factors_.begin(); factor - != lfg.factors_.end(); factor++) { + != lfg.factors_.end(); factor++) { push_back(*factor); } } @@ -73,7 +73,7 @@ namespace gtsam { // add the second factors_ in the graph for (const_iterator factor = lfg2.factors_.begin(); factor - != lfg2.factors_.end(); factor++) { + != lfg2.factors_.end(); factor++) { fg.push_back(*factor); } return fg; @@ -81,22 +81,22 @@ namespace gtsam { /* ************************************************************************* */ std::vector > GaussianFactorGraph::sparseJacobian() const { - // First find dimensions of each variable - FastVector dims; - BOOST_FOREACH(const sharedFactor& factor, *this) { - for(GaussianFactor::const_iterator pos = factor->begin(); pos != factor->end(); ++pos) { - if(dims.size() <= *pos) - dims.resize(*pos + 1, 0); - dims[*pos] = factor->getDim(pos); - } - } + // First find dimensions of each variable + FastVector dims; + BOOST_FOREACH(const sharedFactor& factor, *this) { + for(GaussianFactor::const_iterator pos = factor->begin(); pos != factor->end(); ++pos) { + if(dims.size() <= *pos) + dims.resize(*pos + 1, 0); + dims[*pos] = factor->getDim(pos); + } + } - // Compute first scalar column of each variable - vector columnIndices(dims.size()+1, 0); - for(size_t j=1; j columnIndices(dims.size()+1, 0); + for(size_t j=1; j triplet; FastVector entries; size_t row = 0; @@ -105,7 +105,7 @@ namespace gtsam { JacobianFactor::shared_ptr jacobianFactor( boost::dynamic_pointer_cast(factor)); if (!jacobianFactor) { - HessianFactor::shared_ptr hessian(boost::dynamic_pointer_cast(factor)); + HessianFactor::shared_ptr hessian(boost::dynamic_pointer_cast(factor)); if (hessian) jacobianFactor.reset(new JacobianFactor(*hessian)); else @@ -117,21 +117,21 @@ namespace gtsam { // iterate over all variables in the factor const JacobianFactor whitened(jacobianFactor->whiten()); for(JacobianFactor::const_iterator pos=whitened.begin(); pos 1e-12) entries.push_back( - boost::make_tuple(row+i, column_start+j, s)); - } + JacobianFactor::constABlock whitenedA = whitened.getA(pos); + // find first column index for this key + size_t column_start = columnIndices[*pos]; + for (size_t i = 0; i < (size_t) whitenedA.rows(); i++) + for (size_t j = 0; j < (size_t) whitenedA.cols(); j++) { + double s = whitenedA(i,j); + if (std::abs(s) > 1e-12) entries.push_back( + boost::make_tuple(row+i, column_start+j, s)); + } } JacobianFactor::constBVector whitenedb(whitened.getb()); size_t bcolumn = columnIndices.back(); for (size_t i = 0; i < (size_t) whitenedb.size(); i++) - entries.push_back(boost::make_tuple(row+i, bcolumn, whitenedb(i))); + entries.push_back(boost::make_tuple(row+i, bcolumn, whitenedb(i))); // Increment row index row += jacobianFactor->rows(); @@ -161,54 +161,54 @@ namespace gtsam { /* ************************************************************************* */ Matrix GaussianFactorGraph::denseJacobian() const { // combine all factors - JacobianFactor combined(*CombineJacobians(*convertCastFactors > (), VariableSlots(*this))); - return combined.matrix_augmented(); + return combined.matrix_augmented(); } - /* ************************************************************************* */ - // Helper functions for Combine - 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 - vector varDims(variableSlots.size()); - #endif - size_t m = 0; - size_t n = 0; - { - Index jointVarpos = 0; - BOOST_FOREACH(const VariableSlots::value_type& slots, variableSlots) { + /* ************************************************************************* */ + // Helper functions for Combine + 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 + vector varDims(variableSlots.size()); +#endif + size_t m = 0; + size_t n = 0; + { + Index jointVarpos = 0; + BOOST_FOREACH(const VariableSlots::value_type& slots, variableSlots) { - assert(slots.second.size() == factors.size()); + assert(slots.second.size() == factors.size()); - Index sourceFactorI = 0; - BOOST_FOREACH(const Index sourceVarpos, slots.second) { - if(sourceVarpos < numeric_limits::max()) { - const JacobianFactor& sourceFactor = *factors[sourceFactorI]; - size_t vardim = sourceFactor.getDim(sourceFactor.begin() + sourceVarpos); - #ifndef NDEBUG - if(varDims[jointVarpos] == numeric_limits::max()) { - varDims[jointVarpos] = vardim; - n += vardim; - } else - assert(varDims[jointVarpos] == vardim); - #else - varDims[jointVarpos] = vardim; - n += vardim; - break; - #endif - } - ++ sourceFactorI; - } - ++ jointVarpos; - } - BOOST_FOREACH(const JacobianFactor::shared_ptr& factor, factors) { - m += factor->rows(); - } - } - return boost::make_tuple(varDims, m, n); - } + Index sourceFactorI = 0; + BOOST_FOREACH(const Index sourceVarpos, slots.second) { + if(sourceVarpos < numeric_limits::max()) { + const JacobianFactor& sourceFactor = *factors[sourceFactorI]; + size_t vardim = sourceFactor.getDim(sourceFactor.begin() + sourceVarpos); +#ifndef NDEBUG +if(varDims[jointVarpos] == numeric_limits::max()) { + varDims[jointVarpos] = vardim; + n += vardim; +} else + assert(varDims[jointVarpos] == vardim); +#else + varDims[jointVarpos] = vardim; +n += vardim; +break; +#endif + } + ++ sourceFactorI; + } + ++ jointVarpos; + } + BOOST_FOREACH(const JacobianFactor::shared_ptr& factor, factors) { + m += factor->rows(); + } + } + return boost::make_tuple(varDims, m, n); + } /* ************************************************************************* */ JacobianFactor::shared_ptr CombineJacobians( @@ -261,7 +261,7 @@ namespace gtsam { const JacobianFactor& source(*factors[info.factorI]); size_t sourceRow = info.factorRowI; Index sourceSlot = varslot.second[info.factorI]; - combined->copyRow(source, sourceRow, sourceSlot, row, combinedSlot); + combined->copyRow(source, sourceRow, sourceSlot, row, combinedSlot); } ++combinedSlot; } @@ -303,56 +303,56 @@ namespace gtsam { return make_pair(gbn, jointFactor); } - /* ************************************************************************* */ - static - FastMap findScatterAndDims - (const FactorGraph& factors) { + /* ************************************************************************* */ + static + FastMap findScatterAndDims + (const FactorGraph& factors) { - const bool debug = ISDEBUG("findScatterAndDims"); + const bool debug = ISDEBUG("findScatterAndDims"); - // The "scatter" is a map from global variable indices to slot indices in the - // union of involved variables. We also include the dimensionality of the - // variable. + // The "scatter" is a map from global variable indices to slot indices in the + // union of involved variables. We also include the dimensionality of the + // variable. - Scatter scatter; + Scatter scatter; - // First do the set union. - BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factors) { - for(GaussianFactor::const_iterator variable = factor->begin(); variable != factor->end(); ++variable) { - scatter.insert(make_pair(*variable, SlotEntry(0, factor->getDim(variable)))); - } - } + // First do the set union. + BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factors) { + for(GaussianFactor::const_iterator variable = factor->begin(); variable != factor->end(); ++variable) { + scatter.insert(make_pair(*variable, SlotEntry(0, factor->getDim(variable)))); + } + } - // Next fill in the slot indices (we can only get these after doing the set - // union. - size_t slot = 0; - BOOST_FOREACH(Scatter::value_type& var_slot, scatter) { - var_slot.second.slot = (slot ++); - if(debug) - cout << "scatter[" << var_slot.first << "] = (slot " << var_slot.second.slot << ", dim " << var_slot.second.dimension << ")" << endl; - } + // Next fill in the slot indices (we can only get these after doing the set + // union. + size_t slot = 0; + BOOST_FOREACH(Scatter::value_type& var_slot, scatter) { + var_slot.second.slot = (slot ++); + if(debug) + cout << "scatter[" << var_slot.first << "] = (slot " << var_slot.second.slot << ", dim " << var_slot.second.dimension << ")" << endl; + } - return scatter; - } + return scatter; + } - /* ************************************************************************* */ - Matrix GaussianFactorGraph::denseHessian() const { + /* ************************************************************************* */ + Matrix GaussianFactorGraph::denseHessian() const { - Scatter scatter = findScatterAndDims(*this); + Scatter scatter = findScatterAndDims(*this); - vector dims; dims.reserve(scatter.size() + 1); - BOOST_FOREACH(const Scatter::value_type& index_entry, scatter) { - dims.push_back(index_entry.second.dimension); - } - dims.push_back(1); + vector dims; dims.reserve(scatter.size() + 1); + BOOST_FOREACH(const Scatter::value_type& index_entry, scatter) { + dims.push_back(index_entry.second.dimension); + } + dims.push_back(1); - // combine all factors and get upper-triangular part of Hessian - HessianFactor combined(*this, dims, scatter); - Matrix result = combined.info(); - // Fill in lower-triangular part of Hessian - result.triangularView() = result.transpose(); - return result; - } + // combine all factors and get upper-triangular part of Hessian + HessianFactor combined(*this, dims, scatter); + Matrix result = combined.info(); + // Fill in lower-triangular part of Hessian + result.triangularView() = result.transpose(); + return result; + } /* ************************************************************************* */ GaussianFactorGraph::EliminationResult EliminateCholesky(const FactorGraph< @@ -399,7 +399,7 @@ namespace gtsam { // Extract conditional and fill in details of the remaining factor tic(5, "split"); GaussianConditional::shared_ptr conditional = - combinedFactor->splitEliminatedFactor(nrFrontals); + combinedFactor->splitEliminatedFactor(nrFrontals); if (debug) { conditional->print("Extracted conditional: "); combinedFactor->print("Eliminated factor (L piece): "); @@ -422,26 +422,26 @@ namespace gtsam { FactorGraph jacobians; jacobians.reserve(factors.size()); BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factors) - if (factor) { - J::shared_ptr jacobian(boost::dynamic_pointer_cast(factor)); - if (jacobian) { - jacobians.push_back(jacobian); - if (debug) jacobian->print("Existing JacobianFactor: "); - } else { - H::shared_ptr hessian(boost::dynamic_pointer_cast(factor)); - if (!hessian) throw std::invalid_argument( - "convertToJacobians: factor is neither a JacobianFactor nor a HessianFactor."); - J::shared_ptr converted(new J(*hessian)); - if (debug) { - cout << "Converted HessianFactor to JacobianFactor:\n"; - hessian->print("HessianFactor: "); - converted->print("JacobianFactor: "); - if (!assert_equal(*hessian, HessianFactor(*converted), 1e-3)) throw runtime_error( - "convertToJacobians: Conversion between Jacobian and Hessian incorrect"); - } - jacobians.push_back(converted); + if (factor) { + J::shared_ptr jacobian(boost::dynamic_pointer_cast(factor)); + if (jacobian) { + jacobians.push_back(jacobian); + if (debug) jacobian->print("Existing JacobianFactor: "); + } else { + H::shared_ptr hessian(boost::dynamic_pointer_cast(factor)); + if (!hessian) throw std::invalid_argument( + "convertToJacobians: factor is neither a JacobianFactor nor a HessianFactor."); + J::shared_ptr converted(new J(*hessian)); + if (debug) { + cout << "Converted HessianFactor to JacobianFactor:\n"; + hessian->print("HessianFactor: "); + converted->print("JacobianFactor: "); + if (!assert_equal(*hessian, HessianFactor(*converted), 1e-3)) throw runtime_error( + "convertToJacobians: Conversion between Jacobian and Hessian incorrect"); } + jacobians.push_back(converted); } + } return jacobians; } @@ -452,7 +452,7 @@ namespace gtsam { const bool debug = ISDEBUG("EliminateQR"); // Convert all factors to the appropriate type and call the type-specific EliminateGaussian. - if (debug) cout << "Using QR:"; + if (debug) cout << "Using QR" << endl; tic(1, "convert to Jacobian"); FactorGraph jacobians = convertToJacobians(factors); @@ -467,155 +467,6 @@ namespace gtsam { return make_pair(conditional, factor); } // \EliminateQR - /* ************************************************************************* */ - GaussianFactorGraph::EliminationResult EliminatePreferCholesky( - const FactorGraph& factors, size_t nrFrontals) { - - typedef JacobianFactor J; - typedef HessianFactor H; - - // If any JacobianFactors have constrained noise models, we have to convert - // all factors to JacobianFactors. Otherwise, we can convert all factors - // to HessianFactors. This is because QR can handle constrained noise - // models but Cholesky cannot. - - // Decide whether to use QR or Cholesky - // Check if any JacobianFactors have constrained noise models. - bool useQR = false; - useQR = false; - BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factors) { - J::shared_ptr jacobian(boost::dynamic_pointer_cast(factor)); - if (jacobian && jacobian->get_model()->isConstrained()) { - useQR = true; - break; - } - } - - // Convert all factors to the appropriate type - // and call the type-specific EliminateGaussian. - if (useQR) return EliminateQR(factors, nrFrontals); - - GaussianFactorGraph::EliminationResult ret; -#ifdef NDEBUG - static const bool diag = false; -#else - static const bool diag = !ISDEBUG("NoCholeskyDiagnostics"); -#endif - if(!diag) { - tic(2, "EliminateCholesky"); - ret = EliminateCholesky(factors, nrFrontals); - toc(2, "EliminateCholesky"); - } else { - try { - tic(2, "EliminateCholesky"); - ret = EliminateCholesky(factors, nrFrontals); - toc(2, "EliminateCholesky"); - } catch (const exception& e) { - cout << "Exception in EliminateCholesky: " << e.what() << endl; - SETDEBUG("EliminateCholesky", true); - SETDEBUG("updateATA", true); - SETDEBUG("JacobianFactor::eliminate", true); - SETDEBUG("JacobianFactor::Combine", true); - SETDEBUG("choleskyPartial", true); - factors.print("Combining factors: "); - EliminateCholesky(factors, nrFrontals); - throw; - } - } - - const bool checkCholesky = ISDEBUG("EliminateGaussian Check Cholesky"); - if (checkCholesky) { - GaussianFactorGraph::EliminationResult expected; - FactorGraph jacobians = convertToJacobians(factors); - try { - // Compare with QR - expected = EliminateJacobians(jacobians, nrFrontals); - } catch (...) { - cout << "Exception in QR" << endl; - throw; - } - - H actual_factor(*ret.second); - H expected_factor(*expected.second); - if (!assert_equal(*expected.first, *ret.first, 100.0) || !assert_equal( - expected_factor, actual_factor, 1.0)) { - cout << "Cholesky and QR do not agree" << endl; - - SETDEBUG("EliminateCholesky", true); - SETDEBUG("updateATA", true); - SETDEBUG("JacobianFactor::eliminate", true); - SETDEBUG("JacobianFactor::Combine", true); - jacobians.print("Jacobian Factors: "); - EliminateJacobians(jacobians, nrFrontals); - EliminateCholesky(factors, nrFrontals); - factors.print("Combining factors: "); - - throw runtime_error("Cholesky and QR do not agree"); - } - } - - return ret; - - } // \EliminatePreferCholesky - - /* ************************************************************************* */ - GaussianFactorGraph::EliminationResult EliminateLDL( - const FactorGraph& factors, size_t nrFrontals) { - const bool debug = ISDEBUG("EliminateLDL"); - - // Find the scatter and variable dimensions - tic(1, "find scatter"); - Scatter scatter(findScatterAndDims(factors)); - toc(1, "find scatter"); - - // Pull out keys and dimensions - tic(2, "keys"); - vector dimensions(scatter.size() + 1); - BOOST_FOREACH(const Scatter::value_type& var_slot, scatter) { - dimensions[var_slot.second.slot] = var_slot.second.dimension; - } - // This is for the r.h.s. vector - dimensions.back() = 1; - toc(2, "keys"); - - // Form Ab' * Ab - tic(3, "combine"); - - if (debug) { - // print out everything before combine - factors.print("Factors to be combined into hessian"); - cout << "Dimensions (" << dimensions.size() << "): "; - BOOST_FOREACH(size_t d, dimensions) cout << d << " "; - cout << "\nScatter:" << endl; - BOOST_FOREACH(const Scatter::value_type& p, scatter) - cout << " Index: " << p.first << ", " << p.second.toString() << endl; - } - - HessianFactor::shared_ptr // - combinedFactor(new HessianFactor(factors, dimensions, scatter)); - toc(3, "combine"); - - // Do LDL, note that after this, the lower triangle still contains - // some untouched non-zeros that should be zero. We zero them while - // extracting submatrices next. - tic(4, "partial LDL"); - Eigen::LDLT::TranspositionType permutation = combinedFactor->partialLDL(nrFrontals); - toc(4, "partial LDL"); - - // Extract conditional and fill in details of the remaining factor - tic(5, "split"); - GaussianConditional::shared_ptr conditional = - combinedFactor->splitEliminatedFactor(nrFrontals, permutation); - if (debug) { - conditional->print("Extracted conditional: "); - combinedFactor->print("Eliminated factor (L piece): "); - } - toc(5, "split"); - - combinedFactor->assertInvariants(); - return make_pair(conditional, combinedFactor); - } // \EliminateLDL - /* ************************************************************************* */ bool hasConstraints(const FactorGraph& factors) { typedef JacobianFactor J; @@ -628,6 +479,142 @@ namespace gtsam { return false; } + /* ************************************************************************* */ + GaussianFactorGraph::EliminationResult EliminatePreferCholesky( + const FactorGraph& factors, size_t nrFrontals) { + + typedef JacobianFactor J; + typedef HessianFactor H; + + // If any JacobianFactors have constrained noise models, we have to convert + // all factors to JacobianFactors. Otherwise, we can convert all factors + // to HessianFactors. This is because QR can handle constrained noise + // models but Cholesky cannot. + if (hasConstraints(factors)) + return EliminateQR(factors, nrFrontals); + else { + GaussianFactorGraph::EliminationResult ret; +#ifdef NDEBUG + static const bool diag = false; +#else + static const bool diag = !ISDEBUG("NoCholeskyDiagnostics"); +#endif + if (!diag) { + tic(2, "EliminateCholesky"); + ret = EliminateCholesky(factors, nrFrontals); + toc(2, "EliminateCholesky"); + } else { + try { + tic(2, "EliminateCholesky"); + ret = EliminateCholesky(factors, nrFrontals); + toc(2, "EliminateCholesky"); + } catch (const exception& e) { + cout << "Exception in EliminateCholesky: " << e.what() << endl; + SETDEBUG("EliminateCholesky", true); + SETDEBUG("updateATA", true); + SETDEBUG("JacobianFactor::eliminate", true); + SETDEBUG("JacobianFactor::Combine", true); + SETDEBUG("choleskyPartial", true); + factors.print("Combining factors: "); + EliminateCholesky(factors, nrFrontals); + throw; + } + } + + const bool checkCholesky = ISDEBUG("EliminateGaussian Check Cholesky"); + if (checkCholesky) { + GaussianFactorGraph::EliminationResult expected; + FactorGraph jacobians = convertToJacobians(factors); + try { + // Compare with QR + expected = EliminateJacobians(jacobians, nrFrontals); + } catch (...) { + cout << "Exception in QR" << endl; + throw; + } + + H actual_factor(*ret.second); + H expected_factor(*expected.second); + if (!assert_equal(*expected.first, *ret.first, 100.0) + || !assert_equal(expected_factor, actual_factor, 1.0)) { + cout << "Cholesky and QR do not agree" << endl; + + SETDEBUG("EliminateCholesky", true); + SETDEBUG("updateATA", true); + SETDEBUG("JacobianFactor::eliminate", true); + SETDEBUG("JacobianFactor::Combine", true); + jacobians.print("Jacobian Factors: "); + EliminateJacobians(jacobians, nrFrontals); + EliminateCholesky(factors, nrFrontals); + factors.print("Combining factors: "); + + throw runtime_error("Cholesky and QR do not agree"); + } + } + + return ret; + } + + } // \EliminatePreferCholesky + + /* ************************************************************************* */ + GaussianFactorGraph::EliminationResult EliminateLDL( + const FactorGraph& factors, size_t nrFrontals) { + const bool debug = ISDEBUG("EliminateLDL"); + + // Find the scatter and variable dimensions + tic(1, "find scatter"); + Scatter scatter(findScatterAndDims(factors)); + toc(1, "find scatter"); + + // Pull out keys and dimensions + tic(2, "keys"); + vector dimensions(scatter.size() + 1); + BOOST_FOREACH(const Scatter::value_type& var_slot, scatter) { + dimensions[var_slot.second.slot] = var_slot.second.dimension; + } + // This is for the r.h.s. vector + dimensions.back() = 1; + toc(2, "keys"); + + // Form Ab' * Ab + tic(3, "combine"); + + if (debug) { + // print out everything before combine + factors.print("Factors to be combined into hessian"); + cout << "Dimensions (" << dimensions.size() << "): "; + BOOST_FOREACH(size_t d, dimensions) cout << d << " "; + cout << "\nScatter:" << endl; + BOOST_FOREACH(const Scatter::value_type& p, scatter) + cout << " Index: " << p.first << ", " << p.second.toString() << endl; + } + + HessianFactor::shared_ptr // + combinedFactor(new HessianFactor(factors, dimensions, scatter)); + toc(3, "combine"); + + // Do LDL, note that after this, the lower triangle still contains + // some untouched non-zeros that should be zero. We zero them while + // extracting submatrices next. + tic(4, "partial LDL"); + Eigen::LDLT::TranspositionType permutation = combinedFactor->partialLDL(nrFrontals); + toc(4, "partial LDL"); + + // Extract conditional and fill in details of the remaining factor + tic(5, "split"); + GaussianConditional::shared_ptr conditional = + combinedFactor->splitEliminatedFactor(nrFrontals, permutation); + if (debug) { + conditional->print("Extracted conditional: "); + combinedFactor->print("Eliminated factor (L piece): "); + } + toc(5, "split"); + + combinedFactor->assertInvariants(); + return make_pair(conditional, combinedFactor); + } // \EliminateLDL + /* ************************************************************************* */ GaussianFactorGraph::EliminationResult EliminatePreferLDL( const FactorGraph& factors, size_t nrFrontals) { @@ -643,72 +630,72 @@ namespace gtsam { // Decide whether to use QR or LDL // Check if any JacobianFactors have constrained noise models. if (hasConstraints(factors)) - EliminateQR(factors, nrFrontals); - - GaussianFactorGraph::EliminationResult ret; + return EliminateQR(factors, nrFrontals); + else { + GaussianFactorGraph::EliminationResult ret; #ifdef NDEBUG - static const bool diag = false; + static const bool diag = false; #else - static const bool diag = !ISDEBUG("NoLDLDiagnostics"); + static const bool diag = !ISDEBUG("NoLDLDiagnostics"); #endif - if(!diag) { - tic(2, "EliminateLDL"); - ret = EliminateLDL(factors, nrFrontals); - toc(2, "EliminateLDL"); - } else { - try { - tic(2, "EliminateLDL"); - ret = EliminateLDL(factors, nrFrontals); - toc(2, "EliminateLDL"); - } catch (const NegativeMatrixException& e) { - throw; - } catch (const exception& e) { - cout << "Exception in EliminateLDL: " << e.what() << endl; - SETDEBUG("EliminateLDL", true); - SETDEBUG("updateATA", true); - SETDEBUG("JacobianFactor::eliminate", true); - SETDEBUG("JacobianFactor::Combine", true); - SETDEBUG("ldlPartial", true); - SETDEBUG("findScatterAndDims", true); - factors.print("Combining factors: "); - EliminateLDL(factors, nrFrontals); - throw; - } - } - - const bool checkLDL = ISDEBUG("EliminateGaussian Check LDL"); - if (checkLDL) { - GaussianFactorGraph::EliminationResult expected; - FactorGraph jacobians = convertToJacobians(factors); - try { - // Compare with QR - expected = EliminateJacobians(jacobians, nrFrontals); - } catch (...) { - cout << "Exception in QR" << endl; - throw; + if(!diag) { + tic(2, "EliminateLDL"); + ret = EliminateLDL(factors, nrFrontals); + toc(2, "EliminateLDL"); + } else { + try { + tic(2, "EliminateLDL"); + ret = EliminateLDL(factors, nrFrontals); + toc(2, "EliminateLDL"); + } catch (const NegativeMatrixException& e) { + throw; + } catch (const exception& e) { + cout << "Exception in EliminateLDL: " << e.what() << endl; + SETDEBUG("EliminateLDL", true); + SETDEBUG("updateATA", true); + SETDEBUG("JacobianFactor::eliminate", true); + SETDEBUG("JacobianFactor::Combine", true); + SETDEBUG("ldlPartial", true); + SETDEBUG("findScatterAndDims", true); + factors.print("Combining factors: "); + EliminateLDL(factors, nrFrontals); + throw; + } } - H actual_factor(*ret.second); - H expected_factor(*expected.second); - if (!assert_equal(*expected.first, *ret.first, 100.0) || !assert_equal( - expected_factor, actual_factor, 1.0)) { - cout << "LDL and QR do not agree" << endl; + const bool checkLDL = ISDEBUG("EliminateGaussian Check LDL"); + if (checkLDL) { + GaussianFactorGraph::EliminationResult expected; + FactorGraph jacobians = convertToJacobians(factors); + try { + // Compare with QR + expected = EliminateJacobians(jacobians, nrFrontals); + } catch (...) { + cout << "Exception in QR" << endl; + throw; + } - SETDEBUG("EliminateLDL", true); - SETDEBUG("updateATA", true); - SETDEBUG("JacobianFactor::eliminate", true); - SETDEBUG("JacobianFactor::Combine", true); - jacobians.print("Jacobian Factors: "); - EliminateJacobians(jacobians, nrFrontals); - EliminateLDL(factors, nrFrontals); - factors.print("Combining factors: "); + H actual_factor(*ret.second); + H expected_factor(*expected.second); + if (!assert_equal(*expected.first, *ret.first, 100.0) || !assert_equal( + expected_factor, actual_factor, 1.0)) { + cout << "LDL and QR do not agree" << endl; - throw runtime_error("LDL and QR do not agree"); + SETDEBUG("EliminateLDL", true); + SETDEBUG("updateATA", true); + SETDEBUG("JacobianFactor::eliminate", true); + SETDEBUG("JacobianFactor::Combine", true); + jacobians.print("Jacobian Factors: "); + EliminateJacobians(jacobians, nrFrontals); + EliminateLDL(factors, nrFrontals); + factors.print("Combining factors: "); + + throw runtime_error("LDL and QR do not agree"); + } } + + return ret; } - - return ret; - } // \EliminatePreferLDL } // namespace gtsam