本文整理汇总了C++中Pair::resize方法的典型用法代码示例。如果您正苦于以下问题:C++ Pair::resize方法的具体用法?C++ Pair::resize怎么用?C++ Pair::resize使用的例子?那么, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类Pair
的用法示例。
在下文中一共展示了Pair::resize方法的1个代码示例,这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞,您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: UpdateAndReject
inline void ICP::UpdateAndReject(Pair& init_f){
double sigma=0.0;
double mean=0.0;
unsigned int N = data_indices.size() + init_f.size();
Eigen::Vector4d point_s(0.0,0.0,0.0,1.0);
Eigen::Vector4d point_d(0.0,0.0,0.0,1.0);
std::vector<double> dists(N);
//compute mean
unsigned int k=0;
for (unsigned int i=0 ; i < N; i++){
if(i<data_indices.size()){
point_s(0) = cloud_m->points[ model_indices[i] ].x;
point_s(1) = cloud_m->points[ model_indices[i] ].y;
point_s(2) = cloud_m->points[ model_indices[i] ].z;
point_d(0) = cloud_d->points[ data_indices[i] ].x;
point_d(1) = cloud_d->points[ data_indices[i] ].y;
point_d(2) = cloud_d->points[ data_indices[i] ].z;
}else{
point_s(0) = cloud_m->points[ init_f[k].first ].x;
point_s(1) = cloud_m->points[ init_f[k].first ].y;
point_s(2) = cloud_m->points[ init_f[k].first ].z;
point_d(0) = cloud_d->points[ init_f[k].second ].x;
point_d(1) = cloud_d->points[ init_f[k].second ].y;
point_d(2) = cloud_d->points[ init_f[k].second ].z;
k++;
}
point_d = T*point_d;
dists[i]= (point_d - point_s).norm();
mean = mean + dists[i];
}
mean = mean/N;
//compute standart diviation
for (unsigned int i=0; i < N; i++){
sigma = sigma + (dists[i]-mean)*(dists[i]-mean);
}
sigma = sigma/N;
sigma = sqrt(sigma);
//How good is the registration
if (mean<D) //very good
Dmax = mean + 3*sigma;
else if (mean<3*D) //good
Dmax = mean + 2*sigma;
else if (mean<6*D) //bad
Dmax = mean + sigma;
else { //very bad
std::vector<double> dists2 = dists;
sort (dists2.begin(), dists2.end());
if (dists2.size() % 2 == 0) {
Dmax = (dists2[dists2.size()/2-1] + dists2[dists2.size()/2]) / 2.0;
}else {
Dmax = dists2[dists2.size()/2];
}
}
//Update the maching
k=0;
unsigned int i=0;
for (i=0 ; i <data_indices.size() ; i++){
if (dists[i] < Dmax){
model_indices[k] = model_indices[i];
data_indices[k] = data_indices[i];
k++;
}
}
model_indices.resize(k);
data_indices.resize(k);
k=0;
unsigned int j,l;
for (j=i, l=0; j<N ; j++,l++){
if (dists[j] < Dmax){
init_f[k]= init_f[l];
k++;
}
}
if(k!=init_f.size())
init_f.resize(k);
}