C++ DataPoints::swapCols方法代码示例

//.........这里部分代码省略.........
			// Compute the covariance matrix Cov = FF'
			cov = F * F.transpose(); 
		};
		
	Matrix66 covariance;
	computeCovariance(covariance);
	
	Eigen::EigenSolver<Matrix66> solver(covariance);		
	const Matrix66  eigenVe = solver.eigenvectors().real();
	const Vector6   eigenVa = solver.eigenvalues().real();
	
	///---- Part B
	//B.1 - Compute the v-6 for each candidate
	std::vector<Vector6, Eigen::aligned_allocator<Vector6>> v; // v[i] = [(pi-c) x ni ; ni ]'
	v.resize(nbCandidates);

	for(std::size_t i = 0; i < nbCandidates; ++i)
	{
		const Vector3 p = cloud.features.col(candidates[i]).head(3) - center; // pi-c
		const Vector3 ni = normals.col(candidates[i]).head(3);
		
		v[i].template block<3, 1>(0, 0) = (1. / Lnorm) * p.cross(ni);
		v[i].template block<3, 1>(3, 0) = ni;
	}
	
	//B.2 - Compute the 6 sorted list based on dot product (vi . Xk) = magnitude, with Xk the kth-EigenVector
	std::vector<std::list<std::pair<int, T>>> L; // contain list of pair (index, magnitude) contribution to the eigens vectors
	L.resize(6);
	
	//sort by decreasing magnitude
	auto comp = [](const std::pair<int, T>& p1, const std::pair<int, T>& p2) -> bool {
			return p1.second > p2.second;
		};
	
	for(std::size_t k = 0; k < 6; ++k)
	{		
		for(std::size_t i = 0; i < nbCandidates; ++i )
		{
			L[k].push_back(std::make_pair(i, std::fabs( v[i].dot(eigenVe.template block<6,1>(0, k)) )));
		}
		
		L[k].sort(comp);
	}	
	
	std::vector<T> t(6, T(0.)); //contains the sums of squared magnitudes
	std::vector<bool> sampledPoints(nbCandidates, false); //maintain flag to avoid resampling the same point in an other list 
	
	///Add point iteratively till we got the desired number of point
	for(std::size_t i = 0; i < nbSample; ++i)
	{
		//B.3 - Equally constrained all eigen vectors		
		// magnitude contribute to t_i where i is the indice of th least contrained eigen vector
		
		//Find least constrained vector
		std::size_t k = 0;
		for (std::size_t i = 0; i < 6; ++i)
		{
			if (t[k] > t[i])
				k = i;
		}
		// Add the point from the top of the list corresponding to the dimension to the set of samples
		while(sampledPoints[L[k].front().first])
			L[k].pop_front(); //remove already sampled point
		
		//Get index to keep
		const std::size_t idToKeep = static_cast<std::size_t>(L[k].front().first);
		L[k].pop_front();
			
		sampledPoints[idToKeep] = true; //set flag to avoid resampling
				
		//B.4 - Update the running total
		for (std::size_t k = 0; k < 6; ++k)
		{
			const T magnitude = v[idToKeep].dot(eigenVe.template block<6, 1>(0, k));
			t[k] += (magnitude * magnitude);
		}
		
		keepIndexes[i] = candidates[idToKeep];
	}

	//TODO: evaluate performances between this solution and sorting the indexes
	// We build map of (old index to new index), in case we sample pts at the begining of the pointcloud
	std::unordered_map<std::size_t, std::size_t> mapidx;
	std::size_t idx = 0;
	
	///(4) Sample the point cloud
	for(std::size_t id : keepIndexes)
	{
		//retrieve index from lookup table if sampling in already switched element
		if(id<idx)
			id = mapidx[id];
		//Switch columns id and idx
		cloud.swapCols(idx, id);	
		//Maintain new index position	
		mapidx[idx] = id;
		//Update index
		++idx;
	}
	cloud.conservativeResize(nbSample);
}