12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Extra debugging checks for Cholesky

release/4.3a0
Richard Roberts 2010-11-19 17:20:04 +00:00
parent 739764ca8e
commit 2fc5ab3bdb
2 changed files with 41 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

52
gtsam/linear/GaussianFactor.cpp
View File

@ -353,18 +353,40 @@ GaussianConditional::shared_ptr GaussianFactor::eliminateFirst(SolveMethod solve
if(debug) gtsam::print(matrix_, "Augmented Ab: ");
size_t firstVarDim = Ab_(0).size2();
// Use in-place QR or Cholesky on dense Ab appropriate to NoiseModel
SharedDiagonal noiseModel;
if(solveMethod == SOLVE_QR || model_->isConstrained()) {
tic("eliminateFirst: QR");
if(!debug) {
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);
} else {
// In debug mode, compare outputs of Cholesky and QR
MatrixColMajor matrixCopy = matrix_;
SharedDiagonal noiseModelDup = model_->Cholesky(matrixCopy);
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);
matrixCopy = ublas::triangular_adaptor<MatrixColMajor, ublas::upper>(matrixCopy);
matrix_ = ublas::triangular_adaptor<MatrixColMajor, ublas::upper>(matrix_);
assert(linear_dependent(
Matrix(
ublas::project(ublas::triangular_adaptor<MatrixColMajor, ublas::upper>(matrix_),
ublas::range(0,noiseModel->dim()), ublas::range(0,matrix_.size2()))),
Matrix(
ublas::project(ublas::triangular_adaptor<MatrixColMajor, ublas::upper>(matrixCopy),
ublas::range(0,noiseModel->dim()), ublas::range(0,matrix_.size2()))),
1e-2));
assert(assert_equal(*noiseModel, *noiseModelDup, 1e-2));
}
if(matrix_.size1() > 0) {
for(size_t j=0; j<matrix_.size2(); ++j)
@ -374,8 +396,6 @@ GaussianConditional::shared_ptr GaussianFactor::eliminateFirst(SolveMethod solve
if(debug) gtsam::print(matrix_, "QR result: ");
size_t firstVarDim = Ab_(0).size2();
// Check for singular factor
if(noiseModel->dim() < firstVarDim) {
throw domain_error((boost::format(
@ -421,6 +441,10 @@ GaussianConditional::shared_ptr GaussianFactor::eliminateFirst(SolveMethod solve
++ varpos;
firstNonzeroBlocks_[row] = varpos;
if(debug) cout << "firstNonzeroVars_[" << row << "] = " << firstNonzeroBlocks_[row] << endl;
// if(debug && varpos < size()) {
// ABlock block(Ab_(varpos));
// assert(!gtsam::equal_with_abs_tol(ublas::row(block, row), zero(block.size2()), 1e-5));
// }
}
toc("eliminateFirst: rowstarts");
@ -473,6 +497,8 @@ GaussianBayesNet::shared_ptr GaussianFactor::eliminate(size_t nrFrontals, SolveM
if(debug) gtsam::print(matrix_, "Augmented Ab: ");
size_t frontalDim = Ab_.range(0,nrFrontals).size2();
// Use in-place QR or Cholesky on dense Ab appropriate to NoiseModel
SharedDiagonal noiseModel;
if(solveMethod == SOLVE_QR || model_->isConstrained()) {
@ -497,8 +523,6 @@ GaussianBayesNet::shared_ptr GaussianFactor::eliminate(size_t nrFrontals, SolveM
if(debug) gtsam::print(matrix_, "QR result: ");
size_t frontalDim = Ab_.range(0,nrFrontals).size2();
// Check for singular factor
if(noiseModel->dim() < frontalDim) {
throw domain_error((boost::format(
@ -622,6 +646,8 @@ GaussianFactor::shared_ptr GaussianFactor::Combine(const FactorGraph<GaussianFac
static const bool debug = false;
if (verbose) std::cout << "GaussianFactor::GaussianFactor (factors)" << std::endl;
if(debug) factors.print("Combining factors: ");
if(debug) variableSlots.print();
// Determine dimensions

3
gtsam/linear/NoiseModel.cpp
View File

@ -257,7 +257,8 @@ SharedDiagonal Gaussian::Cholesky(MatrixColMajor& Ab) const {
WhitenInPlace(Ab);
// Use Cholesky to factor Ab
choleskyFactorUnderdetermined(Ab);
size_t rank = choleskyFactorUnderdetermined(Ab);
assert(rank == maxRank);
return Unit::Create(maxRank);
}