In-progress integrating Cholesky into solving
Richard Roberts
parent
50a26c1dbb
commit
fd597762c7
|
|
@ -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<MatrixColMajor> F(ublas::project(Ab, ublas::range(0,m), ublas::range(0,rank)));
|
||||
ublas::matrix_range<MatrixColMajor> G(ublas::project(Ab, ublas::range(0,m), ublas::range(rank,n)));
|
||||
if(rank > 0) {
|
||||
|
||||
ublas::matrix_range<MatrixColMajor> R(ublas::project(Ab, ublas::range(0,rank), ublas::range(0,rank)));
|
||||
ublas::matrix_range<MatrixColMajor> 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<MatrixColMajor> F(ublas::project(Ab, ublas::range(0,m), ublas::range(0,rank)));
|
||||
ublas::matrix_range<MatrixColMajor> 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<MatrixColMajor> R(ublas::project(Ab, ublas::range(0,rank), ublas::range(0,rank)));
|
||||
ublas::matrix_range<MatrixColMajor> 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;
|
||||
}
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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; j<matrix_.size2(); ++j)
|
||||
for(size_t i=j+1; i<noiseModel->dim(); ++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!!!
|
||||
|
|
|
|||
|
|
@ -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<size_t> 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<class STORAGE>
|
||||
// static shared_ptr Combine(const FactorGraph<GaussianFactor>& factorGraph,
|
||||
// const GaussianVariableIndex<STORAGE>& variableIndex, const std::vector<size_t>& factorIndices,
|
||||
// const std::vector<Index>& variables, const std::vector<std::vector<size_t> >& 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; }
|
||||
|
||||
|
|
|
|||
|
|
@ -29,6 +29,7 @@
|
|||
#include <boost/random/normal_distribution.hpp>
|
||||
#include <boost/random/variate_generator.hpp>
|
||||
|
||||
#include <gtsam/base/cholesky.h>
|
||||
#include <gtsam/linear/NoiseModel.h>
|
||||
#include <gtsam/linear/SharedDiagonal.h>
|
||||
#include <gtsam/base/DenseQRUtil.h>
|
||||
|
|
@ -219,11 +220,7 @@ SharedDiagonal Gaussian::QR(Matrix& Ab, boost::optional<vector<int>&> firstZeroR
|
|||
|
||||
|
||||
// General QR, see also special version in Constrained
|
||||
SharedDiagonal Gaussian::QRColumnWise(ublas::matrix<double, ublas::column_major>& Ab, vector<int>& 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<int>& 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<double, ublas::column_major>
|
|||
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)) {
|
||||
|
|
|
|||
|
|
@ -167,8 +167,12 @@ namespace gtsam {
|
|||
/**
|
||||
* Version for column-wise matrices
|
||||
*/
|
||||
virtual SharedDiagonal QRColumnWise(boost::numeric::ublas::matrix<double, boost::numeric::ublas::column_major>& Ab,
|
||||
std::vector<int>& firstZeroRows) const;
|
||||
virtual SharedDiagonal QRColumnWise(MatrixColMajor& Ab, std::vector<int>& firstZeroRows) const;
|
||||
|
||||
/**
|
||||
* Cholesky factorization
|
||||
*/
|
||||
virtual SharedDiagonal Cholesky(MatrixColMajor& Ab) const;
|
||||
|
||||
/**
|
||||
* Return R itself, but note that Whiten(H) is cheaper than R*H
|
||||
|
|
|
|||
|
|
@ -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));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -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 <CppUnitLite/TestHarness.h>
|
||||
|
||||
#include <boost/foreach.hpp>
|
||||
#include <iostream>
|
||||
#include <boost/foreach.hpp>
|
||||
#include <boost/assign/std/vector.hpp>
|
||||
using namespace boost::assign;
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
#include <gtsam/linear/NoiseModel.h>
|
||||
#include <gtsam/linear/SharedGaussian.h>
|
||||
#include <gtsam/linear/SharedDiagonal.h>
|
||||
|
|
@ -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<int> 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 )
|
||||
{
|
||||
|
|
|
|||
Loading…
Reference in New Issue