Sampler.cpp
Go to the documentation of this file.
1 /* ----------------------------------------------------------------------------
2 
3  * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4  * Atlanta, Georgia 30332-0415
5  * All Rights Reserved
6  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7 
8  * See LICENSE for the license information
9 
10  * -------------------------------------------------------------------------- */
11 
19 #include <gtsam/linear/Sampler.h>
20 
21 #include <cassert>
22 
23 namespace gtsam {
24 
25 /* ************************************************************************* */
27  uint_fast64_t seed)
28  : model_(model), generator_(seed) {
29  if (!model) {
30  throw std::invalid_argument("Sampler::Sampler needs a non-null model.");
31  }
32 }
33 
34 /* ************************************************************************* */
36  : model_(noiseModel::Diagonal::Sigmas(sigmas, true)), generator_(seed) {}
37 
38 /* ************************************************************************* */
39 Vector Sampler::sampleDiagonal(const Vector& sigmas, std::mt19937_64* rng) {
40  size_t d = sigmas.size();
41  Vector result(d);
42  for (size_t i = 0; i < d; i++) {
43  double sigma = sigmas(i);
44 
45  // handle constrained case separately
46  if (sigma == 0.0) {
47  result(i) = 0.0;
48  } else {
49  std::normal_distribution<double> dist(0.0, sigma);
50  result(i) = dist(*rng);
51  }
52  }
53  return result;
54 }
55 
56 /* ************************************************************************* */
59 }
60 
61 /* ************************************************************************* */
63  assert(model_.get());
64  const Vector& sigmas = model_->sigmas();
65  return sampleDiagonal(sigmas);
66 }
67 
68 /* ************************************************************************* */
69 
70 } // namespace gtsam
rng
static std::mt19937 rng
Definition: timeFactorOverhead.cpp:31
gtsam::Sampler::sigmas
Vector sigmas() const
Definition: Sampler.h:68
d
static const double d[K][N]
Definition: igam.h:11
test_constructor::sigmas
Vector1 sigmas
Definition: testHybridNonlinearFactor.cpp:52
gtsam::Sampler::model
const noiseModel::Diagonal::shared_ptr & model() const
Definition: Sampler.h:70
gtsam::Sampler::Sampler
Sampler(const noiseModel::Diagonal::shared_ptr &model, uint_fast64_t seed=42u)
Definition: Sampler.cpp:26
gtsam::Sampler::model_
noiseModel::Diagonal::shared_ptr model_
Definition: Sampler.h:34
uint_fast64_t
uint64_t uint_fast64_t
Definition: ms_stdint.h:116
gtsam::Sampler::generator_
std::mt19937_64 generator_
Definition: Sampler.h:37
gtsam::Sampler::sampleDiagonal
static Vector sampleDiagonal(const Vector &sigmas, std::mt19937_64 *rng)
sample with given random number generator
Definition: Sampler.cpp:39
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:39
result
Values result
Definition: OdometryOptimize.cpp:8
sampling::sigma
static const double sigma
Definition: testGaussianBayesNet.cpp:170
model
noiseModel::Diagonal::shared_ptr model
Definition: doc/Code/Pose2SLAMExample.cpp:7
gtsam::Sampler::sample
Vector sample() const
sample from distribution
Definition: Sampler.cpp:62
gtsam
traits
Definition: SFMdata.h:40
gtsam::noiseModel::Diagonal::shared_ptr
std::shared_ptr< Diagonal > shared_ptr
Definition: NoiseModel.h:321
gtsam::Diagonal
static noiseModel::Diagonal::shared_ptr Diagonal(const Matrix &covariance)
Definition: ScenarioRunner.h:27
Sampler.h
sampling from a NoiseModel
i
int i
Definition: BiCGSTAB_step_by_step.cpp:9


gtsam
Author(s):
autogenerated on Sun Dec 22 2024 04:13:09