8 Gaussian::Gaussian(
unsigned int dim,
10 mode(ModeFromExtras(extraInputs)),
11 inputTypes(extraInputs),
12 covPrec(Eigen::VectorXd::Ones(dim))
19 mode(ModeFromExtras(extraInputs)),
20 inputTypes(extraInputs),
21 covPrec(Eigen::VectorXd::Ones(muIn.size()))
28 Eigen::MatrixXd
const& obj,
32 inputTypes(extraInputs),
50 if( (((extraInputs & ExtraInputs::DiagCovariance)>0) || ((extraInputs & ExtraInputs::FullCovariance)>0)) && (
mode == Mode::Precision))
51 throw std::logic_error(
"Extra arguments passed to Gaussian constructor do not match the covariance mode.");
52 if( (((extraInputs & ExtraInputs::DiagPrecision)>0) || ((extraInputs & ExtraInputs::FullPrecision)>0)) && (
mode == Mode::Covariance))
53 throw std::logic_error(
"Extra arguments passed to Gaussian constructor do not match the covariance mode.");
58 if( ((extraInputs & ExtraInputs::DiagCovariance)>0) || ((extraInputs & ExtraInputs::FullCovariance)>0)){
59 return Gaussian::Mode::Covariance;
61 return Gaussian::Mode::Precision;
67 Eigen::VectorXi output(2);
70 if((extraInputs & ExtraInputs::Mean)>0){
75 if( ((extraInputs & ExtraInputs::DiagCovariance)>0) || ((extraInputs & ExtraInputs::DiagPrecision)>0)){
76 output(numExtras) = dim;
81 if( ((extraInputs & ExtraInputs::FullCovariance)>0) || ((extraInputs & ExtraInputs::FullPrecision)>0)){
83 output(numExtras) = dim*dim;
87 return output.head(numExtras);
93 if(
mode==Mode::Covariance){
95 return covPrec.col(0).asDiagonal();
101 return covPrec.col(0).array().inverse().matrix().asDiagonal();
103 return covPrec.selfadjointView<Eigen::Lower>().llt().solve(Eigen::MatrixXd::Identity(
mean.rows(),
mean.rows()));
110 if(
mode==Mode::Precision){
112 return covPrec.col(0).asDiagonal();
118 return covPrec.col(0).array().inverse().matrix().asDiagonal();
120 return covPrec.selfadjointView<Eigen::Lower>().llt().solve(Eigen::MatrixXd::Identity(
mean.rows(),
mean.rows()));
128 if(
mode==Gaussian::Mode::Covariance ) {
139 }
else if(
mode==Gaussian::Mode::Precision ) {
155 unsigned currInd = 0;
158 assert(inputs.at(currInd).get().size() ==
mean.size());
159 mean = inputs.at(currInd);
163 if(((
inputTypes & ExtraInputs::DiagCovariance)>0) || ((
inputTypes & ExtraInputs::DiagPrecision)>0)){
165 if(inputs.at(currInd).get().size() !=
mean.size())
168 covPrec = inputs.at(currInd).get();
169 }
else if(((
inputTypes & ExtraInputs::FullCovariance)>0) || ((
inputTypes & ExtraInputs::FullPrecision)>0)){
171 if(inputs.at(currInd).get().size() !=
mean.size()*
mean.size())
174 Eigen::Map<const Eigen::MatrixXd> mat(inputs.at(currInd).get().data(),
mean.size(),
mean.size());
184 mode = Gaussian::Mode::Covariance;
186 assert(newCov.rows() ==
mean.rows());
191 assert(newCov.cols() ==
mean.rows());
201 mode = Gaussian::Mode::Precision;
203 assert(newPrec.rows() ==
mean.rows());
206 assert(newPrec.cols() ==
mean.rows());
217 if(
mode==Gaussian::Mode::Covariance){
219 return covPrec.col(0).array().sqrt().matrix().asDiagonal()*x;
225 return covPrec.col(0).array().inverse().sqrt().matrix().asDiagonal() * x;
233 if(
mode==Gaussian::Mode::Precision){
235 return covPrec.col(0).array().sqrt().matrix().asDiagonal()*x;
241 return covPrec.col(0).array().inverse().sqrt().matrix().asDiagonal() * x;
250 if(
mode==Gaussian::Mode::Covariance){
252 return covPrec.col(0).matrix().asDiagonal()*x;
254 return covPrec.selfadjointView<Eigen::Lower>()*x;
258 return covPrec.col(0).array().inverse().matrix().asDiagonal() * x;
266 if(
mode==Gaussian::Mode::Covariance){
268 return covPrec.col(0).array().inverse().matrix().asDiagonal()*x;
274 return covPrec.col(0).asDiagonal() * x;
276 return covPrec.selfadjointView<Eigen::Lower>()*x;
283 Eigen::VectorXd
const& data,
284 Eigen::MatrixXd
const& obsCov)
const
288 if(obsMat.rows() != data.rows())
290 if(obsCov.rows() != data.rows())
292 if((obsCov.rows() != obsCov.cols()) && (obsCov.cols()!=1))
296 Eigen::MatrixXd S = obsMat * HP.transpose();
297 if(obsCov.cols()==1){
298 S += obsCov.asDiagonal();
303 Eigen::MatrixXd K = S.selfadjointView<Eigen::Lower>().ldlt().solve(HP).transpose();
305 Eigen::VectorXd postMu =
mean + K*(data - obsMat*
mean);
308 return std::make_shared<Gaussian>(postMu, postCov);
Defines an abstract Gaussian class.@seealso Gaussian.
virtual unsigned int Dimension() const
Gaussian::InputMask inputTypes
What form do the extra inputs take? Just the mean, or the mean and covariance?
Eigen::MatrixXd GetPrecision() const
Gaussian::Mode mode
Have we specified the covariance or the precision.
void SetCovariance(Eigen::MatrixXd const &newCov)
Set the covariance matrix.
void SetPrecision(Eigen::MatrixXd const &newPrec)
Set the precision matrix.
static Gaussian::Mode ModeFromExtras(InputMask extraInputs)
static void CheckInputTypes(InputMask extraInputs, Mode mode)
virtual Eigen::MatrixXd ApplyCovSqrt(Eigen::Ref< const Eigen::MatrixXd > const &x) const override
void ComputeNormalization()
Compute the distribution's scaling constant.
Eigen::LLT< Eigen::MatrixXd > sqrtCovPrec
std::shared_ptr< Gaussian > Condition(Eigen::MatrixXd const &obsMat, Eigen::VectorXd const &data, Eigen::MatrixXd const &obsCov) const
Returns a new Gaussian distribution conditioned on a linear observation.
static Eigen::VectorXi GetExtraSizes(unsigned dim, InputMask extraInputs)
Mode
Are we specifying the mean, covariance matrix, or precision matrix.
Gaussian(unsigned int dim, InputMask extraInputs=ExtraInputs::None)
virtual Eigen::MatrixXd ApplyPrecSqrt(Eigen::Ref< const Eigen::MatrixXd > const &x) const override
double logDet
The log determinant of the covariance matrix.
virtual Eigen::MatrixXd ApplyCovariance(Eigen::Ref< const Eigen::MatrixXd > const &x) const override
virtual Eigen::MatrixXd ApplyPrecision(Eigen::Ref< const Eigen::MatrixXd > const &x) const override
Eigen::MatrixXd GetCovariance() const
Get the covariance.
void ResetHyperparameters(ref_vector< Eigen::VectorXd > const ¶ms) override
Exception to throw when matrices, vectors, or arrays are the wrong size.
std::vector< std::reference_wrapper< const T > > ref_vector
A vector of references to something ...
NLOHMANN_BASIC_JSON_TPL_DECLARATION std::string to_string(const NLOHMANN_BASIC_JSON_TPL &j)
user-defined to_string function for JSON values