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;
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;
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));
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);
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;
111 histogram[data2[i] ]++;
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)
121 return (dataType)mode/normalizeConst;
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)
177 for(
typename std::vector<Eigen::Vector4f>::iterator it=dataTemp.begin(); it != dataTemp.end(); )
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;
215 for(
typename std::vector<Eigen::Vector4f>::iterator it=data.begin(); it != data.end(); it++)
216 if((it->head(3) - meanShift).norm() < 0.05 )
218 concentration =
static_cast<dataType
>(countFringe05) / data.size();
220 return dominantColor;
230 size_t size = data.size();
231 std::vector<dataType> dataTemp = data;
234 for(
size_t i=0; i < data.size(); i++){
236 dataType meanShift =sum/size;
247 int iteration_counter = 0;
248 double convergence = range * 0.001;
249 while(2*dataTemp.size() > size && shift > convergence)
252 for(
typename std::vector<dataType>::iterator it=dataTemp.begin(); it != dataTemp.end(); )
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);
Eigen::Matrix< dataType, 3, 1 > compose(Eigen::Matrix< dataType, 4, 4 > &pose, Eigen::Matrix< dataType, 3, 1 > &point)