77 lines
2.3 KiB
C++
77 lines
2.3 KiB
C++
/* ----------------------------------------------------------------------------
|
|
|
|
* 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)
|
|
|
|
* See LICENSE for the license information
|
|
|
|
* -------------------------------------------------------------------------- */
|
|
|
|
/**
|
|
* @file Sampler.cpp
|
|
* @author Alex Cunningham
|
|
*/
|
|
|
|
#include <boost/random/normal_distribution.hpp>
|
|
#include <boost/random/variate_generator.hpp>
|
|
|
|
#include <gtsam/linear/Sampler.h>
|
|
namespace gtsam {
|
|
|
|
/* ************************************************************************* */
|
|
Sampler::Sampler(const noiseModel::Diagonal::shared_ptr& model, int32_t seed)
|
|
: model_(model), generator_(static_cast<unsigned>(seed))
|
|
{
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
Sampler::Sampler(const Vector& sigmas, int32_t seed)
|
|
: model_(noiseModel::Diagonal::Sigmas(sigmas, true)), generator_(static_cast<unsigned>(seed))
|
|
{
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
Sampler::Sampler(int32_t seed)
|
|
: generator_(static_cast<unsigned>(seed))
|
|
{
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
Vector Sampler::sampleDiagonal(const Vector& sigmas) {
|
|
size_t d = sigmas.size();
|
|
Vector result(d);
|
|
for (size_t i = 0; i < d; i++) {
|
|
double sigma = sigmas(i);
|
|
|
|
// handle constrained case separately
|
|
if (sigma == 0.0) {
|
|
result(i) = 0.0;
|
|
} else {
|
|
typedef boost::normal_distribution<double> Normal;
|
|
Normal dist(0.0, sigma);
|
|
boost::variate_generator<boost::minstd_rand&, Normal> norm(generator_, dist);
|
|
result(i) = norm();
|
|
}
|
|
}
|
|
return result;
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
Vector Sampler::sample() {
|
|
assert(model_.get());
|
|
const Vector& sigmas = model_->sigmas();
|
|
return sampleDiagonal(sigmas);
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
Vector Sampler::sampleNewModel(const noiseModel::Diagonal::shared_ptr& model) {
|
|
assert(model.get());
|
|
const Vector& sigmas = model->sigmas();
|
|
return sampleDiagonal(sigmas);
|
|
}
|
|
/* ************************************************************************* */
|
|
|
|
} // \namespace gtsam
|