Main MRPT website > C++ reference for MRPT 1.4.0
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-2016, 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
32namespace mrpt {
33namespace 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 {
86 P0(p0), P1(p1)
87 {};
88
90 };
91
92 /*! Square of the distance between two segments */
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
double stddev(const VECTORLIKE &v, bool unbiased=true)
Computes the standard deviation of a vector.
dataType getMode(std::vector< dataType > data, dataType range)
Definition: Miscellaneous.h:99
bool PBMAP_IMPEXP isInHull(PointT &point3D, pcl::PointCloud< PointT >::Ptr hull3D)
float PBMAP_IMPEXP dist3D_Segment_to_Segment2(Segment S1, Segment S2)
Eigen::Vector3f diffPoints(const POINT &P1, const POINT &P2)
Definition: Miscellaneous.h:44
Eigen::Matrix< dataType, 3, 1 > compose(Eigen::Matrix< dataType, 4, 4 > &pose, Eigen::Matrix< dataType, 3, 1 > &point)
Definition: Miscellaneous.h:55
pcl::PointXYZRGBA PointT
Definition: Miscellaneous.h:34
dataType getHistogramMeanShift(std::vector< dataType > &data, double range, dataType &stdDevHist_out)
Eigen::Vector3f getVector3fromPointXYZ(pointPCL &pt)
Definition: Miscellaneous.h:38
Eigen::Matrix< dataType, 4, 4 > inverse(Eigen::Matrix< dataType, 4, 4 > &pose)
Definition: Miscellaneous.h:74
std::ostream & operator<<(std::ostream &os, const std::vector< T > &v)
Output a vector as a stream that is space separated.
Eigen::Vector4f getMultiDimMeanShift_color(std::vector< Eigen::Vector4f > &data, dataType &stdDevHist, dataType &concentration)
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Segment(PointT p0, PointT p1)
Definition: Miscellaneous.h:85



Page generated by Doxygen 1.9.6 for MRPT 1.4.0 SVN: at Wed Mar 22 04:35:51 UTC 2023