16 #ifndef __MISCELLANEOUS_H 17 #define __MISCELLANEOUS_H 19 #include <mrpt/config.h> 28 #include <pcl/point_types.h> 29 #include <pcl/point_cloud.h> 37 template <
class po
intPCL>
40 return Eigen::Vector3f(pt.x, pt.y, pt.z);
43 template <
class POINT>
44 inline Eigen::Vector3f
diffPoints(
const POINT& P1,
const POINT& P2)
47 diff[0] = P1.x - P2.x;
48 diff[1] = P1.y - P2.y;
49 diff[2] = P1.z - P2.z;
54 template <
class dataType>
56 Eigen::Matrix<dataType, 4, 4>& pose, Eigen::Matrix<dataType, 3, 1>& point)
58 Eigen::Matrix<dataType, 3, 1> transformedPoint =
59 pose.block(0, 0, 3, 3) * point + pose.block(0, 3, 3, 1);
60 return transformedPoint;
64 template <
class dataType>
66 Eigen::Matrix<dataType, 4, 4>& pose1, Eigen::Matrix<dataType, 4, 4>& pose2)
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;
79 template <
class dataType>
80 Eigen::Matrix<dataType, 4, 4>
inverse(Eigen::Matrix<dataType, 4, 4>& pose)
82 Eigen::Matrix<dataType, 4, 4>
inverse;
83 inverse.block(0, 0, 3, 3) = pose.block(0, 0, 3, 3).transpose();
85 -(
inverse.block(0, 0, 3, 3) * pose.block(0, 3, 3, 1));
101 bool isInHull(
PointT& point3D, pcl::PointCloud<PointT>::Ptr hull3D);
103 template <
typename dataType>
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);
112 for (
size_t i = 0; i < data2.size(); i++)
121 if (bin->second >
count)
127 return (dataType)
mode / normalizeConst;
151 template <
typename dataType>
153 std::vector<Eigen::Vector4f>&
data, dataType& stdDevHist,
154 dataType& concentration)
158 std::vector<Eigen::Vector4f> dataTemp =
data;
162 Eigen::Vector3f
sum = Eigen::Vector3f::Zero();
163 for (
size_t i = 0; i <
data.size(); i++)
167 Eigen::Vector3f meanShift =
sum /
size;
170 Eigen::Matrix3f
cov = Eigen::Matrix3f::Zero();
171 for (
size_t i = 0; i <
data.size(); i++)
173 Eigen::Vector3f diff =
data[i].head(3) - meanShift;
174 cov += diff * diff.transpose();
177 Eigen::JacobiSVD<Eigen::Matrix3f> svd(
cov);
178 stdDevHist = svd.singularValues().maxCoeff() / sqrt((
double)
size);
182 int iteration_counter = 0;
183 double convergence = 0.001;
184 while (2 * dataTemp.size() >
size && shift > convergence)
191 it != dataTemp.end();)
194 Eigen::Vector3f diff = (*it).head(3) - meanShift;
195 if (diff.norm() > stdDevHist)
197 sum -= (*it).head(3);
198 cov -= diff * diff.transpose();
206 Eigen::Vector3f meanUpdated =
sum / dataTemp.size();
207 shift = (meanUpdated - meanShift).
norm();
208 meanShift = meanUpdated;
209 svd = Eigen::JacobiSVD<Eigen::Matrix3f>(
cov);
212 svd.singularValues().maxCoeff() / sqrt((
double)dataTemp.size());
223 Eigen::Vector4f dominantColor;
224 dominantColor.head(3) = meanShift;
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;
232 int countFringe05 = 0;
234 it !=
data.end(); it++)
235 if ((it->head(3) - meanShift).
norm() <
238 concentration =
static_cast<dataType
>(countFringe05) /
data.size();
240 return dominantColor;
245 template <
typename dataType>
247 std::vector<dataType>&
data,
double range,
248 dataType& stdDevHist_out)
254 std::vector<dataType> dataTemp =
data;
257 for (
size_t i = 0; i <
data.size(); i++)
261 dataType meanShift =
sum /
size;
272 int iteration_counter = 0;
273 double convergence =
range * 0.001;
274 while (2 * dataTemp.size() >
size && shift > convergence)
279 it != dataTemp.end();)
282 if (fabs(*it - meanShift) > stdDevHist)
292 double meanUpdated =
sum / dataTemp.size();
293 shift = fabs(meanUpdated - meanShift);
294 meanShift = meanUpdated;
315 return static_cast<dataType
>(meanShift);
340 std::ostream& operator<<(std::ostream& os, const std::vector<T>&
v)
342 std::copy(
v.begin(),
v.end(), std::ostream_iterator<T>(os,
" "));
Eigen::Vector4f getMultiDimMeanShift_color(std::vector< Eigen::Vector4f > &data, dataType &stdDevHist, dataType &concentration)
GLuint GLuint GLsizei count
Eigen::Vector3f diffPoints(const POINT &P1, const POINT &P2)
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...
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)
dataType getHistogramMeanShift(std::vector< dataType > &data, double range, dataType &stdDevHist_out)
Segment(PointT p0, PointT p1)
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...
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...
GLsizei GLsizei GLenum GLenum const GLvoid * data
CONTAINER::Scalar norm(const CONTAINER &v)
Eigen::Vector3f getVector3fromPointXYZ(pointPCL &pt)
Eigen::Matrix< dataType, 3, 1 > compose(Eigen::Matrix< dataType, 4, 4 > &pose, Eigen::Matrix< dataType, 3, 1 > &point)