46 msg.type = problib::PDF::DISCRETE;
48 msg.type = msg.data[0];
65 if (msg.type == problib::PDF::GAUSSIAN) {
73 if (msg.type == problib::PDF::MIXTURE) {
80 if (msg.type == problib::PDF::DISCRETE) {
90 return static_cast<const Mixture*
>(&pdf);
113 return static_cast<const PMF*
>(&pdf);
120 return static_cast<const Hybrid*
>(&pdf);
151 const PMF* pmf =
static_cast<const PMF*
>(&pdf);
154 const Hybrid* hybrid =
static_cast<const Hybrid*
>(&pdf);
160 if (type == problib::PDF::MIXTURE) {
162 }
else if (type == problib::PDF::GAUSSIAN) {
164 }
else if (type == problib::PDF::UNIFORM) {
166 }
else if (type == problib::PDF::DISCRETE) {
168 }
else if (type == problib::PDF::EXACT) {
170 }
else if (type == problib::PDF::HYBRID) {
178 int new_data_size = dimensions + ((dimensions + 1) * dimensions / 2) + 1;
180 msg.data.reserve(msg.data.size() + new_data_size);
182 msg.type = problib::PDF::GAUSSIAN;
183 msg.data.push_back(msg.type);
186 for(
int i = 0; i < dimensions; ++i) {
187 msg.data.push_back(mu(i));
191 for(
int i = 0; i < dimensions; ++i) {
192 for(
int j = i; j < dimensions; ++j) {
193 msg.data.push_back(
cov(i, j));
200 for(
unsigned int i = 0; i < msg.dimensions; ++i) {
201 mu(i) = msg.data[i_data++];
205 for(
unsigned int i = 0; i < msg.dimensions; ++i) {
206 for(
unsigned int j = i; j < msg.dimensions; ++j) {
207 cov(i, j) = msg.data[i_data];
208 cov(j, i) = msg.data[i_data];
218 msg.data.push_back(problib::PDF::MIXTURE);
234 int num_components = (int)msg.data[i_data++];
236 for(
int c = 0; c < num_components; ++c) {
237 double w = msg.data[i_data++];
238 int type = (int)msg.data[i_data++];
251 msg.data.push_back(problib::PDF::UNIFORM);
268 PMF* pmf =
new PMF(msg.domain_size);
280 msg.data.push_back(problib::PDF::HYBRID);
283 msg.data.push_back(hybrid.
getPDFS().size());
286 const std::vector<PDF*> pdfs = hybrid.
getPDFS();
288 const PDF& pdf = **it;
291 bool already_contains_pmf = msg.domain_size != 0 || !msg.values.empty();
292 assert_msg(!already_contains_pmf,
"Can currently only decode at most one discrete pdf in hybrid pdf message.");
293 msg.data.push_back(problib::PDF::DISCRETE);
303 int num_components = (int)msg.data[i_data++];
305 for(
int c = 0; c < num_components; ++c) {
306 int type = (int)msg.data[i_data++];
317 if (!msg.exact_value_vec.empty()) {
319 unsigned int dim = msg.exact_value_vec.size();
321 for(
unsigned int i = 0; i < dim; ++i) {
322 mu(i) = msg.exact_value_vec[i];
326 }
else if (msg.exact_value_str !=
"") {
PDF * msgToPDF(const problib::PDF &msg)
Converts a PDF ROS message to a PDF object.
Gaussian * msgToGaussian(const problib::PDF &msg)
Converts a PDF ROS message to a Gaussian object if the message represents a Gaussian.
This class represents the weighted sum of a finite set of probability density functions.
void serialize_hybrid(const Hybrid &hybrid, problib::PDF &msg)
arma_hot const Col< eT > & zeros()
#define assert_msg(_Expression, _Msg)
Gaussian * deserialize_gaussian(const problib::PDF &msg, int &i_data)
void normalizeWeights()
Normalizes the weights of all components.
int components() const
Returns the number of components.
This class represents a multi-variate Gaussian (Normal) distribution.
void addPDF(const PDF &pdf, double priority)
const PMF * PDFtoPMF(const PDF &pdf)
Casts a PDF object to a PMF if the PDF represents a PMF.
std::string typeToName(PDF::PDFType type)
const Uniform * PDFtoUniform(const PDF &pdf)
Casts a PDF object to a Uniform distribution if the PDF represents a Uniform distribution.
void addComponent(const PDF &pdf, double w)
Adds a component pdf with given weight.
const arma::mat & getCovariance() const
Returns the covariance matrix of the Gaussian.
PMF * deserialize_discrete(const problib::PDF &msg)
void serialize_mixture(const Mixture &mix, problib::PDF &msg)
void setDomainSize(int domain_size)
Sets the domain size of this discrete distribution.
void serialize(const PDF &gauss, problib::PDF &msg)
const PDF & getComponent(int i) const
Returns a reference to the i'th component.
void serialize_discrete(const PMF &pmf, problib::PDF &msg)
const_iterator(const field< oT > &in_M, const bool at_end=false)
void getValues(std::vector< std::string > &values) const
Returns a vector of values for which a probability is specified.
void serialize_uniform(const Uniform &uniform, problib::PDF &msg)
const arma::vec & getMean() const
Returns the mean of the Gaussian.
Hybrid * deserialize_hybrid(const problib::PDF &msg, int &i_data)
void serialize_gaussian(const Gaussian &gauss, problib::PDF &msg)
const std::vector< PDF * > & getPDFS() const
const Op< T1, op_cov > cov(const Base< typename T1::elem_type, T1 > &X, const uword norm_type=0)
int getDomainSize() const
Returns the domain size of this distribution.
void getProbabilities(std::vector< double > &probabilities) const
Returns all probabilities of the known values.
PMF * msgToPMF(const problib::PDF &msg)
Converts a PDF ROS message to a PMF object if the message represents a PMF.
Uniform * deserialize_uniform(const problib::PDF &msg, int &i_data)
double getWeight(int i) const
Returns the weight of the i'th component.
PDF * deserialize_exact(const problib::PDF &msg)
Mixture * msgToMixture(const problib::PDF &msg)
Converts a PDF ROS message to a Mixture object if the message represents a Mixture.
const Mixture * PDFtoMixture(const PDF &pdf)
Casts a PDF object to a Mixture if the PDF represents a Mixture.
Mixture * deserialize_mixture(const problib::PDF &msg, int &i_data)
void setProbability(const std::string &value, double p)
Set the probability of a given value.
PDF * deserialize(const problib::PDF &msg, int type, int &i_data)
This class represents a discrete probability distribution (or probability mass function). Currently, this PMF can only take strings as values.
const Gaussian * PDFtoGaussian(const PDF &pdf)
Casts a PDF object to a Gaussian if the PDF represents a Gaussian.
void PDFtoMsg(const PDF &pdf, problib::PDF &msg)
Converts a PDF object to a ROS message.
const Hybrid * PDFtoHybrid(const PDF &pdf)