Main MRPT website > C++ reference for MRPT 1.5.7
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-2017, 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/utils/types_math.h> // Eigen
22 #include <map>
23 #include <string>
24 #include <iostream>
25 #include <iterator>
26 #include <vector>
27 #include <pcl/point_types.h>
28 #include <pcl/point_cloud.h>
30 #include <mrpt/math.h>
31 
32 namespace mrpt {
33 namespace pbmap {
34  typedef pcl::PointXYZRGBA PointT;
35 
36  /*!Transform the (x,y,z) coordinates of a PCL point into a Eigen::Vector3f.*/
37  template<class pointPCL>
38  Eigen::Vector3f getVector3fromPointXYZ(pointPCL &pt)
39  {
40  return Eigen::Vector3f(pt.x,pt.y,pt.z);
41  }
42 
43  template <class POINT>
44  inline Eigen::Vector3f diffPoints(const POINT &P1, const POINT &P2)
45  {
46  Eigen::Vector3f diff;
47  diff[0] = P1.x - P2.x;
48  diff[1] = P1.y - P2.y;
49  diff[2] = P1.z - P2.z;
50  return diff;
51  }
52 
53  /*!Compose a 3D-point with a pose.*/
54  template<class dataType>
55  Eigen::Matrix<dataType,3,1> compose(Eigen::Matrix<dataType,4,4> &pose, Eigen::Matrix<dataType,3,1> &point)
56  {
57  Eigen::Matrix<dataType,3,1> transformedPoint = pose.block(0,0,3,3) * point + pose.block(0,3,3,1);
58  return transformedPoint;
59  }
60 
61  /*!Compose two poses.*/
62  template<class dataType>
63  Eigen::Matrix<dataType,4,4> compose(Eigen::Matrix<dataType,4,4> &pose1, Eigen::Matrix<dataType,4,4> &pose2)
64  {
65  Eigen::Matrix<dataType,4,4> transformedPose;
66  transformedPose.block(0,0,3,3) = pose1.block(0,0,3,3) * pose2.block(0,0,3,3);
67  transformedPose.block(0,3,3,1) = pose1.block(0,3,3,1) + pose1.block(0,0,3,3)*pose2.block(0,3,3,1);
68  transformedPose.row(3) << 0,0,0,1;
69  return transformedPose;
70  }
71 
72  /*!Get the pose's inverse.*/
73  template<class dataType>
74  Eigen::Matrix<dataType,4,4> inverse(Eigen::Matrix<dataType,4,4> &pose)
75  {
76  Eigen::Matrix<dataType,4,4> inverse;
77  inverse.block(0,0,3,3) = pose.block(0,0,3,3).transpose();
78  inverse.block(0,3,3,1) = -(inverse.block(0,0,3,3) * pose.block(0,3,3,1));
79  inverse.row(3) << 0,0,0,1;
80  return inverse;
81  }
82 
83  struct Segment
84  {
85  Segment(PointT p0, PointT p1) :
86  P0(p0), P1(p1)
87  {};
88 
89  PointT P0, P1;
90  };
91 
92  /*! Square of the distance between two segments */
93  float PBMAP_IMPEXP dist3D_Segment_to_Segment2( Segment S1, Segment S2);
94 
95  /*! Check if a point lays inside a convex hull */
96  bool PBMAP_IMPEXP isInHull(PointT &point3D, pcl::PointCloud<PointT>::Ptr hull3D);
97 
98  template<typename dataType>
99  dataType getMode(std::vector<dataType> data, dataType range)
100  {
101  float normalizeConst = 255.0/range;
102  std::vector<int> data2(data.size() );
103  for(size_t i=0; i < data.size(); i++)
104  data2[i] = (int)(data[i]*normalizeConst);
105 
106  std::map<int,int> histogram;
107  for(size_t i=0; i < data2.size(); i++)
108  if(histogram.count(data2[i]) == 0)
109  histogram[data2[i] ] = 1;
110  else
111  histogram[data2[i] ]++;
112 
113  int mode = 0, count = 0;
114  for(std::map<int,int>::iterator bin = histogram.begin(); bin != histogram.end(); bin++)
115  if(bin->second > count)
116  {
117  count = bin->second;
118  mode = bin->first;
119  }
120 
121  return (dataType)mode/normalizeConst;
122  }
123 
124 // Eigen::Matrix4f& getMoorePenroseInverse(Eigen::Matrix4f &input)
125 // {
126 //// Eigen::Matrix4f generalizedInverse;
127 //// Eigen::JacobiSVD<Eigen::Matrix3f> svd(input);
128 //// stdDevHist = svd.singularValues().maxCoeff() / sqrt(size);
129 // void pinv( MatrixType& pinvmat) const
130 // {
131 //// eigen_assert(m_isInitialized && "SVD is not initialized.");
132 // double pinvtoler=1.e-6; // choose your tolerance wisely!
133 // Eigen::SingularValuesType singularValues_inv = m_singularValues;
134 // for ( long i=0; i<m_workMatrix.cols(); ++i) {
135 // if ( m_singularValues(i) > pinvtoler )
136 // singularValues_inv(i)=1.0/m_singularValues(i);
137 // else singularValues_inv(i)=0;
138 // }
139 // pinvmat= (m_matrixV*singularValues_inv.asDiagonal()*m_matrixU.transpose());
140 // }
141 
142  // Gets the center of a single-mode distribution, it performs variable mean shift
143  template<typename dataType>
144  Eigen::Vector4f getMultiDimMeanShift_color(std::vector<Eigen::Vector4f> &data, dataType &stdDevHist, dataType &concentration)
145  {
146 // cout << "Do meanShift\n";
147 
148  std::vector<Eigen::Vector4f> dataTemp = data;
149  size_t size = data.size();
150 
151 // This one is specific for normalized color
152  Eigen::Vector3f sum = Eigen::Vector3f::Zero();
153  for(size_t i=0; i < data.size(); i++)
154  {
155  sum += data[i].head(3);
156  }
157  Eigen::Vector3f meanShift = sum/size;
158 //cout << "First meanShift " << meanShift.transpose() << endl;
159 
160  Eigen::Matrix3f cov = Eigen::Matrix3f::Zero();
161  for(size_t i=0; i < data.size(); i++)
162  {
163  Eigen::Vector3f diff = data[i].head(3) - meanShift;
164  cov += diff * diff.transpose();
165  }
166 // cov /= size;
167  Eigen::JacobiSVD<Eigen::Matrix3f> svd(cov);
168  stdDevHist = svd.singularValues().maxCoeff() / sqrt((double) size);
169 // stdDevHist = 0.05;
170 
171  double shift = 1000; // Large limit
172  int iteration_counter = 0;
173  double convergence = 0.001;
174  while(2*dataTemp.size() > size && shift > convergence)
175  {
176 // std::cout << "iteration " << iteration_counter << " Std " << stdDevHist << " maxEig " << svd.singularValues().maxCoeff() << std::endl;
177  for(typename std::vector<Eigen::Vector4f>::iterator it=dataTemp.begin(); it != dataTemp.end(); )
178  {
179 // cout << "CHeck\n";
180  Eigen::Vector3f diff = (*it).head(3) - meanShift;
181  if(diff.norm() > stdDevHist)
182  {
183  sum -= (*it).head(3);
184  cov -= diff * diff.transpose();
185  dataTemp.erase(it);
186  }
187  else
188  it++;
189  }
190 // cout << "sum " << sum.transpose() << " newdatasize " << dataTemp.size() << endl;
191  Eigen::Vector3f meanUpdated = sum / dataTemp.size();
192  shift = (meanUpdated - meanShift).norm();
193  meanShift = meanUpdated;
194  svd = Eigen::JacobiSVD<Eigen::Matrix3f>(cov);
195 // stdDevHist = svd.singularValues().maxCoeff() / dataTemp.size();
196  stdDevHist = svd.singularValues().maxCoeff() / sqrt((double) dataTemp.size());
197 
198  iteration_counter++;
199  }
200  // std::cout << "Number of iterations: " << iteration_counter << " shift " << shift
201  // << " size " << (float)dataTemp.size() / size << " in " << clock.Tac() * 1e3 << " ms." << std::endl;
202 
203  // stdDevHist = calcStdDev(data, meanShift);
204 
205  Eigen::Vector4f dominantColor;
206  dominantColor.head(3) = meanShift;
207  float averageIntensity = 0;
208  for(unsigned i=0; i < dataTemp.size(); i++)
209  averageIntensity += dataTemp[i][3];
210  averageIntensity /= dataTemp.size();
211  dominantColor(3) = averageIntensity;
212 
213 // concentration = float(dataTemp.size()) / size;
214  int countFringe05 = 0;
215  for(typename std::vector<Eigen::Vector4f>::iterator it=data.begin(); it != data.end(); it++)
216  if((it->head(3) - meanShift).norm() < 0.05 ) //&& *it(3) - averageIntensity < 0.3)
217  ++countFringe05;
218  concentration = static_cast<dataType>(countFringe05) / data.size();
219 
220  return dominantColor;
221  }
222 
223  // Gets the center of a single-mode distribution, it performs variable mean shift
224  template<typename dataType>
225  dataType getHistogramMeanShift(std::vector<dataType> &data, double range, dataType &stdDevHist_out)//, dataType &concentration05)
226  {
227 // cout << "Do meanShift\n";
228  // mrpt::utils::CTicTac clock;
229  // clock.Tic();
230  size_t size = data.size();
231  std::vector<dataType> dataTemp = data;
232 
233  dataType sum = 0;
234  for(size_t i=0; i < data.size(); i++){
235  sum += data[i];}
236  dataType meanShift =sum/size;
237  dataType stdDevHist = mrpt::math::stddev(data);
238 
239 //// dataType meanShift;
240 // double meanShift, stdDevHist;
241 // mrpt::math::meanAndStd(data,meanShift,stdDevHist);
242 // double sum = meanShift*data.size();
243 //cout << "mean " << meanShift << endl;
244 
245  //dataType step = 1;
246  double shift = 1000;
247  int iteration_counter = 0;
248  double convergence = range * 0.001;
249  while(2*dataTemp.size() > size && shift > convergence)
250  {
251 // std::cout << "iteration " << iteration_counter << " Std " << stdDevHist << std::endl;
252  for(typename std::vector<dataType>::iterator it=dataTemp.begin(); it != dataTemp.end(); )
253  {
254 // cout << "CHeck\n";
255  if(fabs(*it - meanShift) > stdDevHist)
256  {
257  sum -= *it;
258  dataTemp.erase(it);
259  }
260  else
261  it++;
262  }
263 // cout << "sum " << sum << " newdatasize " << dataTemp.size() << endl;
264  double meanUpdated = sum / dataTemp.size();
265  shift = fabs(meanUpdated - meanShift);
266  meanShift = meanUpdated;
267  stdDevHist = mrpt::math::stddev(dataTemp);
268 
269  iteration_counter++;
270  }
271  // std::cout << "Number of iterations: " << iteration_counter << " shift " << shift
272  // << " size " << (float)dataTemp.size() / size << " in " << clock.Tac() * 1e3 << " ms." << std::endl;
273 
274  // stdDevHist = calcStdDev(data, meanShift);
275 
276 // // Calculate concentration05
277 //// stdDevHist_out = float(dataTemp.size()) / size;
278 // int countFringe05 = 0;
279 // for(typename std::vector<dataType>::iterator it=data.begin(); it != data.end(); it++)
280 // if(fabs(*it - meanShift) < 0.05)
281 // ++countFringe05;
282 // concentration05 = static_cast<dataType>(countFringe05) / data.size();
283 
284  return static_cast<dataType>(meanShift);
285  }
286 
287 // // Bhattacharyya histogram distance function
288 // double PBMAP_IMPEXP BhattacharyyaDist(std::vector<float> &hist1, std::vector<float> &hist2)
289 // {
290 // assert(hist1.size() == hist2.size());
291 // double BhattachDist;
292 // double BhattachDist_aux = 0.0;
293 // for(unsigned i=0; i < hist1.size(); i++)
294 // BhattachDist_aux += sqrt(hist1[i]*hist2[i]);
295 //
296 // BhattachDist = sqrt(1 - BhattachDist_aux);
297 //
298 // return BhattachDist;
299 // }
300 
301  /**
302  * Output a vector as a stream that is space separated.
303  * @param os Output stream
304  * @param v Vector to output
305  * @return the stream output
306  */
307  template<class T>
308  std::ostream& operator<<(std::ostream& os, const std::vector<T>& v)
309  {
310  std::copy(v.begin(), v.end(), std::ostream_iterator<T>(os, " "));
311  return os;
312  }
313 
314 } } // End of namespaces
315 
316 #endif
317 
318 #endif
Eigen::Vector4f getMultiDimMeanShift_color(std::vector< Eigen::Vector4f > &data, dataType &stdDevHist, dataType &concentration)
GLsizei range
Definition: glext.h:5281
GLuint GLuint GLsizei count
Definition: glext.h:3512
Eigen::Vector3f diffPoints(const POINT &P1, const POINT &P2)
Definition: Miscellaneous.h:44
double stddev(const VECTORLIKE &v, bool unbiased=true)
Computes the standard deviation of a vector.
std::vector< double > histogram(const CONTAINER &v, double limit_min, double limit_max, size_t number_bins, bool do_normalization=false, std::vector< double > *out_bin_centers=NULL)
Computes the normalized or normal histogram of a sequence of numbers given the number of bins and the...
Scalar * iterator
Definition: eigen_plugins.h:23
pcl::PointXYZRGBA PointT
Definition: PbMapMaker.h:33
Eigen::Matrix< dataType, 3, 1 > compose(Eigen::Matrix< dataType, 4, 4 > &pose, Eigen::Matrix< dataType, 3, 1 > &point)
Definition: Miscellaneous.h:55
Eigen::Matrix< typename MATRIX::Scalar, MATRIX::ColsAtCompileTime, MATRIX::ColsAtCompileTime > cov(const MATRIX &v)
Computes the covariance matrix from a list of samples in an NxM matrix, where each row is a sample...
Definition: ops_matrices.h:135
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:74
Segment(PointT p0, PointT p1)
Definition: Miscellaneous.h:85
GLint mode
Definition: glext.h:5078
const GLdouble * v
Definition: glext.h:3603
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
float PBMAP_IMPEXP dist3D_Segment_to_Segment2(Segment S1, Segment S2)
bool PBMAP_IMPEXP isInHull(PointT &point3D, pcl::PointCloud< PointT >::Ptr hull3D)
dataType getMode(std::vector< dataType > data, dataType range)
Definition: Miscellaneous.h:99
GLsizeiptr size
Definition: glext.h:3779
GLsizei GLsizei GLenum GLenum const GLvoid * data
Definition: glext.h:3520
pcl::PointXYZRGBA PointT
Definition: Miscellaneous.h:34
CONTAINER::Scalar norm(const CONTAINER &v)
Eigen::Vector3f getVector3fromPointXYZ(pointPCL &pt)
Definition: Miscellaneous.h:38



Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019