Main MRPT website > C++ reference
MRPT logo
Miscellaneous.h
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2014, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +---------------------------------------------------------------------------+ */
9 
10 /* Plane-based Map (PbMap) library
11  * Construction of plane-based maps and localization in it from RGBD Images.
12  * Writen by Eduardo Fernandez-Moral. See docs for <a href="group__mrpt__pbmap__grp.html" >mrpt-pbmap</a>
13  */
14 
15 #ifndef __MISCELLANEOUS_H
16 #define __MISCELLANEOUS_H
17 
18 #include <mrpt/config.h>
19 #if MRPT_HAS_PCL
20 
21 #include <mrpt/base.h>
22 #include <string>
23 #include <iostream>
24 #include <iterator>
25 #include <vector>
26 #include <pcl/point_types.h>
28 
29 namespace mrpt {
30 namespace pbmap {
31  typedef pcl::PointXYZRGBA PointT;
32 
33  /*!Transform the (x,y,z) coordinates of a PCL point into a Eigen::Vector3f.*/
34  template<class pointPCL>
35  Eigen::Vector3f getVector3fromPointXYZ(pointPCL &pt)
36  {
37  return Eigen::Vector3f(pt.x,pt.y,pt.z);
38  }
39 
40  template <class POINT>
41  inline Eigen::Vector3f diffPoints(const POINT &P1, const POINT &P2)
42  {
43  Eigen::Vector3f diff;
44  diff[0] = P1.x - P2.x;
45  diff[1] = P1.y - P2.y;
46  diff[2] = P1.z - P2.z;
47  return diff;
48  }
49 
50  /*!Compose a 3D-point with a pose.*/
51  template<class dataType>
52  Eigen::Matrix<dataType,3,1> compose(Eigen::Matrix<dataType,4,4> &pose, Eigen::Matrix<dataType,3,1> &point)
53  {
54  Eigen::Matrix<dataType,3,1> transformedPoint = pose.block(0,0,3,3) * point + pose.block(0,3,3,1);
55  return transformedPoint;
56  }
57 
58  /*!Compose two poses.*/
59  template<class dataType>
60  Eigen::Matrix<dataType,4,4> compose(Eigen::Matrix<dataType,4,4> &pose1, Eigen::Matrix<dataType,4,4> &pose2)
61  {
62  Eigen::Matrix<dataType,4,4> transformedPose;
63  transformedPose.block(0,0,3,3) = pose1.block(0,0,3,3) * pose2.block(0,0,3,3);
64  transformedPose.block(0,3,3,1) = pose1.block(0,3,3,1) + pose1.block(0,0,3,3)*pose2.block(0,3,3,1);
65  transformedPose.row(3) << 0,0,0,1;
66  return transformedPose;
67  }
68 
69  /*!Get the pose's inverse.*/
70  template<class dataType>
71  Eigen::Matrix<dataType,4,4> inverse(Eigen::Matrix<dataType,4,4> &pose)
72  {
73  Eigen::Matrix<dataType,4,4> inverse;
74  inverse.block(0,0,3,3) = pose.block(0,0,3,3).transpose();
75  inverse.block(0,3,3,1) = inverse.block(0,0,3,3) * pose.block(0,3,3,1);
76  inverse.row(3) << 0,0,0,1;
77  return inverse;
78  }
79 
80  struct Segment
81  {
82  Segment(PointT p0, PointT p1) :
83  P0(p0), P1(p1)
84  {};
85 
86  PointT P0, P1;
87  };
88 
89  /*! Square of the distance between two segments */
90  float PBMAP_IMPEXP dist3D_Segment_to_Segment2( Segment S1, Segment S2);
91 
92  /*! Check if a point lays inside a convex hull */
93  bool PBMAP_IMPEXP isInHull(PointT &point3D, pcl::PointCloud<PointT>::Ptr hull3D);
94 
95  template<typename dataType>
96  dataType getMode(std::vector<dataType> data, dataType range)
97  {
98  float normalizeConst = 255.0/range;
99  std::vector<int> data2(data.size() );
100  for(size_t i=0; i < data.size(); i++)
101  data2[i] = (int)(data[i]*normalizeConst);
102 
103  std::map<int,int> histogram;
104  for(size_t i=0; i < data2.size(); i++)
105  if(histogram.count(data2[i]) == 0)
106  histogram[data2[i] ] = 1;
107  else
108  histogram[data2[i] ]++;
109 
110  int mode = 0, count = 0;
111  for(std::map<int,int>::iterator bin = histogram.begin(); bin != histogram.end(); bin++)
112  if(bin->second > count)
113  {
114  count = bin->second;
115  mode = bin->first;
116  }
117 
118  return (dataType)mode/normalizeConst;
119  }
120 
121  // Gets the center of a single-mode distribution, it performs variable mean shift
122  template<typename dataType>
123  dataType getHistogramMeanShift(std::vector<dataType> &data, double range, dataType &stdDevHist_out)
124  {
125  // mrpt::utils::CTicTac clock;
126  // clock.Tic();
127  size_t size = data.size();
128  std::vector<dataType> dataTemp = data;
129 
130 // dataType sum = 0;
131 // for(size_t i=0; i < data.size(); i++)
132 // sum += data[i];
133 // dataType meanShift =sum/size;
134 // stdDevHist = mrpt::math::stddev(data);
135 
136 // dataType meanShift;
137  double meanShift, stdDevHist;
138  mrpt::math::meanAndStd(data,meanShift,stdDevHist);
139  double sum = meanShift*data.size();
140 
141  //dataType step = 1;
142  double shift = 1000;
143  int iteration_counter = 0;
144  double convergence = range * 0.001;
145  while(2*dataTemp.size() > size && shift > convergence)
146  {
147  //std::cout << "iteration " << iteration_counter << " Std " << stdDevHist << std::endl;
148 
149  for(typename std::vector<dataType>::iterator it=dataTemp.begin(); it != dataTemp.end(); )
150  {
151  if(fabs(*it - meanShift) > stdDevHist)
152  {
153  sum -= *it;
154  dataTemp.erase(it);
155  }
156  else
157  it++;
158  }
159  double meanUpdated = sum / dataTemp.size();
160  shift = fabs(meanUpdated - meanShift);
161  meanShift = meanUpdated;
162  stdDevHist = mrpt::math::stddev(dataTemp);
163 
164  iteration_counter++;
165  }
166  // std::cout << "Number of iterations: " << iteration_counter << " shift " << shift
167  // << " size " << (float)dataTemp.size() / size << " in " << clock.Tac() * 1e3 << " ms." << std::endl;
168 
169  // stdDevHist = calcStdDev(data, meanShift);
170 
171  stdDevHist_out = static_cast<float>(stdDevHist);
172  return static_cast<float>(meanShift);
173  }
174 
175  /**
176  * Output a vector as a stream that is space separated.
177  * @param os Output stream
178  * @param v Vector to output
179  * @return the stream output
180  */
181  template<class T>
182  std::ostream& operator<<(std::ostream& os, const std::vector<T>& v)
183  {
184  std::copy(v.begin(), v.end(), std::ostream_iterator<T>(os, " "));
185  return os;
186  }
187 
188 } } // End of namespaces
189 
190 #endif
191 
192 #endif
Eigen::Vector3f diffPoints(const POINT &P1, const POINT &P2)
Definition: Miscellaneous.h:41
double stddev(const VECTORLIKE &v, bool unbiased=true)
Computes the standard deviation of a vector.
Scalar * iterator
Definition: eigen_plugins.h:23
pcl::PointXYZRGBA PointT
Definition: PbMapMaker.h:33
vector_double histogram(const CONTAINER &v, double limit_min, double limit_max, size_t number_bins, bool do_normalization=false, vector_double *out_bin_centers=NULL)
Computes the normalized or normal histogram of a sequence of numbers given the number of bins and the...
Eigen::Matrix< dataType, 3, 1 > compose(Eigen::Matrix< dataType, 4, 4 > &pose, Eigen::Matrix< dataType, 3, 1 > &point)
Definition: Miscellaneous.h:52
CONTAINER::Scalar sum(const CONTAINER &v)
Computes the sum of all the elements.
dataType getHistogramMeanShift(std::vector< dataType > &data, double range, dataType &stdDevHist_out)
Eigen::Matrix< dataType, 4, 4 > inverse(Eigen::Matrix< dataType, 4, 4 > &pose)
Definition: Miscellaneous.h:71
Segment(PointT p0, PointT p1)
Definition: Miscellaneous.h:82
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
bool PBMAP_IMPEXP isInHull(PointT &point3D, pcl::PointCloud< PointT >::Ptr hull3D)
void meanAndStd(const VECTORLIKE &v, double &out_mean, double &out_std, bool unbiased=true)
Computes the standard deviation of a vector.
size_t size(const MATRIXLIKE &m, int dim)
Definition: bits.h:46
dataType getMode(std::vector< dataType > data, dataType range)
Definition: Miscellaneous.h:96
float PBMAP_IMPEXP dist3D_Segment_to_Segment2(Segment S1, Segment S2)
pcl::PointXYZRGBA PointT
Definition: Miscellaneous.h:31
Eigen::Vector3f getVector3fromPointXYZ(pointPCL &pt)
Definition: Miscellaneous.h:35



Page generated by Doxygen 1.8.14 for MRPT 1.1.0 SVN: at lun oct 28 00:54:49 CET 2019 Hosted on:
SourceForge.net Logo