Example #1
0
 inline Eigen::Matrix<T,R,1> 
 rows_dot_self(const Eigen::Matrix<T,R,C>& x) {
   return x.rowwise().squaredNorm();
 }
Eigen::Matrix<double, 4, Eigen::Dynamic> bb_cluster_confidence(Eigen::Matrix<
		double, 4, 20> const & iBB, Eigen::Matrix<double, 20, 1> const & iConf,
		int nD) {

	double SPACE_THR = 0.5;
	Eigen::VectorXd T;
	Eigen::VectorXd Tbak;
	unsigned int iBBcols = nD;
	Eigen::MatrixXd bdist;
	//Calculates the index of the bb that fits the best
	switch (iBBcols) {
	//0 cols, set indices to 1
	case 0:
		T = Eigen::VectorXd::Zero(1);
		break;
		//1 col, set index to 0;
	case 1:
		T.resize(1);
		T(0) = 0;
		break;
		//2 cols, set indices to zero; if above treshhold to 1
	case 2:
		T = Eigen::VectorXd::Zero(2);
		bdist = bb_distance(iBB);
		if (bdist(0, 0) > SPACE_THR) {
			T(1) = 1;
		}
		break;
		//workaround for clustering.
	default:
		Eigen::Vector4d meanBB = iBB.rowwise().mean();
		int maxIndex = 0;
		double maxDist = 10;
		for (int penis = 0; penis < nD; penis++) {
			Eigen::MatrixXd bd = bb_distance(iBB.col(penis), meanBB);
			//save the shortest distance
			if (bd(0, 0) < maxDist) {
				maxIndex = penis;
				maxDist = bd(0, 0);
			}
		}
		//set the indices to the index of the bounding box with the
		//shortest distance
		T = Eigen::VectorXd::Constant(iBBcols, maxIndex);
		break;
	}
	Eigen::VectorXd idx_cluster;
	idx_cluster.resize(0);
	bool breaker;
	//filter indices that occur twice
	for (int p = 0; p < T.size(); p++) {
		breaker = false;
		for (int j = 0; j < idx_cluster.size(); j++)
			if (idx_cluster(j) == T(p)) {
				breaker = true;
				break;
			}
		if (breaker)
			continue;
		Eigen::VectorXd unibak(idx_cluster.size());
		unibak = idx_cluster;
		idx_cluster.resize(unibak.size() + 1);
		idx_cluster << unibak, T(p);
	}
	int num_clusters = idx_cluster.size();

	Eigen::MatrixXd oBB = Eigen::MatrixXd::Constant(4, num_clusters,
			std::numeric_limits<double>::quiet_NaN());
	Eigen::MatrixXd oConf = Eigen::MatrixXd::Constant(4, num_clusters,
			std::numeric_limits<double>::quiet_NaN());
	Eigen::MatrixXd oSize = Eigen::MatrixXd::Constant(4, num_clusters,
			std::numeric_limits<double>::quiet_NaN());

	for (int k = 0; k < num_clusters; k++) {
		std::vector<int> idx;
		for (int u = 0; u < T.size(); u++)
			if (T(u) == idx_cluster(k))
				idx.push_back(u);

		Eigen::MatrixXd iBBidx(4, idx.size());

		for (unsigned int f = 0; f < idx.size(); f++) {
			iBBidx.col(f) = iBB.block(0, idx[f], 4, 1);
		}
		oBB.col(k) = iBBidx.rowwise().mean();
		Eigen::VectorXd iConfidx(idx.size());
		for (unsigned int f = 0; f < idx.size(); f++)
			iConfidx(f) = iConf(idx[f]);
		//save information how valid a certain bounding box is
		oConf(0, k) = iConfidx.mean();
		oSize(0, k) = idx.size();
	}
	Eigen::MatrixXd ret(4, 3 * num_clusters);
	ret << oBB, oConf, oSize;
	return ret;
}