void Reconstruction::ReconstructPlanarUnconstrIter(const mat& matchesInit, LaplacianMesh& resMesh, uvec& inlierMatchIdxs)
{
// Input check
if (matchesInit.n_rows == 0)
{
inlierMatchIdxs.resize(0);
return;
}
Timer timer;
double wr = this->wrInit; // Currently used regularization weight
double radius = this->radiusInit; // Currently used radius of the estimator
vec reprojErrors; // Reprojection errors
// First, we need to build the correspondent matrix with all given matches to avoid re-computation
this->buildCorrespondenceMatrix(matchesInit);
// Then compute MPinit. Function reconstructPlanarUnconstr() will use part of MPinit w.r.t currently used matches
this->MPinit = this->Minit * this->refMesh.GetBigParamMat();
uvec matchesInitIdxs = linspace<uvec>(0, matchesInit.n_rows-1, matchesInit.n_rows);
// Currently used matches represented by their indices. Initially, use all matches: [0,1,2..n-1]
inlierMatchIdxs = matchesInitIdxs;
for (int i = 0; i < nUncstrIters; i++)
{
this->reconstructPlanarUnconstr(inlierMatchIdxs, wr, resMesh);
// If it is the final iteration, break and don't update "inlierMatchIdxs" or "weights", "radius"
if (i == nUncstrIters - 1)
{
//cout << "Current radius: " << radius << endl;
//cout << "Current wr: " << wr << endl;
//Reconstruction::computeCurrentMatrices(currentMatchIdxs, 325); // For Ferns
break;
}
// Otherwise, remove outliers
int iterTO = nUncstrIters - 2;
if (i >= iterTO)
reprojErrors = this->computeReprojectionErrors(resMesh, matchesInit, matchesInitIdxs);
else
reprojErrors = this->computeReprojectionErrors(resMesh, matchesInit, inlierMatchIdxs);
uvec idxs = find( reprojErrors < radius );
if (idxs.n_elem == 0)
break;
if (i >= iterTO)
inlierMatchIdxs = matchesInitIdxs.elem(idxs);
else
inlierMatchIdxs = inlierMatchIdxs.elem(idxs);
// Update parameters
wr = wr / Reconstruction::ROBUST_SCALE;
radius = radius / Reconstruction::ROBUST_SCALE;
}
}
/*! Returns the action that maximizes the Q-value for the given state
*
*/
size_t argmax_q(const mat & Q, const size_t &state, const uvec & possible_a){
// There might be several actions that give the max Q-value
vector<size_t> maxq_actions;
// Calculate the max Q-value for this state and actions possible from here
double maxq = Q(state, possible_a(0));
for(auto i : range(1,possible_a.size())){
if(Q(state,possible_a(i)) > maxq){
maxq = Q(state,possible_a(i));
}
}
// Check if there are several actions with same q-value as maxq
for(auto i : range(possible_a.size())){
if (Q(state,possible_a(i)) == maxq){
maxq_actions.push_back(possible_a(i));
}
}
// Return the single action that maximizes the Q-value or
// randomly one of the actions that maximize the Q-value.
if(maxq_actions.size() > 1){
return maxq_actions[randint(maxq_actions.size())];
}else{
return maxq_actions[0];
}
}
// Set difference
// - Return vector Ea with elements of Ec removed
// ======================================================================
uvec setdifference(uvec Ea, uvec Ec){
int na = Ea.size();
for(int j=na; j --> 0;){
bool matchind = any(Ec == Ea(j));
if(matchind == TRUE){
Ea.shed_row(j);
}
}
return(Ea);
}
/*! Combinations of integer ranges
*
* Calculates all combinations of integer ranges of which the length is given in the input vector.
*
* @param dim vector of dimensions for each variable
*
* @retval Matrix of size (prod(dim) x #variables)
*/
Mat<size_t> combinations(const uvec & dim){
size_t nvariables = dim.size();
size_t ncombinations = prod(dim);
Mat<size_t> result(ncombinations, nvariables);
for(auto var_i : range(nvariables)){
size_t var_val = 0;
size_t var_len;
if (var_i == nvariables-1){
var_len = 1; // Last variable
}else{
var_len = prod(dim(span(var_i+1,nvariables-1)));
}
for(auto j : range(ncombinations)){
if ((j % (var_len * dim(var_i)) == 0) && j > 0){
var_val = 0;
}else if ((j % var_len == 0) && j > 0){
var_val += 1;
}
result(j, var_i) = var_val;
}
}
return result;
};
//[[Rcpp::export]]
mat update_mstates_arma(const uvec& extantLines, const mat& Q, mat mstates)
{
int u;
vec p_u ;
double sms;
int m = Q.n_rows;
for (int i = 0; i < extantLines.size(); i++){
u = extantLines(i);
p_u = mstates.col(u-1);
mstates.col(u-1) = Q * p_u ;
//~ for (int k = 0; k < m; k++){
//~ mstates(u-1, k) = dot( p_u, Q.col(k));
//~ }
sms = sum(mstates.col(u-1));
mstates.col(u-1) /= sms;
}
return mstates;
}
void calc_overlap(vec &ov,vec &ov_n1,Dres_det dres1,int f1,Dres_det dres2,uvec f2)
{
int n = f2.size();
double cx1 = dres1.x(f1);
double cx2 = dres1.x(f1)+dres1.w(f1)-1;
double cy1 = dres1.y(f1);
double cy2 = dres1.y(f1)+dres1.h(f1)-1;
vec gx1 = dres2.x(f2);
vec gx2 = dres2.x(f2)+dres2.w(f2)-1;
vec gy1 = dres2.y(f2);
vec gy2 = dres2.y(f2)+dres2.h(f2)-1;
double ca = dres1.h(f1)*dres1.w(f1);
vec ga = dres2.h(f2)*dres2.w(f2);
//find the overlapping area
vec xx1 = zeros<vec>(n);
vec yy1 = zeros<vec>(n);
vec xx2 = zeros<vec>(n);
vec yy2 = zeros<vec>(n);
for(int i = 1;i<=n;i++)
{
xx1(i) = max(cx1,gx1(i));
yy1(i) = max(cy1,gy1(i));
xx2(i) = min(cx2,gx2(i));
yy2(i) = min(cy2,gy2(i));
}
vec w = xx2-xx1+1;
vec h = yy2-yy1+1;
uvec inds = find((w>0)*(h>0));
ov = zeros<vec>(n);
ov_n1 = zeros<vec>(n);
if(inds.is_empty() == 0)
{
vec inter = w(inds)*h(inds);
vec u = ca+ga(inds)-w(inds)*h(inds);
ov(inds) = inter/u;
ov_n1(inds) = inter/ca;
}
}
double backtracking(uvec & sequence) {
sequence.set_size(observations.n_cols);
uword index;
double best_likelihood = v.max(index);
// cout << index << endl;
sequence[backtrack.n_cols - 1] = index;
for (int t = backtrack.n_cols - 2; t >= 0; t--) {
// cout << "t=" << t << endl;
// seq.print();
// cout << "bt[t]=" << backtrack.unsafe_col(t)(seq[t + 1]) << endl;
// cout << seq[t + 1] << endl;
// cout << backtrack.col(t);
// ivec r = backtrack.col(t);
// seq[t] = r(seq[t + 1]);
sequence[t] = backtrack.unsafe_col(t + 1)(sequence[t + 1]);
}
return best_likelihood;
}
//[[Rcpp::export]]
mat finite_size_correction(const vec& p_a, const vec& A, const uvec& extantLines, mat mstates)
{
// NOTE mstates m X n
int u;
vec rho;
vec rterm;
//~ vec lterm;
double lterm;
vec p_u;
for (int iu = 0; iu < extantLines.size(); iu++){
u = extantLines(iu);
p_u = mstates.col(u-1);
rterm = p_a / ( A - p_u);
rho = A / (A - p_u );
lterm = dot( rho, p_a); //
p_u = p_u % (lterm - rterm) ;
p_u = p_u / sum(p_u ) ;
mstates.col(u - 1) = p_u;
}
return mstates;
}
void costfunc::compute_correspondences(mat &ptns, mat &sphM, uvec &matchId) {
/* ptns: reference to observed points
* sphM: reference to spheres model
* matchId:returned vector containing the matchId
*
* Match every point in the observed model to the closest sphere of
* the hand model
*
* This uses the BFMatcher provided by OpenCV
*
* ==> need to firstly convert arma::mat to cv::mat
*/
// start conversion from arma::mat --> cv::mat
fmat spheresM = conv_to<fmat>::from(sphM);
fmat ptncloud = conv_to<fmat>::from(ptns);
cv::Mat cvspheresM(spheresM.n_cols, spheresM.n_rows, CV_32F, spheresM.memptr());
cv::Mat cvptncloud(ptncloud.n_cols, ptncloud.n_rows, CV_32F, ptncloud.memptr());
// cv::mat is row-major while arma::mat is col-major
cvspheresM = cvspheresM.t();
cvptncloud = cvptncloud.t();
cv::BFMatcher matcher(cv::NORM_L2); // initialise a brute-force matcher
std::vector<cv::DMatch> matches; // initialise a container for the matches
matcher.match(cvptncloud, cvspheresM, matches); // perform matching
int cnt = ptncloud.n_rows;
matchId = zeros<uvec>(cnt);
for (int i = 0; i < cnt; i++) {
matchId.at(i) = matches[i].trainIdx;
}
}
//-----------------------------------------------------
// Cross-validation across an array of lambda and pick up the best.
List CV_lam_grid_cpp(vec y_vect, mat x_mat, vec id_vect, mat hat_R_full, vec beta_ini, int fold, int n, vec m, int obs_n, int p, uvec start, uvec end, vec lam_vect, double eps_tozero, double eps_stop, int iter_try){
int lam_length = lam_vect.n_elem;
double lam_temp, cv_sum, flag_stop_sum, iter_n_sum, cv_min = math::inf(), lam_min = -1;
uvec cvgrps_seq = linspace<uvec>(0, (n-1), n);
uvec cvgrps_subsets = shuffle(cvgrps_seq);
uvec cvgrps_which = cvgrps_seq - floor( cvgrps_seq / fold) * fold;
uvec index_cv_train, index_cv_test;
vec cv_vect(lam_length), flag_stop_vect(lam_length), iter_n_vect(lam_length);
uvec idx_train, idx_test;
vec y_train, y_test, id_train, m_train, beta_train;
mat x_train, x_test;
List indGen_res, beta_shrink_res;
for(int lam_iter = 0; lam_iter < lam_length; lam_iter++)
{
lam_temp = lam_vect(lam_iter);
cv_sum = 0;
flag_stop_sum = 0;
iter_n_sum = 0;
for(int k = 0; k < fold; k++)
{
index_cv_train = cvgrps_subsets.elem(find(cvgrps_which != k));
index_cv_test = cvgrps_subsets.elem(find(cvgrps_which == k));
idx_train = unique(seqJoin_vec(start.elem(index_cv_train), end.elem(index_cv_train), m.elem(index_cv_train)));
idx_test = unique(seqJoin_vec(start.elem(index_cv_test), end.elem(index_cv_test), m.elem(index_cv_test)));
y_train = y_vect.elem(idx_train);
x_train = x_mat.rows(idx_train);
id_train = id_vect.elem(idx_train);
indGen_res = indGen_cpp(id_train);
y_test = y_vect.elem(idx_test);
x_test = x_mat.rows(idx_test);
beta_shrink_res = beta_shrink_normal_cpp(y_train, x_train, id_train, hat_R_full, beta_ini, as<int>(indGen_res[0]), as<vec>(indGen_res[1]), as<int>(indGen_res[2]), p, as<uvec>(indGen_res[3]), as<uvec>(indGen_res[4]), as<vec>(indGen_res[5]), as<uvec>(indGen_res[6]), lam_temp, eps_tozero, eps_stop, iter_try);
cv_sum += sqrt(mean(pow((y_test - x_test * as<vec>(beta_shrink_res[0])),2)));
flag_stop_sum += as<double>(beta_shrink_res[2]);
iter_n_sum += as<double>(beta_shrink_res[3]);
}
// Calculate average across k-fold validation
cv_vect(lam_iter) = cv_sum / fold;
flag_stop_vect(lam_iter) = flag_stop_sum / fold;
iter_n_vect(lam_iter) = iter_n_sum / fold;
if(cv_sum < cv_min)
{
lam_min = lam_temp;
cv_min = cv_sum;
}
}
return List::create(Named("lam.vect") = lam_vect,
Named("cv.vect") = cv_vect,
Named("flag_stop_vect") = flag_stop_vect,
Named("iter_n_vect") = iter_n_vect,
Named("lam.min") = lam_min,
Named("cv.min") = cv_min
);
}
inline uvec vclip(uvec v, double low = vmin, double high = vmax)/*{{{*/
{
for (int i = 0;i < v.size();i ++)
v[i] = min(max(v[i], vmin), vmax);
return v;
}/*}}}*/