15 #ifndef __MISCELLANEOUS_H
16 #define __MISCELLANEOUS_H
18 #include <mrpt/config.h>
27 #include <pcl/point_types.h>
28 #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>
55 Eigen::Matrix<dataType,3,1>
compose(Eigen::Matrix<dataType,4,4> &pose, Eigen::Matrix<dataType,3,1> &point)
57 Eigen::Matrix<dataType,3,1> transformedPoint = pose.block(0,0,3,3) * point + pose.block(0,3,3,1);
58 return transformedPoint;
62 template<
class dataType>
63 Eigen::Matrix<dataType,4,4>
compose(Eigen::Matrix<dataType,4,4> &pose1, Eigen::Matrix<dataType,4,4> &pose2)
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;
73 template<
class dataType>
74 Eigen::Matrix<dataType,4,4>
inverse(Eigen::Matrix<dataType,4,4> &pose)
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));
98 template<
typename dataType>
99 dataType
getMode(std::vector<dataType> data, dataType range)
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);
107 for(
size_t i=0; i < data2.size(); i++)
113 int mode = 0, count = 0;
115 if(bin->second > count)
121 return (dataType)mode/normalizeConst;
143 template<
typename dataType>
148 std::vector<Eigen::Vector4f> dataTemp = data;
149 size_t size = data.size();
152 Eigen::Vector3f
sum = Eigen::Vector3f::Zero();
153 for(
size_t i=0; i < data.size(); i++)
155 sum += data[i].head(3);
157 Eigen::Vector3f meanShift =
sum/
size;
160 Eigen::Matrix3f
cov = Eigen::Matrix3f::Zero();
161 for(
size_t i=0; i < data.size(); i++)
163 Eigen::Vector3f diff = data[i].head(3) - meanShift;
164 cov += diff * diff.transpose();
167 Eigen::JacobiSVD<Eigen::Matrix3f> svd(
cov);
168 stdDevHist = svd.singularValues().maxCoeff() / sqrt((
double)
size);
172 int iteration_counter = 0;
173 double convergence = 0.001;
174 while(2*dataTemp.size() >
size && shift > convergence)
180 Eigen::Vector3f diff = (*it).head(3) - meanShift;
181 if(diff.norm() > stdDevHist)
183 sum -= (*it).head(3);
184 cov -= diff * diff.transpose();
191 Eigen::Vector3f meanUpdated =
sum / dataTemp.size();
192 shift = (meanUpdated - meanShift).
norm();
193 meanShift = meanUpdated;
194 svd = Eigen::JacobiSVD<Eigen::Matrix3f>(
cov);
196 stdDevHist = svd.singularValues().maxCoeff() / sqrt((
double) dataTemp.size());
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;
214 int countFringe05 = 0;
216 if((it->head(3) - meanShift).
norm() < 0.05 )
218 concentration = static_cast<dataType>(countFringe05) / data.size();
220 return dominantColor;
224 template<
typename dataType>
230 size_t size = data.size();
231 std::vector<dataType> dataTemp = data;
234 for(
size_t i=0; i < data.size(); i++){
247 int iteration_counter = 0;
248 double convergence = range * 0.001;
249 while(2*dataTemp.size() >
size && shift > convergence)
255 if(fabs(*it - meanShift) > stdDevHist)
264 double meanUpdated =
sum / dataTemp.size();
265 shift = fabs(meanUpdated - meanShift);
266 meanShift = meanUpdated;
284 return static_cast<dataType>(meanShift);
308 std::ostream&
operator<<(std::ostream& os,
const std::vector<T>& v)
310 std::copy(v.begin(), v.end(), std::ostream_iterator<T>(os,
" "));