EarlyVisionConverters.cpp
Go to the documentation of this file.
1 /*
2 * This file is part of ArmarX.
3 *
4 * ArmarX is free software; you can redistribute it and/or modify
5 * it under the terms of the GNU General Public License version 2 as
6 * published by the Free Software Foundation.
7 *
8 * ArmarX is distributed in the hope that it will be useful, but
9 * WITHOUT ANY WARRANTY; without even the implied warranty of
10 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
11 * GNU General Public License for more details.
12 *
13 * You should have received a copy of the GNU General Public License
14 * along with this program. If not, see <http://www.gnu.org/licenses/>.
15 *
16 * @package MemoryX::Helpers
17 * @author Kai Welke ( welke at kit dot edu)
18 * @date 2012
19 * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
20 * GNU General Public License
21 */
22 
23 #include "EarlyVisionConverters.h"
24 
26 
27 namespace memoryx
28 {
29  Gaussian EarlyVisionConverters::convertToGaussian(const NormalDistributionBasePtr& normalDistribution)
30  {
31  if (!normalDistribution)
32  {
33  ARMARX_WARNING_S << "normalDistribution is NULL";
34  return Gaussian();
35  }
36 
37  int dimensions = normalDistribution->getDimensions();
38  Gaussian result(dimensions);
39 
40  Eigen::MatrixXd cov(dimensions, dimensions);
41  Eigen::VectorXd mean(dimensions);
42 
43  for (int r = 0 ; r < dimensions ; r++)
44  {
45  mean(r) = normalDistribution->getMean()[r];
46 
47  for (int c = 0 ; c < dimensions ; c++)
48  {
49  cov(r, c) = normalDistribution->getCovariance(r, c);
50  }
51  }
52 
53  result.setCovariance(cov);
54  result.setMean(mean);
55 
56  return result;
57  }
58 
60  {
61  int dimensions = gaussian.getDimensions();
63 
65 
66  for (int r = 0 ; r < dimensions ; r++)
67  {
68  Eigen::VectorXd meanV = gaussian.getMean();
69  mean.push_back(meanV(r));
70 
71  for (int c = 0 ; c < dimensions ; c++)
72  {
73  Eigen::MatrixXd covV = gaussian.getCovariance();
74  result->setCovariance(r, c, covV(r, c));
75  }
76  }
77 
78  result->setMean(mean);
79 
80  return result;
81  }
82 
84  {
85  int dimensions = gaussian.getDimensions();
87 
89 
90  for (int d = 0 ; d < dimensions ; d++)
91  {
92  mean.push_back(gaussian.getMean()(d));
93  result->setVariance(d, gaussian.getCovariance()(d, d));
94  }
95 
96  result->setMean(mean);
97 
98  return result;
99  }
100 
102  {
103  UnivariateNormalDistributionPtr result = new UnivariateNormalDistribution(gaussian.getMean()(0), gaussian.getCovariance()(0, 0));
104 
105  return result;
106  }
107 }
Gaussian::getCovariance
const covariance_type & getCovariance() const
Definition: Gaussian.h:77
armarx::VariantType::IsotropicNormalDistribution
const armarx::VariantTypeId IsotropicNormalDistribution
Definition: ProbabilityMeasures.h:35
memoryx
VirtualRobot headers.
Definition: CommonPlacesTester.cpp:48
c
constexpr T c
Definition: UnscentedKalmanFilterTest.cpp:43
Gaussian::getDimensions
int getDimensions() const
Definition: Gaussian.h:85
IceInternal::Handle
Definition: forward_declarations.h:8
memoryx::EarlyVisionConverters::convertToMemoryX_ISO
IsotropicNormalDistributionPtr convertToMemoryX_ISO(const Gaussian &gaussian)
Definition: EarlyVisionConverters.cpp:83
memoryx::EarlyVisionConverters::convertToGaussian
Gaussian convertToGaussian(const NormalDistributionBasePtr &normalDistribution)
Definition: EarlyVisionConverters.cpp:29
armarx::mean
std::optional< float > mean(const boost::circular_buffer< NameValueMap > &buffer, const std::string &key)
Definition: KinematicUnitGuiPlugin.cpp:1615
armarx::FloatVector
::std::vector< ::Ice::Float > FloatVector
Definition: KinematicUnitGuiPlugin.h:327
Gaussian::setMean
void setMean(const value_type &mean)
Definition: Gaussian.cpp:245
memoryx::EarlyVisionConverters::convertToMemoryX_UNI
UnivariateNormalDistributionPtr convertToMemoryX_UNI(const Gaussian &gaussian)
Definition: EarlyVisionConverters.cpp:101
Gaussian::setCovariance
void setCovariance(const covariance_type &cov)
Definition: Gaussian.cpp:261
ARMARX_WARNING_S
#define ARMARX_WARNING_S
Definition: Logging.h:206
armarx::VariantType::UnivariateNormalDistribution
const armarx::VariantTypeId UnivariateNormalDistribution
Definition: ProbabilityMeasures.h:34
EarlyVisionConverters.h
memoryx::EarlyVisionConverters::convertToMemoryX_MULTI
MultivariateNormalDistributionPtr convertToMemoryX_MULTI(const Gaussian &gaussian)
Definition: EarlyVisionConverters.cpp:59
Logging.h
Gaussian::getMean
const value_type & getMean() const
Definition: Gaussian.h:81
Gaussian
Definition: Gaussian.h:46
armarx::VariantType::MultivariateNormalDistribution
const armarx::VariantTypeId MultivariateNormalDistribution
Definition: ProbabilityMeasures.h:36