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