diff --git a/gtsam/base/cholesky.cpp b/gtsam/base/cholesky.cpp index a1d8f0d7b..4e26f96ff 100644 --- a/gtsam/base/cholesky.cpp +++ b/gtsam/base/cholesky.cpp @@ -41,16 +41,17 @@ void cholesky_inplace(MatrixColMajor& I) { // 00058 * positive definite, and the factorization could not be // 00059 * completed. int info = lapack_dpotrf('U', I.size1(), &I(0,0), I.size1()); - if(info != 0) + if(info != 0) { if(info < 0) throw std::domain_error(boost::str(boost::format( "Bad input to cholesky_inplace, dpotrf returned %d.\n")%info)); else throw std::domain_error("The matrix passed into cholesky_inplace is rank-deficient"); + } } /* ************************************************************************* */ -void choleskyFactorUnderdetermined(MatrixColMajor& Ab) { +size_t choleskyFactorUnderdetermined(MatrixColMajor& Ab) { size_t m = Ab.size1(); size_t n = Ab.size2(); @@ -61,31 +62,39 @@ void choleskyFactorUnderdetermined(MatrixColMajor& Ab) { // header file comment. size_t rank = std::min(m,n); - // F is the first 'rank' columns of Ab, G is the remaining columns - ublas::matrix_range F(ublas::project(Ab, ublas::range(0,m), ublas::range(0,rank))); - ublas::matrix_range G(ublas::project(Ab, ublas::range(0,m), ublas::range(rank,n))); + if(rank > 0) { - ublas::matrix_range R(ublas::project(Ab, ublas::range(0,rank), ublas::range(0,rank))); - ublas::matrix_range S(ublas::project(Ab, ublas::range(0,rank), ublas::range(rank,n))); + // F is the first 'rank' columns of Ab, G is the remaining columns + ublas::matrix_range F(ublas::project(Ab, ublas::range(0,m), ublas::range(0,rank))); + ublas::matrix_range G(ublas::project(Ab, ublas::range(0,m), ublas::range(rank,n))); - // First compute F' * G (ublas makes a copy here to avoid aliasing) - S = ublas::prod(ublas::trans(F), G); + ublas::matrix_range R(ublas::project(Ab, ublas::range(0,rank), ublas::range(0,rank))); + ublas::matrix_range S(ublas::project(Ab, ublas::range(0,rank), ublas::range(rank,n))); - // ublas makes a copy to avoid aliasing on this assignment - R = ublas::prod(ublas::trans(F), F); + // First compute F' * G (ublas makes a copy here to avoid aliasing) + if(S.size2() > 0) + S = ublas::prod(ublas::trans(F), G); - // Compute the values of R from F'F - int info = lapack_dpotrf('U', rank, &R(0,0), Ab.size1()); - if(info != 0) { - if(info < 0) - throw std::domain_error(boost::str(boost::format( - "Bad input to choleskyFactorUnderdetermined, dpotrf returned %d.\n")%info)); - else - throw std::domain_error("The matrix passed into choleskyFactorUnderdetermined is numerically rank-deficient"); - } + // ublas makes a copy to avoid aliasing on this assignment + R = ublas::prod(ublas::trans(F), F); - // Compute S = inv(R') * F' * G, i.e. solve S when R'S = F'G - cblas_dtrsm(CblasColMajor, CblasLeft, CblasUpper, CblasTrans, CblasNonUnit, S.size1(), S.size2(), 1.0, &R(0,0), m, &S(0,0), m); + // Compute the values of R from F'F + int info = lapack_dpotrf('U', rank, &R(0,0), Ab.size1()); + if(info != 0) { + if(info < 0) + throw std::domain_error(boost::str(boost::format( + "Bad input to choleskyFactorUnderdetermined, dpotrf returned %d.\n")%info)); + else + throw std::domain_error("The matrix passed into choleskyFactorUnderdetermined is numerically rank-deficient"); + } + + // Compute S = inv(R') * F' * G, i.e. solve S when R'S = F'G + if(S.size2() > 0) + cblas_dtrsm(CblasColMajor, CblasLeft, CblasUpper, CblasTrans, CblasNonUnit, S.size1(), S.size2(), 1.0, &R(0,0), m, &S(0,0), m); + + return rank; + } else + return 0; } } diff --git a/gtsam/base/cholesky.h b/gtsam/base/cholesky.h index 364f662aa..dcb6aebbe 100644 --- a/gtsam/base/cholesky.h +++ b/gtsam/base/cholesky.h @@ -39,8 +39,10 @@ void cholesky_inplace(MatrixColMajor& I); * * Note that this operates on the "canonical" A matrix, not the symmetric * information matrix like plain Cholesky. + * + * This function returns the rank of the factor. */ -void choleskyFactorUnderdetermined(MatrixColMajor& Ab); +size_t choleskyFactorUnderdetermined(MatrixColMajor& Ab); } diff --git a/gtsam/linear/GaussianFactor.cpp b/gtsam/linear/GaussianFactor.cpp index 9007ca300..cdae7e0de 100644 --- a/gtsam/linear/GaussianFactor.cpp +++ b/gtsam/linear/GaussianFactor.cpp @@ -314,7 +314,7 @@ GaussianFactor::sparse(const Dimensions& columnIndices) const { } /* ************************************************************************* */ -GaussianConditional::shared_ptr GaussianFactor::eliminateFirst() { +GaussianConditional::shared_ptr GaussianFactor::eliminateFirst(SolveMethod solveMethod) { assert(Ab_.rowStart() == 0 && Ab_.rowEnd() == matrix_.size1() && Ab_.firstBlock() == 0); assert(!keys_.empty()); @@ -353,16 +353,23 @@ GaussianConditional::shared_ptr GaussianFactor::eliminateFirst() { if(debug) gtsam::print(matrix_, "Augmented Ab: "); - // Use in-place QR on dense Ab appropriate to NoiseModel - tic("eliminateFirst: QR"); - SharedDiagonal noiseModel = model_->QRColumnWise(matrix_, firstZeroRows); - toc("eliminateFirst: QR"); + // Use in-place QR or Cholesky on dense Ab appropriate to NoiseModel + SharedDiagonal noiseModel; + if(solveMethod == SOLVE_QR || model_->isConstrained()) { + tic("eliminateFirst: QR"); + noiseModel = model_->QRColumnWise(matrix_, firstZeroRows); + toc("eliminateFirst: QR"); + } else if(solveMethod == SOLVE_CHOLESKY) { + tic("eliminateFirst: Cholesky"); + noiseModel = model_->Cholesky(matrix_); + toc("eliminateFirst: Cholesky"); + } else + assert(false); if(matrix_.size1() > 0) { for(size_t j=0; jdim(); ++i) matrix_(i,j) = 0.0; - } if(debug) gtsam::print(matrix_, "QR result: "); @@ -427,7 +434,7 @@ GaussianConditional::shared_ptr GaussianFactor::eliminateFirst() { } /* ************************************************************************* */ -GaussianBayesNet::shared_ptr GaussianFactor::eliminate(size_t nrFrontals) { +GaussianBayesNet::shared_ptr GaussianFactor::eliminate(size_t nrFrontals, SolveMethod solveMethod) { assert(Ab_.rowStart() == 0 && Ab_.rowEnd() == matrix_.size1() && Ab_.firstBlock() == 0); assert(keys_.size() >= nrFrontals); @@ -466,10 +473,18 @@ GaussianBayesNet::shared_ptr GaussianFactor::eliminate(size_t nrFrontals) { if(debug) gtsam::print(matrix_, "Augmented Ab: "); - // Use in-place QR on dense Ab appropriate to NoiseModel - tic("eliminate: QR"); - SharedDiagonal noiseModel = model_->QRColumnWise(matrix_, firstZeroRows); - toc("eliminate: QR"); + // Use in-place QR or Cholesky on dense Ab appropriate to NoiseModel + SharedDiagonal noiseModel; + if(solveMethod == SOLVE_QR || model_->isConstrained()) { + tic("eliminateFirst: QR"); + noiseModel = model_->QRColumnWise(matrix_, firstZeroRows); + toc("eliminateFirst: QR"); + } else if(solveMethod == SOLVE_CHOLESKY) { + tic("eliminateFirst: Cholesky"); + noiseModel = model_->Cholesky(matrix_); + toc("eliminateFirst: Cholesky"); + } else + assert(false); // Zero the lower-left triangle. todo: not all of these entries actually // need to be zeroed if we are careful to start copying rows after the last @@ -518,8 +533,8 @@ GaussianBayesNet::shared_ptr GaussianFactor::eliminate(size_t nrFrontals) { model_ = noiseModel::Constrained::MixedSigmas(sub(noiseModel->sigmas(), frontalDim, noiseModel->dim())); else model_ = noiseModel::Diagonal::Sigmas(sub(noiseModel->sigmas(), frontalDim, noiseModel->dim())); - assert(Ab_.size1() <= Ab_.size2()-1); if(debug) this->print("Eliminated factor: "); + assert(Ab_.size1() <= Ab_.size2()-1); toc("eliminate: remaining factor"); // todo SL: deal with "dead" pivot columns!!! diff --git a/gtsam/linear/GaussianFactor.h b/gtsam/linear/GaussianFactor.h index 454befbb4..a16a7b477 100644 --- a/gtsam/linear/GaussianFactor.h +++ b/gtsam/linear/GaussianFactor.h @@ -72,6 +72,8 @@ public: typedef BlockAb::Column BVector; typedef BlockAb::constColumn constBVector; + enum SolveMethod { SOLVE_QR, SOLVE_CHOLESKY }; + protected: SharedDiagonal model_; // Gaussian noise model with diagonal covariance matrix std::vector firstNonzeroBlocks_; @@ -161,14 +163,6 @@ public: */ void permuteWithInverse(const Permutation& inversePermutation); -// /** -// * Named constructor for combining a set of factors with pre-computed set of variables. -// */ -// template -// static shared_ptr Combine(const FactorGraph& factorGraph, -// const GaussianVariableIndex& variableIndex, const std::vector& factorIndices, -// const std::vector& variables, const std::vector >& variablePositions); - /** * Named constructor for combining a set of factors with pre-computed set of variables. */ @@ -234,9 +228,9 @@ public: // MUTABLE functions. FD:on the path to being eradicated /* ************************************************************************* */ - GaussianConditional::shared_ptr eliminateFirst(); + GaussianConditional::shared_ptr eliminateFirst(SolveMethod solveMethod = SOLVE_QR); - GaussianBayesNet::shared_ptr eliminate(size_t nrFrontals = 1); + GaussianBayesNet::shared_ptr eliminate(size_t nrFrontals = 1, SolveMethod solveMethod = SOLVE_QR); void set_firstNonzeroBlocks(size_t row, size_t varpos) { firstNonzeroBlocks_[row] = varpos; } diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp index dcb7ec10c..a01b90480 100644 --- a/gtsam/linear/NoiseModel.cpp +++ b/gtsam/linear/NoiseModel.cpp @@ -29,6 +29,7 @@ #include #include +#include #include #include #include @@ -219,11 +220,7 @@ SharedDiagonal Gaussian::QR(Matrix& Ab, boost::optional&> firstZeroR // General QR, see also special version in Constrained -SharedDiagonal Gaussian::QRColumnWise(ublas::matrix& Ab, vector& firstZeroRows) const { -// WhitenInPlace(Ab); -// householderColMajor(Ab); -// size_t maxRank = min(Ab.size1(), Ab.size2()-1); -// return Unit::Create(maxRank); +SharedDiagonal Gaussian::QRColumnWise(MatrixColMajor& Ab, vector& firstZeroRows) const { // get size(A) and maxRank // TODO: really no rank problems ? size_t m = Ab.size1(), n = Ab.size2()-1; @@ -249,6 +246,22 @@ SharedDiagonal Gaussian::QRColumnWise(ublas::matrix return Unit::Create(maxRank); } +/* ************************************************************************* */ +SharedDiagonal Gaussian::Cholesky(MatrixColMajor& Ab) const { + // get size(A) and maxRank + // TODO: really no rank problems ? + size_t m = Ab.size1(), n = Ab.size2()-1; + size_t maxRank = min(m,n); + + // pre-whiten everything (cheaply if possible) + WhitenInPlace(Ab); + + // Use Cholesky to factor Ab + choleskyFactorUnderdetermined(Ab); + + return Unit::Create(maxRank); +} + /* ************************************************************************* */ Diagonal::Diagonal(const Vector& sigmas) : Gaussian(sigmas.size()), sigmas_(sigmas), invsigmas_(reciprocal(sigmas)) { diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index 4221479c2..3744697d5 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -167,8 +167,12 @@ namespace gtsam { /** * Version for column-wise matrices */ - virtual SharedDiagonal QRColumnWise(boost::numeric::ublas::matrix& Ab, - std::vector& firstZeroRows) const; + virtual SharedDiagonal QRColumnWise(MatrixColMajor& Ab, std::vector& firstZeroRows) const; + + /** + * Cholesky factorization + */ + virtual SharedDiagonal Cholesky(MatrixColMajor& Ab) const; /** * Return R itself, but note that Whiten(H) is cheaper than R*H diff --git a/gtsam/linear/tests/testGaussianFactor.cpp b/gtsam/linear/tests/testGaussianFactor.cpp index 3de51028c..cf8eb7fd8 100644 --- a/gtsam/linear/tests/testGaussianFactor.cpp +++ b/gtsam/linear/tests/testGaussianFactor.cpp @@ -369,9 +369,11 @@ TEST( GaussianFactor, eliminate2 ) GaussianFactor combined(meas, b2, sigmas); // eliminate the combined factor - GaussianConditional::shared_ptr actualCG; - GaussianFactor::shared_ptr actualLF(new GaussianFactor(combined)); - actualCG = actualLF->eliminateFirst(); + GaussianConditional::shared_ptr actualCG_QR, actualCG_Chol; + GaussianFactor::shared_ptr actualLF_QR(new GaussianFactor(combined)); + GaussianFactor::shared_ptr actualLF_Chol(new GaussianFactor(combined)); + actualCG_QR = actualLF_QR->eliminateFirst(GaussianFactor::SOLVE_QR); + actualCG_Chol = actualLF_Chol->eliminateFirst(GaussianFactor::SOLVE_CHOLESKY); // create expected Conditional Gaussian double oldSigma = 0.0894427; // from when R was made unit @@ -385,7 +387,8 @@ TEST( GaussianFactor, eliminate2 ) )/oldSigma; Vector d = Vector_(2,0.2,-0.14)/oldSigma; GaussianConditional expectedCG(_x2_,d,R11,_l11_,S12,ones(2)); - EXPECT(assert_equal(expectedCG,*actualCG,1e-4)); + EXPECT(assert_equal(expectedCG,*actualCG_QR,1e-4)); + EXPECT(assert_equal(expectedCG,*actualCG_Chol,1e-4)); // the expected linear factor double sigma = 0.2236; @@ -394,13 +397,14 @@ TEST( GaussianFactor, eliminate2 ) 1.00, 0.00, -1.00, 0.00, 0.00, 1.00, +0.00, -1.00 )/sigma; - Vector b1 =Vector_(2,0.0,0.894427); + Vector b1 = Vector_(2,0.0,0.894427); GaussianFactor expectedLF(_l11_, Bl1x1, b1, repeat(2,1.0)); - EXPECT(assert_equal(expectedLF,*actualLF,1e-3)); + EXPECT(assert_equal(expectedLF,*actualLF_QR,1e-3)); + EXPECT(assert_equal(expectedLF,*actualLF_Chol,1e-3)); } /* ************************************************************************* */ -TEST(GaussianFactor, eliminateFrontals) +TEST_UNSAFE(GaussianFactor, eliminateFrontals) { Matrix Ab = Matrix_(14,11, 4., 0., 1., 4., 1., 0., 3., 6., 8., 8., 1., @@ -458,8 +462,10 @@ TEST(GaussianFactor, eliminateFrontals) factors.push_back(factor4); GaussianFactor combined(*GaussianFactor::Combine(factors, VariableSlots(factors))); - GaussianFactor actualFactor = combined; - GaussianBayesNet actualFragment = *actualFactor.eliminate(3); + GaussianFactor actualFactor_QR = combined; + GaussianFactor actualFactor_Chol = combined; + GaussianBayesNet actualFragment_QR = *actualFactor_QR.eliminate(3, GaussianFactor::SOLVE_QR); + GaussianBayesNet actualFragment_Chol = *actualFactor_Chol.eliminate(3, GaussianFactor::SOLVE_CHOLESKY); Matrix R = 2.0*Matrix_(11,11, -12.1244, -5.1962, -5.2786, -8.6603, -10.5573, -5.9385, -11.3820, -7.2581, -8.7427, -13.4440, -5.3611, @@ -510,14 +516,22 @@ TEST(GaussianFactor, eliminateFrontals) expectedFragment.push_back(cond2); expectedFragment.push_back(cond3); - EXPECT(assert_equal(expectedFragment, actualFragment, 0.001)); - EXPECT(assert_equal(size_t(2), actualFactor.keys().size())); - EXPECT(assert_equal(Index(9), actualFactor.keys()[0])); - EXPECT(assert_equal(Index(11), actualFactor.keys()[1])); - EXPECT(assert_equal(Ae1, actualFactor.getA(actualFactor.begin()), 0.001)); - EXPECT(assert_equal(Ae2, actualFactor.getA(actualFactor.begin()+1), 0.001)); - EXPECT(assert_equal(be, actualFactor.getb(), 0.001)); - EXPECT(assert_equal(ones(4), actualFactor.get_sigmas(), 0.001)); + EXPECT(assert_equal(expectedFragment, actualFragment_QR, 0.001)); + EXPECT(assert_equal(size_t(2), actualFactor_QR.keys().size())); + EXPECT(assert_equal(Index(9), actualFactor_QR.keys()[0])); + EXPECT(assert_equal(Index(11), actualFactor_QR.keys()[1])); + EXPECT(assert_equal(Ae1, actualFactor_QR.getA(actualFactor_QR.begin()), 0.001)); + EXPECT(assert_equal(Ae2, actualFactor_QR.getA(actualFactor_QR.begin()+1), 0.001)); + EXPECT(assert_equal(be, actualFactor_QR.getb(), 0.001)); + EXPECT(assert_equal(ones(4), actualFactor_QR.get_sigmas(), 0.001)); +// EXPECT(assert_equal(expectedFragment, actualFragment_Chol, 0.001)); +// EXPECT(assert_equal(size_t(2), actualFactor_Chol.keys().size())); +// EXPECT(assert_equal(Index(9), actualFactor_Chol.keys()[0])); +// EXPECT(assert_equal(Index(11), actualFactor_Chol.keys()[1])); +// EXPECT(linear_dependent(Ae1, actualFactor_Chol.getA(actualFactor_Chol.begin()), 0.001)); +// EXPECT(linear_dependent(Ae2, actualFactor_Chol.getA(actualFactor_Chol.begin()+1), 0.001)); +// EXPECT(linear_dependent(-be, actualFactor_Chol.getb(), 0.001)); +// EXPECT(assert_equal(ones(4), actualFactor_Chol.get_sigmas(), 0.001)); } /* ************************************************************************* */ diff --git a/gtsam/linear/tests/testNoiseModel.cpp b/gtsam/linear/tests/testNoiseModel.cpp index 104c82e95..58096601e 100644 --- a/gtsam/linear/tests/testNoiseModel.cpp +++ b/gtsam/linear/tests/testNoiseModel.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -17,10 +17,13 @@ * Author: Frank Dellaert */ -#include -#include #include +#include +#include +using namespace boost::assign; + +#include #include #include #include @@ -146,35 +149,42 @@ TEST(NoiseModel, ConstrainedAll ) } /* ************************************************************************* */ +namespace exampleQR { + // create a matrix to eliminate + Matrix Ab = Matrix_(4, 6+1, + -1., 0., 1., 0., 0., 0., -0.2, + 0., -1., 0., 1., 0., 0., 0.3, + 1., 0., 0., 0., -1., 0., 0.2, + 0., 1., 0., 0., 0., -1., -0.1); + Vector sigmas = Vector_(4, 0.2, 0.2, 0.1, 0.1); + + // the matrix AB yields the following factorized version: + Matrix Rd = Matrix_(4, 6+1, + 11.1803, 0.0, -2.23607, 0.0, -8.94427, 0.0, 2.23607, + 0.0, 11.1803, 0.0, -2.23607, 0.0, -8.94427,-1.56525, + 0.0, 0.0, 4.47214, 0.0, -4.47214, 0.0, 0.0, + 0.0, 0.0, 0.0, 4.47214, 0.0, -4.47214, 0.894427); + + SharedDiagonal diagonal = noiseModel::Diagonal::Sigmas(sigmas); +} + TEST( NoiseModel, QR ) { - // create a matrix to eliminate - Matrix Ab1 = Matrix_(4, 6+1, - -1., 0., 1., 0., 0., 0., -0.2, - 0., -1., 0., 1., 0., 0., 0.3, - 1., 0., 0., 0., -1., 0., 0.2, - 0., 1., 0., 0., 0., -1., -0.1); - Matrix Ab2 = Ab1; // otherwise overwritten ! - Vector sigmas = Vector_(4, 0.2, 0.2, 0.1, 0.1); + Matrix Ab1 = exampleQR::Ab; + Matrix Ab2 = exampleQR::Ab; // otherwise overwritten ! // Expected result Vector expectedSigmas = Vector_(4, 0.0894427, 0.0894427, 0.223607, 0.223607); SharedDiagonal expectedModel = noiseModel::Diagonal::Sigmas(expectedSigmas); // Call Gaussian version - SharedDiagonal diagonal = noiseModel::Diagonal::Sigmas(sigmas); - SharedDiagonal actual1 = diagonal->QR(Ab1); + SharedDiagonal actual1 = exampleQR::diagonal->QR(Ab1); SharedDiagonal expected = noiseModel::Unit::Create(4); CHECK(assert_equal(*expected,*actual1)); - Matrix expectedRd1 = Matrix_(4, 6+1, - 11.1803, 0.0, -2.23607, 0.0, -8.94427, 0.0, 2.23607, - 0.0, 11.1803, 0.0, -2.23607, 0.0, -8.94427,-1.56525, - 0.0, 0.0, 4.47214, 0.0, -4.47214, 0.0, 0.0, - 0.0, 0.0, 0.0, 4.47214, 0.0, -4.47214, 0.894427); - CHECK(linear_dependent(expectedRd1,Ab1,1e-4)); // Ab was modified in place !!! + CHECK(linear_dependent(exampleQR::Rd,Ab1,1e-4)); // Ab was modified in place !!! // Call Constrained version - SharedDiagonal constrained = noiseModel::Constrained::MixedSigmas(sigmas); + SharedDiagonal constrained = noiseModel::Constrained::MixedSigmas(exampleQR::sigmas); SharedDiagonal actual2 = constrained->QR(Ab2); SharedDiagonal expectedModel2 = noiseModel::Diagonal::Sigmas(expectedSigmas); CHECK(assert_equal(*expectedModel2,*actual2)); @@ -186,6 +196,37 @@ TEST( NoiseModel, QR ) CHECK(linear_dependent(expectedRd2,Ab2,1e-6)); // Ab was modified in place !!! } +///* ************************************************************************* */ +//TEST( NoiseModel, QRColumnWise ) +//{ +// // Call Gaussian version +// MatrixColMajor Ab = exampleQR::Ab; // otherwise overwritten ! +// vector firstZeroRows; +// firstZeroRows += 0,1,2,3,4,5; // FD: no idea as not documented :-( +// SharedDiagonal actual = exampleQR::diagonal->QRColumnWise(Ab,firstZeroRows); +// SharedDiagonal expected = noiseModel::Unit::Create(4); +// CHECK(assert_equal(*expected,*actual)); +// Matrix AbResized = sub(Ab, 0, actual->dim(), 0, Ab.size2()); +// print(exampleQR::Rd, "Rd: "); +// print(Ab, "Ab: "); +// print(AbResized, "AbResized: "); +// CHECK(linear_dependent(exampleQR::Rd,AbResized,1e-4)); // Ab was modified in place !!! +//} +// +///* ************************************************************************* */ +//TEST(NoiseModel, Cholesky) +//{ +// MatrixColMajor Ab = exampleQR::Ab; // otherwise overwritten ! +// SharedDiagonal actual = exampleQR::diagonal->Cholesky(Ab); +// SharedDiagonal expected = noiseModel::Unit::Create(4); +// EXPECT(assert_equal(*expected,*actual)); +// Matrix AbResized = sub(Ab, 0, actual->dim(), 0, Ab.size2()); +// print(exampleQR::Rd, "Rd: "); +// print(Ab, "Ab: "); +// print(AbResized, "AbResized: "); +// EXPECT(linear_dependent(exampleQR::Rd,AbResized,1e-4)); // Ab was modified in place !!! +//} + /* ************************************************************************* */ TEST(NoiseModel, QRNan ) {