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



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019