当前位置: 首页>>代码示例>>C++>>正文


C++ Pair::resize方法代码示例

本文整理汇总了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);
	
}
开发者ID:Modasshir,项目名称:socrob-ros-pkg,代码行数:96,代码来源:ICPBlock.hpp


注:本文中的Pair::resize方法示例由纯净天空整理自Github/MSDocs等开源代码及文档管理平台,相关代码片段筛选自各路编程大神贡献的开源项目,源码版权归原作者所有,传播和使用请参考对应项目的License;未经允许,请勿转载。