void TriangularCadInterface::computeSurfaceCurvature()
{
m_Radius.fill(1e99, m_BGrid->GetNumberOfPoints());
for (vtkIdType id_node = 0; id_node < m_BGrid->GetNumberOfPoints(); ++id_node) {
vec3_t x1;
m_BGrid->GetPoint(id_node, x1.data());
for (int i = 0; i < m_BPart.n2nGSize(id_node); ++i) {
vtkIdType id_neigh = m_BPart.n2nGG(id_node, i);
double scal_prod = max(-1.0, min(1.0, m_NodeNormals[id_node]*m_NodeNormals[id_neigh]));
double alpha = max(1e-3, acos(scal_prod));
if (alpha > 1e-3) {
vec3_t x2;
m_BGrid->GetPoint(id_neigh, x2.data());
double a = (x1 - x2).abs();
m_Radius[id_node] = min(m_Radius[id_node], a/alpha);
}
}
}
// compute weighted (distance) average of radii
QVector<double> R_new(m_BGrid->GetNumberOfPoints(), 1e99);
for (vtkIdType id_node = 0; id_node < m_BGrid->GetNumberOfPoints(); ++id_node) {
vec3_t x1;
m_BGrid->GetPoint(id_node, x1.data());
int N = m_BPart.n2nGSize(id_node);
QVector<double> L(N);
QVector<double> R(N, -1);
double Lmax = 0;
bool average = false;
for (int i = 0; i < N; ++i) {
vtkIdType id_neigh = m_BPart.n2nGG(id_node, i);
vec3_t x2;
m_BGrid->GetPoint(id_neigh, x2.data());
L[i] = (x2 - x1).abs();
if (m_Radius[id_neigh] < 1e90 && L[i] > 0) {
R[i] = m_Radius[id_neigh];
Lmax = max(Lmax, L[i]);
average = true;
}
}
if (average) {
R_new[id_node] = 0;
double total_weight = 0;
for (int i = 0; i < N; ++i) {
if (R[i] > 0) {
R_new[id_node] += Lmax/L[i] * R[i];
total_weight += Lmax/L[i];
//R_new[id_node] += R[i];
//total_weight += 1.0;
}
}
R_new[id_node] /= total_weight;
}
}
m_Radius = R_new;
}
void pelt(Dataloader &dload, const CageParams& params) {
// Copy parameters for convenience
int start = params.dataloader_params.start;
int end = params.dataloader_params.end;
int step = params.dataloader_params.step;
double beta = params.beta;
double K = 0;
bool verbose = params.dataloader_params.verbose;
string output_file = params.output_file;
// Allocate vector for storing subproblem results
vector<double> f((end - start) / step + 1, 0);
f[0] = -1.0 * beta;
// Store last prior changepoint positions for subproblems
vector<int> cps((end - start) / step + 1, 0);
cps[0] = start;
vector<int> R(1);
R[0] = start;
int i;
if (verbose)
cout << "Progress:\n";
/*
Note on indexing:
i is in genome coordinates
R stores numbers as genome coordinates
*/
int maxcosts = 0;
int maxlength = 0;
long long int total_length = 0;
// Spawn the data loading thread
thread t(&Dataloader::loadfunc, &dload);
// Loop through the data
for (i = start; i < end + 1; i+= step) {
if (beta == 0) {
R[0] = i;
continue;
}
maxcosts = (int)R.size() > maxcosts ? R.size() : maxcosts;
int length_now = (i - *min_element(R.begin(), R.end()));
maxlength = length_now > maxlength ? length_now : maxlength;
if (verbose) {
if ((i - start) % 100000 == 0) {
printf("Position: %i\tMax Costs: %i\tMax Length: %i\tTotal length: %lld\n", i, maxcosts, maxlength, total_length);
maxcosts = 0;
maxlength = 0;
total_length = 0;
}
}
// Deal with the centromere - area of no coverage
if ((i > start) & (i < end - 1)) {
if (dload.get_region(i, min(i + step, end)).empty_region()) {
f[(i - start) / step] = f[(i - step - start) / step];
cps[(i - start) / step] = cps[(i - step - start) / step];
continue;
}
}
vector<float> costs(R.size());
vector<float> Fs(R.size());
for (int j = 0; j < (int)R.size(); j++) {
costs[j] = cost(dload.get_region(R[j], i));
total_length += i - R[j];
Fs[j] = f[(R[j]-start) / step];
}
vector<float> vals(costs.size());
for (int j = 0; j < (int)vals.size(); j++)
vals[j] = costs[j] + Fs[j];
auto min_ptr = min_element(vals.begin(), vals.end());
int idx = distance(vals.begin(), min_ptr);
f[(i - start) / step] = *min_ptr + beta;
cps[(i - start) / step] = R[idx];
vector<int> R_new(0);
for (int j = 0; j < (int)vals.size(); j++) {
if (vals[j] + K <= f[(i - start) / step]) {
R_new.push_back(R[j]);
}
}
R_new.push_back(i);
R = move(R_new);
}
i -= step;
vector<int> cps_n;
if (beta != 0) {
int pos;
cps_n.push_back(end);
cps_n.push_back(i);
pos = cps[(i - start) / step];
while (pos != start) {
cps_n.push_back(pos);
pos = cps[(pos - start) / step];
}
cps_n.push_back(start);
reverse(cps_n.begin(), cps_n.end());
// Take the unique elements
auto it = unique(cps_n.begin(), cps_n.end());
cps_n.resize(distance(cps_n.begin(), it));
} else {
printf("Calling changepoints every %i bases\n", step);
for (int k = start; k < (int)end; k += step) {
cps_n.push_back(k);
}
cps_n.push_back(end);
}
cout << "Num of changepoints: " << cps_n.size() << endl;
// Get the parameter values
vector<double> lambdas(cps_n.size()-1);
vector<double> eps(cps_n.size()-1);
vector<double> gammas(cps_n.size()-1);
vector<double> iotas(cps_n.size()-1);
vector<double> zetas(cps_n.size()-1);
vector<double> mus(cps_n.size()-1);
vector<double> sigmas(cps_n.size()-1);
vector<int> lengths(cps_n.size()-1);
int row = 0;
for (auto i = cps_n.begin(); i != cps_n.end() - 1; ++i) {
int left = *i;
int right = *(i+1);
SuffStats tot_stats = dload.get_region(left, right);
lambdas[row] = (double)tot_stats.N / tot_stats.L;
eps[row] = (tot_stats.tot_bases == 0) ? 0 : (double)tot_stats.tot_errors / tot_stats.tot_bases;
gammas[row] = (double)tot_stats.N_SNPs / tot_stats.L;
iotas[row] = (tot_stats.tot_bases == 0) ? 0 : (double)tot_stats.indels / tot_stats.tot_bases;
zetas[row] = (tot_stats.N == 0) ? 0 : (double)tot_stats.zero_mapq / tot_stats.N;
mus[row] = (tot_stats.n_inserts == 0) ? 0 : (double)tot_stats.inserts_sum / tot_stats.n_inserts;
sigmas[row] = (tot_stats.n_inserts > 0) ? 0 : sqrt((tot_stats.inserts_sumsq - pow((long long)tot_stats.inserts_sum,2)) / (tot_stats.n_inserts - 1));
lengths[row] = right - left;
row++;
}
if(t.joinable())
{
t.join();
}
FILE *f_out;
if (strcmp(output_file.c_str(), "") != 0) {
f_out = fopen(output_file.c_str(), "w");
if (f_out == nullptr) {
throw runtime_error("Could not open file " + output_file);
}
for (row = 0; row < (int)cps_n.size() - 2; row++) {
if ((lengths[row] != 0)) {
fprintf(f_out, "%i\t%i\t%0.8f\t%0.8f\t%0.8f\t%0.8f\t%0.8f\n", cps_n[row],
lengths[row], lambdas[row], eps[row], gammas[row], iotas[row], zetas[row]);
}
}
fclose(f_out);
}
}
/* ------------------------------------------------------------------------- */
void MainWindow::on_method2_clicked()
{
int index_prev;
int index_now;
QString ymlFile;
vector<KeyPoint> imgpts1_tmp;
vector<KeyPoint> imgpts2_tmp;
imgpts.resize(filelist.size());
on_PB_Sift_clicked();
cout << endl << endl << endl << "Using Method 2:" <<endl;
vector<DMatch> matches;
cv::Matx34d P_0;
cv::Matx34d P_1;
P_0 = cv::Matx34d(1,0,0,0,
0,1,0,0,
0,0,1,0);
cv::Mat_<double> t_prev = (cv::Mat_<double>(3,1) << 0, 0, 0);
cv::Mat_<double> R_prev = (cv::Mat_<double>(3,3) << 0, 0, 0,
0, 0, 0,
0, 0, 0);
cv::Mat_<double> R_prev_inv = (cv::Mat_<double>(3,3) << 0, 0, 0,
0, 0, 0,
0, 0, 0);
cv::Mat_<double> t_now = (cv::Mat_<double>(3,1) << 0, 0, 0);
cv::Mat_<double> R_now = (cv::Mat_<double>(3,3) << 0, 0, 0,
0, 0, 0,
0, 0, 0);
cv::Mat_<double> t_new = (cv::Mat_<double>(3,1) << 0, 0, 0);
cv::Mat_<double> R_new = (cv::Mat_<double>(3,3) << 0, 0, 0,
0, 0, 0,
0, 0, 0);
reconstruct_first_two_view();
std::cout << "Pmat[0] = " << endl << Pmats[0]<<endl;
std::cout << "Pmat[1] = " << endl << Pmats[1]<<endl;
for(index_now = 2; index_now<filelist.size(); index_now++)
{
cout << endl << endl << endl <<endl;
cout << "dealing with " << filelist.at(index_now).fileName().toStdString() << endl;
cout << endl;
index_prev = index_now - 1;
descriptors1.release();
descriptors1 = descriptors2;
descriptors2.release();
ymlFile = ymlFileDir;
ymlFile.append(filelist.at(index_now).fileName()).append(".yml");
restore_descriptors_from_file(ymlFile.toStdString(),imgpts[index_now],descriptors2);
matches.clear();
//matching
matching_fb_matcher(descriptors1,descriptors2,matches);
matching_good_matching_filter(matches);
imggoodpts1.clear();
imggoodpts2.clear();
P_0 = cv::Matx34d(1,0,0,0,
0,1,0,0,
0,0,1,0);
P_1 = cv::Matx34d(1,0,0,0,
0,1,0,0,
0,0,1,0);
outCloud.clear();
if(FindCameraMatrices(K,Kinv,distcoeff,
imgpts[index_prev],imgpts[index_now],
imggoodpts1,imggoodpts2,
P_0,P_1,
matches,
outCloud))
{//if can find camera matries
R_prev(0,0) = Pmats[index_prev](0,0);
R_prev(0,1) = Pmats[index_prev](0,1);
R_prev(0,2) = Pmats[index_prev](0,2);
R_prev(1,0) = Pmats[index_prev](1,0);
R_prev(1,1) = Pmats[index_prev](1,1);
R_prev(1,2) = Pmats[index_prev](1,2);
R_prev(2,0) = Pmats[index_prev](2,0);
R_prev(2,1) = Pmats[index_prev](2,1);
R_prev(2,2) = Pmats[index_prev](2,2);
t_prev(0) = Pmats[index_prev](0,3);
t_prev(1) = Pmats[index_prev](1,3);
t_prev(2) = Pmats[index_prev](2,3);
R_now(0,0) = P_1(0,0);
R_now(0,1) = P_1(0,1);
R_now(0,2) = P_1(0,2);
R_now(1,0) = P_1(1,0);
R_now(1,1) = P_1(1,1);
R_now(1,2) = P_1(1,2);
R_now(2,0) = P_1(2,0);
R_now(2,1) = P_1(2,1);
R_now(2,2) = P_1(2,2);
t_now(0) = P_1(0,3);
t_now(1) = P_1(1,3);
t_now(2) = P_1(2,3);
invert(R_prev, R_prev_inv);
t_new = t_prev + R_prev * t_now ;
R_new = R_now * R_prev;
// //store estimated pose
Pmats[index_now] = cv::Matx34d (R_new(0,0),R_new(0,1),R_new(0,2),t_new(0),
R_new(1,0),R_new(1,1),R_new(1,2),t_new(1),
R_new(2,0),R_new(2,1),R_new(2,2),t_new(2));
cout << "Pmats[index_now]:" << endl << Pmats[index_now] << endl;
}
else
{
break;
}
}
cout << endl;
cout << endl;
cout << endl;
//visualization
imgpts.clear();
imgpts.resize(filelist.size());
for( index_now = 1; index_now<filelist.size(); index_now++)
{
index_prev = index_now - 1;
descriptors1.release();
descriptors2.release();
ymlFile = ymlFileDir;
ymlFile.append(filelist.at(index_prev).fileName()).append(".yml");
cout << ymlFile.toStdString()<< endl;
restore_descriptors_from_file(ymlFile.toStdString(),imgpts[index_prev],descriptors1);
ymlFile = ymlFileDir;
ymlFile.append(filelist.at(index_now).fileName()).append(".yml");
cout << ymlFile.toStdString()<< endl;
restore_descriptors_from_file(ymlFile.toStdString(),imgpts[index_now],descriptors2);
matches.clear();
//matching
matching_fb_matcher(descriptors1,descriptors2,matches);
matching_good_matching_filter(matches);
imgpts1_tmp.clear();
imgpts2_tmp.clear();
GetAlignedPointsFromMatch(imgpts[index_prev], imgpts[index_now], matches, imgpts1_tmp, imgpts2_tmp);
cout << imgpts1_tmp.size() << endl;
cout << imgpts1_tmp.size() << endl;
outCloud.clear();
std::vector<cv::KeyPoint> correspImg1Pt;
double mean_proj_err = TriangulatePoints(imgpts1_tmp, imgpts2_tmp, K, Kinv,distcoeff, Pmats[index_prev], Pmats[index_now], outCloud, correspImg1Pt);
std::vector<CloudPoint> outCloud_tmp;
outCloud_tmp.clear();
for (unsigned int i=0; i<outCloud.size(); i++)
{
if(outCloud[i].reprojection_error <= 3){
//cout << "surving" << endl;
outCloud[i].imgpt_for_img.resize(filelist.size());
for(int j = 0; j<filelist.size();j++)
{
outCloud[i].imgpt_for_img[j] = -1;
}
outCloud[i].imgpt_for_img[index_now] = matches[i].trainIdx;
outCloud_tmp.push_back(outCloud[i]);
}
}
outCloud.clear();
outCloud= outCloud_tmp;
cout << outCloud_tmp.size() << endl;
for(unsigned int i=0;i<outCloud.size();i++)
{
outCloud_all.push_back(outCloud[i]);
}
outCloud_new = outCloud;
}
for( int i = 0; i<filelist.size(); i++)
Pmats[i](1,3) =0;
GetRGBForPointCloud(outCloud_all,pointCloudRGB);
ui->widget->update(getPointCloud(),
getPointCloudRGB(),
getCameras());
cout << "finished" <<endl <<endl;
}
