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

 Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 7d5e6d718 Fri Aug 24 01:51:28 2018 +0200 at lun nov 2 08:35:50 CET 2020