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

//procustres
inline void ICP::minimize(const Pair& init_f){
	
	Eigen::Vector3d centroide_model(0.0,0.0,0.0), centroide_data(0.0,0.0,0.0);
	Eigen::Matrix3d M;
	
	unsigned int N = data_indices.size() + init_f.size();
	
	Eigen::MatrixXd model(3,N);
	Eigen::MatrixXd  data(3,N);
	
		
	//calcula os centroides
	int k=0;
	for(unsigned int i=0; i<N; i++){
		
		if (i<data_indices.size()){
			model(0,i) = cloud_m->points[ model_indices[i] ].x;
			model(1,i) = cloud_m->points[ model_indices[i] ].y;
			model(2,i) = cloud_m->points[ model_indices[i] ].z;
		
			data(0,i) = cloud_d->points[ data_indices[i] ].x*T(0,0) + cloud_d->points[ data_indices[i] ].y*T(0,1) + cloud_d->points[ data_indices[i] ].z*T(0,2) + T(0,3);
			data(1,i) = cloud_d->points[ data_indices[i] ].x*T(1,0) + cloud_d->points[ data_indices[i] ].y*T(1,1) + cloud_d->points[ data_indices[i] ].z*T(1,2) + T(1,3);
			data(2,i) = cloud_d->points[ data_indices[i] ].x*T(2,0) + cloud_d->points[ data_indices[i] ].y*T(2,1) + cloud_d->points[ data_indices[i] ].z*T(2,2) + T(2,3);
		}else{

			model(0,i) = cloud_m->points[ init_f[k].first ].x;
			model(1,i) = cloud_m->points[ init_f[k].first ].y;
			model(2,i) = cloud_m->points[ init_f[k].first ].z;
		
			data(0,i) = cloud_d->points[ init_f[k].second ].x*T(0,0) + cloud_d->points[ init_f[k].second ].y*T(0,1) + cloud_d->points[ init_f[k].second ].z*T(0,2) + T(0,3);
			data(1,i) = cloud_d->points[ init_f[k].second ].x*T(1,0) + cloud_d->points[ init_f[k].second ].y*T(1,1) + cloud_d->points[ init_f[k].second ].z*T(1,2) + T(1,3);
			data(2,i) = cloud_d->points[ init_f[k].second ].x*T(2,0) + cloud_d->points[ init_f[k].second ].y*T(2,1) + cloud_d->points[ init_f[k].second ].z*T(2,2) + T(2,3);	
			k++;
		}
		
		centroide_model += model.block(0,i,3,1);
		centroide_data  +=  data.block(0,i,3,1);
	}
	
	centroide_data = centroide_data/N;
	centroide_model = centroide_model/N;
	
	
  //subtrai os centroides aos dados
	for (unsigned int i=0; i<N; i++){
		model.block(0,i,3,1) -= centroide_model;
		data.block(0,i,3,1) -= centroide_data;
	}
	
	
	//Determina a transformacao
	M = data*model.transpose();
	
	Eigen::JacobiSVD<Eigen::Matrix3d> svd(M, Eigen::ComputeFullU | Eigen::ComputeFullV);
	
	Eigen::Matrix3d U = svd.matrixU();
	Eigen::Matrix3d V = svd.matrixV();
	
	if (U.determinant()*V.determinant()<0) {
		for (int i=0; i<3; ++i) 
			V(i,2) *=-1;
	}
	
		
	Eigen::Matrix3d r = V * U.transpose();
	Eigen::Vector3d t = centroide_model - r * centroide_data;
	
	//~ T.block<3,3>(0,0) =  r*T.block<3,3>(0,0);
	//~ T.block<3,1>(0,3) += t; 
	T.block<3,1>(0,3) = T.block<3,3>(0,0)*t + T.block<3,1>(0,3); 
	T.block<3,3>(0,0) = T.block<3,3>(0,0)*r;
	
	
} 

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);
	
}