MRPT  1.9.9
Miscellaneous.h
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2019, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://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 #pragma once
17 
18 #include <mrpt/config.h>
19 #if MRPT_HAS_PCL
20 
21 #include <Eigen/Dense>
22 
24 #include <pcl/point_cloud.h>
25 #include <pcl/point_types.h>
26 #include <iostream>
27 #include <iterator>
28 #include <map>
29 #include <string>
30 #include <vector>
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>
57 {
58  Eigen::Matrix<dataType, 3, 1> transformedPoint =
59  pose.template block<3, 3>(0, 0) * point +
60  pose.template block<3, 1>(0, 3);
61  return transformedPoint;
62 }
63 
64 /*!Compose two poses.*/
65 template <class dataType>
68 {
69  Eigen::Matrix<dataType, 4, 4> transformedPose;
70  transformedPose.template block<3, 3>(0, 0) =
71  pose1.template block<3, 3>(0, 0) * pose2.template block<3, 3>(0, 0);
72  transformedPose.block(0, 3, 3, 1) =
73  pose1.block(0, 3, 3, 1) +
74  pose1.template block<3, 3>(0, 0) * pose2.block(0, 3, 3, 1);
75  transformedPose.row(3) << 0, 0, 0, 1;
76  return transformedPose;
77 }
78 
79 /*!Get the pose's inverse.*/
80 template <class dataType>
82 {
84  inverse.template block<3, 3>(0, 0) =
85  pose.template block<3, 3>(0, 0).transpose();
86  inverse.block(0, 3, 3, 1) =
87  -(inverse.template block<3, 3>(0, 0) * 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 (auto bin = histogram.begin(); bin != histogram.end(); bin++)
122  if (bin->second > count)
123  {
124  count = bin->second;
125  mode = bin->first;
126  }
127 
128  return (dataType)mode / normalizeConst;
129 }
130 
131 // Eigen::Matrix4f& getMoorePenroseInverse(Eigen::Matrix4f &input)
132 // {
133 //// Eigen::Matrix4f generalizedInverse;
134 //// Eigen::JacobiSVD<Eigen::Matrix3f> svd(input);
135 //// stdDevHist = svd.singularValues().maxCoeff() / sqrt(size);
136 // void pinv( MatrixType& pinvmat) const
137 // {
138 //// eigen_assert(m_isInitialized && "SVD is not initialized.");
139 // double pinvtoler=1.e-6; // choose your tolerance wisely!
140 // Eigen::SingularValuesType singularValues_inv = m_singularValues;
141 // for ( long i=0; i<m_workMatrix.cols(); ++i) {
142 // if ( m_singularValues(i) > pinvtoler )
143 // singularValues_inv(i)=1.0/m_singularValues(i);
144 // else singularValues_inv(i)=0;
145 // }
146 // pinvmat=
147 // (m_matrixV*singularValues_inv.asDiagonal()*m_matrixU.transpose());
148 // }
149 
150 // Gets the center of a single-mode distribution, it performs variable mean
151 // shift
152 template <typename dataType>
154  std::vector<Eigen::Vector4f>& data, dataType& stdDevHist,
155  dataType& concentration)
156 {
157  // cout << "Do meanShift\n";
158 
159  std::vector<Eigen::Vector4f> dataTemp = data;
160  size_t size = data.size();
161 
162  // This one is specific for normalized color
163  Eigen::Vector3f sum = Eigen::Vector3f::Zero();
164  for (size_t i = 0; i < data.size(); i++)
165  {
166  sum += data[i].head(3);
167  }
168  Eigen::Vector3f meanShift = sum / size;
169  // cout << "First meanShift " << meanShift.transpose() << endl;
170 
171  Eigen::Matrix3f cov = Eigen::Matrix3f::Zero();
172  for (size_t i = 0; i < data.size(); i++)
173  {
174  Eigen::Vector3f diff = data[i].head(3) - meanShift;
175  cov += diff * diff.transpose();
176  }
177  // cov /= size;
178  Eigen::JacobiSVD<Eigen::Matrix3f> svd(cov);
179  stdDevHist = svd.singularValues().maxCoeff() / sqrt((double)size);
180  // stdDevHist = 0.05;
181 
182  double shift = 1000; // Large limit
183  int iteration_counter = 0;
184  double convergence = 0.001;
185  while (2 * dataTemp.size() > size && shift > convergence)
186  {
187  // std::cout << "iteration " << iteration_counter << " Std " <<
188  // stdDevHist << " maxEig " << svd.singularValues().maxCoeff() <<
189  // std::endl;
190  for (auto it = dataTemp.begin(); it != dataTemp.end();)
191  {
192  // cout << "CHeck\n";
193  Eigen::Vector3f diff = (*it).head(3) - meanShift;
194  if (diff.norm() > stdDevHist)
195  {
196  sum -= (*it).head(3);
197  cov -= diff * diff.transpose();
198  dataTemp.erase(it);
199  }
200  else
201  it++;
202  }
203  // cout << "sum " << sum.transpose() << " newdatasize " <<
204  // dataTemp.size() << endl;
205  Eigen::Vector3f meanUpdated = sum / dataTemp.size();
206  shift = (meanUpdated - meanShift).norm();
207  meanShift = meanUpdated;
208  svd = Eigen::JacobiSVD<Eigen::Matrix3f>(cov);
209  // stdDevHist = svd.singularValues().maxCoeff() / dataTemp.size();
210  stdDevHist =
211  svd.singularValues().maxCoeff() / sqrt((double)dataTemp.size());
212 
213  iteration_counter++;
214  }
215  // std::cout << "Number of iterations: " << iteration_counter << " shift "
216  // << shift
217  // << " size " << (float)dataTemp.size() / size << " in " <<
218  // clock.Tac() * 1e3 << " ms." << std::endl;
219 
220  // stdDevHist = calcStdDev(data, meanShift);
221 
222  Eigen::Vector4f dominantColor;
223  dominantColor.head(3) = meanShift;
224  float averageIntensity = 0;
225  for (unsigned i = 0; i < dataTemp.size(); i++)
226  averageIntensity += dataTemp[i][3];
227  averageIntensity /= dataTemp.size();
228  dominantColor(3) = averageIntensity;
229 
230  // concentration = float(dataTemp.size()) / size;
231  int countFringe05 = 0;
232  for (auto it = data.begin(); it != data.end(); it++)
233  if ((it->head(3) - meanShift).norm() <
234  0.05) //&& *it(3) - averageIntensity < 0.3)
235  ++countFringe05;
236  concentration = static_cast<dataType>(countFringe05) / data.size();
237 
238  return dominantColor;
239 }
240 
241 // Gets the center of a single-mode distribution, it performs variable mean
242 // shift
243 template <typename dataType>
245  std::vector<dataType>& data, double range,
246  dataType& stdDevHist_out) //, dataType &concentration05)
247 {
248  // cout << "Do meanShift\n";
249  // mrpt::system::CTicTac clock;
250  // clock.Tic();
251  size_t size = data.size();
252  std::vector<dataType> dataTemp = data;
253 
254  dataType sum = 0;
255  for (size_t i = 0; i < data.size(); i++)
256  {
257  sum += data[i];
258  }
259  dataType meanShift = sum / size;
260  dataType stdDevHist = mrpt::math::stddev(data);
261 
262  //// dataType meanShift;
263  // double meanShift, stdDevHist;
264  // mrpt::math::meanAndStd(data,meanShift,stdDevHist);
265  // double sum = meanShift*data.size();
266  // cout << "mean " << meanShift << endl;
267 
268  // dataType step = 1;
269  double shift = 1000;
270  int iteration_counter = 0;
271  double convergence = range * 0.001;
272  while (2 * dataTemp.size() > size && shift > convergence)
273  {
274  // std::cout << "iteration " << iteration_counter << " Std " <<
275  // stdDevHist << std::endl;
276  for (auto it = dataTemp.begin(); it != dataTemp.end();)
277  {
278  // cout << "CHeck\n";
279  if (fabs(*it - meanShift) > stdDevHist)
280  {
281  sum -= *it;
282  dataTemp.erase(it);
283  }
284  else
285  it++;
286  }
287  // cout << "sum " << sum << " newdatasize " << dataTemp.size() <<
288  // endl;
289  double meanUpdated = sum / dataTemp.size();
290  shift = fabs(meanUpdated - meanShift);
291  meanShift = meanUpdated;
292  stdDevHist = mrpt::math::stddev(dataTemp);
293 
294  iteration_counter++;
295  }
296  // std::cout << "Number of iterations: " << iteration_counter << " shift "
297  // << shift
298  // << " size " << (float)dataTemp.size() / size << " in " <<
299  // clock.Tac() * 1e3 << " ms." << std::endl;
300 
301  // stdDevHist = calcStdDev(data, meanShift);
302 
303  // // Calculate concentration05
304  //// stdDevHist_out = float(dataTemp.size()) / size;
305  // int countFringe05 = 0;
306  // for(typename std::vector<dataType>::iterator it=data.begin(); it !=
307  // data.end(); it++)
308  // if(fabs(*it - meanShift) < 0.05)
309  // ++countFringe05;
310  // concentration05 = static_cast<dataType>(countFringe05) / data.size();
311 
312  return static_cast<dataType>(meanShift);
313 }
314 
315 // // Bhattacharyya histogram distance function
316 // double BhattacharyyaDist(std::vector<float> &hist1,
317 // std::vector<float> &hist2)
318 // {
319 // assert(hist1.size() == hist2.size());
320 // double BhattachDist;
321 // double BhattachDist_aux = 0.0;
322 // for(unsigned i=0; i < hist1.size(); i++)
323 // BhattachDist_aux += sqrt(hist1[i]*hist2[i]);
324 //
325 // BhattachDist = sqrt(1 - BhattachDist_aux);
326 //
327 // return BhattachDist;
328 // }
329 
330 /**
331  * Output a vector as a stream that is space separated.
332  * @param os Output stream
333  * @param v Vector to output
334  * @return the stream output
335  */
336 template <class T>
337 std::ostream& operator<<(std::ostream& os, const std::vector<T>& v)
338 {
339  std::copy(v.begin(), v.end(), std::ostream_iterator<T>(os, " "));
340  return os;
341 }
342 } // namespace mrpt::pbmap
343 
344 #endif
Eigen::Vector4f getMultiDimMeanShift_color(std::vector< Eigen::Vector4f > &data, dataType &stdDevHist, dataType &concentration)
GLsizei range
Definition: glext.h:5993
GLuint GLuint GLsizei count
Definition: glext.h:3532
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:31
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:81
dataType getHistogramMeanShift(std::vector< dataType > &data, double range, dataType &stdDevHist_out)
Segment(PointT p0, PointT p1)
Definition: Miscellaneous.h:94
CMatrixDouble 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
GLint mode
Definition: glext.h:5753
const GLdouble * v
Definition: glext.h:3684
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
GLsizeiptr size
Definition: glext.h:3934
GLsizei GLsizei GLenum GLenum const GLvoid * data
Definition: glext.h:3550
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: ce1a28c9f Fri Aug 23 08:02:09 2019 +0200 at vie ago 23 08:10:11 CEST 2019