MathHelper.hpp
Go to the documentation of this file.
1 
20 #ifndef MATHHELPER_HPP_
21 #define MATHHELPER_HPP_
22 
23 #include "typedef.hpp"
24 #include <boost/math/special_functions/binomial.hpp>
25 #include <boost/random/mersenne_twister.hpp>
26 #include <boost/random/uniform_int.hpp>
27 #include <boost/random/uniform_real.hpp>
28 #include <boost/random/normal_distribution.hpp>
29 #include <boost/random/variate_generator.hpp>
30 #include <ros/ros.h>
31 #include <set>
32 #include <vector>
33 #include <geometry_msgs/Quaternion.h>
34 
35 namespace next_best_view {
43  class MathHelper {
44  private:
48  static boost::mt19937& getRandomnessGenerator();
49  public:
55  static SimpleSphereCoordinates convertC2S(const SimpleVector3 &cartesian);
56 
62  static void convertC2S(const SimpleVector3 &cartesian, SimpleSphereCoordinates &sphere);
63 
69  static SimpleVector3 convertS2C(const SimpleSphereCoordinates &sphere);
70 
76  static void convertS2C(const SimpleSphereCoordinates &sphere, SimpleVector3 &cartesian);
77 
78  static SimpleVector3 getVisualAxis(const SimpleQuaternion &orientation);
79 
80  static void getVisualAxis(const SimpleQuaternion &orientation, SimpleVector3 &resultAxis);
81 
86  static double getSignum(const double &value);
87 
93  static SimpleVector3 getProjection(const std::size_t &idx, const SimpleVector3 &X);
94 
100  static Precision getCosinus(const SimpleVector3 &X, const SimpleVector3 &Y);
101 
107  static Precision getAngle(const SimpleVector3 &X, const SimpleVector3 &Y);
108 
114  static Precision getMinimumAngleDifference(const Precision &firstAngle, const Precision &secondAngle);
115 
121  static int getRandomInteger(const int &min, const int &max);
122 
128  static Precision getRandomNumber(const Precision &mean, const Precision &standardDeviation);
129 
135  static SimpleVectorX getRandomVector(const SimpleVectorX &mean, const SimpleVectorX &standardDeviation);
136 
141 
148  static SimpleQuaternion getQuaternionByAngles(const Precision &heading, const Precision &attitude, const Precision &bank);
149 
154  static SimpleQuaternionCollectionPtr getOrientationsOnUnitSphere(const int &numberOfPoints);
155 
160  static double radToDeg(double input);
161 
166  static double degToRad(double input);
167 
168  static double getDotProduct(SimpleVector3 v1, SimpleVector3 v2);
169 
170  static bool vector3Equal(SimpleVector3 v1, SimpleVector3 v2);
171 
172  template<typename Set> static void printSet(boost::shared_ptr<Set> &setPtr) {
173  std::cout << "\t{ ";
174  BOOST_FOREACH(typename Set::value_type value, *setPtr) {
175  std::cout << value << ", ";
176  }
177  std::cout << "}" << std::endl;
178  }
179 
180  template<typename Set> static void printPowerSet(boost::shared_ptr<std::set<boost::shared_ptr<Set> > > &powerSetPtr) {
181  std::cout << "{ " << std::endl;
182  BOOST_FOREACH(boost::shared_ptr<Set> subSetPtr, *powerSetPtr) {
183  printSet(subSetPtr);
184  }
185  std::cout << "} " << std::endl;
186  std::cout << powerSetPtr->size() << " Items" << std::endl;
187  }
188 
189  template<typename Set> static boost::shared_ptr<std::set<boost::shared_ptr<Set> > > powerSet(const boost::shared_ptr<Set> &setPtr) {
191 
192  std::size_t value = 0;
193  assert(setPtr->size() <= 31);
194 
195 
196  std::size_t limit = (1 << setPtr->size());
197  for (std::size_t counter = 0; counter < limit; ++counter) {
198  boost::shared_ptr<Set> subSetPtr(new Set);
199  std::size_t idx = 0;
200  for (typename Set::iterator setIter = setPtr->begin(); setIter != setPtr->end(); ++setIter, ++idx) {
201  if ( (value & (1 << idx)) != 0 ) {
202  subSetPtr->insert(*setIter);
203  }
204  }
205  powerSetPtr->insert(subSetPtr);
206 
207  // the value
208  ++value;
209  }
210 
211  return powerSetPtr;
212  }
213 
214 
215  template<typename PowerSet> static boost::shared_ptr<PowerSet> filterCardinalityPowerSet(const boost::shared_ptr<PowerSet> &powerSetPtr, const std::size_t min, const std::size_t max) {
216  boost::shared_ptr<PowerSet> resultPowerSetPtr(new PowerSet);
217 
218  for (typename PowerSet::iterator powerSetIter = powerSetPtr->begin(); powerSetIter != powerSetPtr->end(); ++powerSetIter) {
219  if ((*powerSetIter)->size() < min || (*powerSetIter)->size() > max) {
220  continue;
221  }
222 
223  resultPowerSetPtr->insert(*powerSetIter);
224  }
225 
226  return resultPowerSetPtr;
227  }
228 
229  template<typename PowerSet> static boost::shared_ptr<PowerSet> filterCardinalityPowerSet(const boost::shared_ptr<PowerSet> &setPtr, const std::size_t min) {
230  return filterCardinalityPowerSet<PowerSet>(setPtr, min, setPtr->size());
231  }
232  };
233 }
234 
235 
236 
237 #endif /* MATHHELPER_HPP_ */
Eigen::Matrix< Precision, Eigen::Dynamic, 1 > SimpleVectorX
Definition: typedef.hpp:56
static bool vector3Equal(SimpleVector3 v1, SimpleVector3 v2)
Definition: MathHelper.cpp:181
SimpleVector3 SimpleSphereCoordinates
Definition: typedef.hpp:54
static double getSignum(const double &value)
Definition: MathHelper.cpp:63
Eigen::Matrix< Precision, 3, 1 > SimpleVector3
Definition: typedef.hpp:53
static Precision getAngle(const SimpleVector3 &X, const SimpleVector3 &Y)
Definition: MathHelper.cpp:81
static Precision getCosinus(const SimpleVector3 &X, const SimpleVector3 &Y)
Definition: MathHelper.cpp:71
MathHelper unites the generally needed math operations.
Definition: MathHelper.hpp:43
static Precision getMinimumAngleDifference(const Precision &firstAngle, const Precision &secondAngle)
Definition: MathHelper.cpp:87
static boost::shared_ptr< PowerSet > filterCardinalityPowerSet(const boost::shared_ptr< PowerSet > &setPtr, const std::size_t min)
Definition: MathHelper.hpp:229
static SimpleQuaternion getRandomQuaternion()
Definition: MathHelper.cpp:113
static Precision getRandomNumber(const Precision &mean, const Precision &standardDeviation)
Definition: MathHelper.cpp:98
this namespace contains all generally usable classes.
static boost::mt19937 & getRandomnessGenerator()
Definition: MathHelper.cpp:24
static SimpleVectorX getRandomVector(const SimpleVectorX &mean, const SimpleVectorX &standardDeviation)
Definition: MathHelper.cpp:104
static SimpleVector3 getVisualAxis(const SimpleQuaternion &orientation)
Definition: MathHelper.cpp:53
static SimpleVector3 getProjection(const std::size_t &idx, const SimpleVector3 &X)
Definition: MathHelper.cpp:67
static void printSet(boost::shared_ptr< Set > &setPtr)
Definition: MathHelper.hpp:172
static SimpleQuaternion getQuaternionByAngles(const Precision &heading, const Precision &attitude, const Precision &bank)
Definition: MathHelper.cpp:118
static double degToRad(double input)
Definition: MathHelper.cpp:172
static double getDotProduct(SimpleVector3 v1, SimpleVector3 v2)
Definition: MathHelper.cpp:176
static double radToDeg(double input)
Definition: MathHelper.cpp:168
int min(int a, int b)
static boost::shared_ptr< std::set< boost::shared_ptr< Set > > > powerSet(const boost::shared_ptr< Set > &setPtr)
Definition: MathHelper.hpp:189
static void printPowerSet(boost::shared_ptr< std::set< boost::shared_ptr< Set > > > &powerSetPtr)
Definition: MathHelper.hpp:180
static boost::shared_ptr< PowerSet > filterCardinalityPowerSet(const boost::shared_ptr< PowerSet > &powerSetPtr, const std::size_t min, const std::size_t max)
Definition: MathHelper.hpp:215
Eigen::Quaternion< Precision > SimpleQuaternion
Definition: typedef.hpp:67
static SimpleSphereCoordinates convertC2S(const SimpleVector3 &cartesian)
converts cartesian coordinates to sphere coordinates
Definition: MathHelper.cpp:29
static SimpleVector3 convertS2C(const SimpleSphereCoordinates &sphere)
converts sphere coordinates to cartesian coordinates (lightweight)
Definition: MathHelper.cpp:41
float Precision
Definition: typedef.hpp:36
static SimpleQuaternionCollectionPtr getOrientationsOnUnitSphere(const int &numberOfPoints)
Definition: MathHelper.cpp:136
static int getRandomInteger(const int &min, const int &max)
Definition: MathHelper.cpp:92


asr_next_best_view
Author(s): Aumann Florian, Borella Jocelyn, Heller Florian, Meißner Pascal, Schleicher Ralf, Stöckle Patrick, Stroh Daniel, Trautmann Jeremias, Walter Milena, Wittenbeck Valerij
autogenerated on Thu Jan 9 2020 07:20:18