diff --git a/gtsam/linear/GaussianConditional.cpp b/gtsam/linear/GaussianConditional.cpp index 85b17373c..4597156bc 100644 --- a/gtsam/linear/GaussianConditional.cpp +++ b/gtsam/linear/GaussianConditional.cpp @@ -300,21 +300,11 @@ double GaussianConditional::logDeterminant() const { "conditionals"); } - // Noise model to sample from. - noiseModel::Diagonal::shared_ptr _model; - if (!model_) { - // Initialize model_ to Unit noiseModel if not initialized. - // Unit noise model specifies sigmas as 1, since - // the noise information should already be in information matrix A. - // Dim should be number of rows since - // noise model dimension should match (Ax - b) - _model = noiseModel::Unit::Create(rows()); - } else { - _model = model_; - } VectorValues solution = solve(parentsValues); Key key = firstFrontalKey(); - const Vector& sigmas = _model->sigmas(); + // The vector of sigma values for sampling. + // If no model, initialize sigmas to 1, else to model sigmas + const Vector& sigmas = (!model_) ? Vector::Ones(rows()) : model_->sigmas(); solution[key] += Sampler::sampleDiagonal(sigmas, rng); return solution; }