NonrigidResult Nonrigid::compute_impl(const MatrixRef X, const MatrixRef Y,
double sigma2) {
assert(X.cols() == Y.cols());
auto M = Y.rows();
auto D = Y.cols();
Matrix T = Y;
size_t max_iter = this->max_iterations();
size_t iter = 0;
double tol = this->tolerance();
double ntol = std::numeric_limits<double>::max();
double L = 1.0;
Matrix W = Matrix::Zero(M, D);
double beta = this->beta();
double lambda = this->lambda();
Vector Pt1;
Vector P1;
Matrix PX;
Matrix G = construct_affinity_matrix(Y, Y, beta);
while (iter < max_iter && ntol > tol &&
sigma2 > 10 * std::numeric_limits<double>::epsilon()) {
double L_old = L;
std::tie(Pt1, P1, PX, L) = calculate_probabilities(X, T, sigma2);
L = L + lambda / 2 * (W.transpose() * G * W).trace();
ntol = std::abs((L - L_old) / L);
// TODO this shouldn't be cout
std::cout << "CPD Nonrigid (FGT) : dL= " << ntol << ", iter= " << iter
<< ", sigma2= " << sigma2 << "\n";
auto dP = P1.asDiagonal();
W = (dP * G + lambda * sigma2 * Matrix::Identity(M, M))
.colPivHouseholderQr()
.solve(PX - dP * Y);
T = Y + G * W;
double Np = P1.sum();
sigma2 =
std::abs(((X.array().pow(2) * Pt1.replicate(1, D).array()).sum() +
(T.array().pow(2) * P1.replicate(1, D).array()).sum() -
2 * (PX.transpose() * T).trace()) /
(Np * D));
++iter;
}
return {T};
}
void separation(const char* filename, double background_integral, double* stream_integrals)
{
int q[ap->number_streams];
double nstars[ap->number_streams];
int total;
double sprob[ap->number_streams];
double prob_s[ap->number_streams];
double prob_b;
double pbx;
double psg[ap->number_streams];
double d;
int twoPanel;
double** cmatrix;
double dnormal[3];
double dortho[3];
double xsun[3];
double epsilon_s[ap->number_streams];
double epsilon_b;
double star_coords[3];
double starxyz[3];
double starxyzTransform[3];
int s_ok = 0;
int i, j, retval;
FILE* file;
double reff_xr_rp3, *qw_r3_N, *r_point;
twoPanel = 1;
for (j = 0; j < ap->number_streams; j++)
{
nstars[j] = 0;
q[j] = 0;
}
total = 0;
prob_ok_init();
printf("Integral complete.\n Beginning probability calculations...\n");
file = fopen(filename, "w");
if (ap->sgr_coordinates == 0)
{
stripe_normal(ap->wedge, dnormal);
}
else if (ap->sgr_coordinates == 1)
{
sgr_stripe_normal(ap->wedge, dnormal);
}
else
{
printf("Error: ap->sgr_coordinates not valid");
}
free_star_points(sp);
free(sp);
sp = (STAR_POINTS*)malloc(sizeof(STAR_POINTS));
retval = read_star_points(star_points_file, sp);
if (retval)
{
fprintf(stderr, "APP: error reading star points: %d\n", retval);
exit(1);
}
printf("read %d stars.\n", sp->number_stars);
cmatrix = (double**)malloc(sizeof(double*) * 3);
for (i = 0; i < 3; i++)
cmatrix[i] = (double*)malloc(sizeof(double) * 3);
dortho[0] = 0.0;
dortho[1] = 0.0;
dortho[2] = 1.0;
get_transform(dnormal, dortho, cmatrix);
printf("\nTransformation matrix:\n");
printf("\t%lf %lf %lf\n", cmatrix[0][0], cmatrix[0][1], cmatrix[0][2]);
printf("\t%lf %lf %lf\n", cmatrix[1][0], cmatrix[1][1], cmatrix[1][2]);
printf("\t%lf %lf %lf\n\n", cmatrix[2][0], cmatrix[2][1], cmatrix[2][2]);
xsun[0] = -8.5;
xsun[1] = 0.0;
xsun[2] = 0.0;
d = dotp(dnormal, xsun);
printf("==============================================\n");
printf("bint: %lf", background_integral);
for (j = 0; j < ap->number_streams; j++)
{
printf(", ");
printf("sint[%d]: %lf", j, stream_integrals[j]);
}
printf("\n");
/*get stream & background weight constants*/
double denom = 1.0;
for (j = 0; j < ap->number_streams; j++)
{
denom += exp(ap->stream_weights[j]);
}
for (j = 0; j < ap->number_streams; j++)
{
epsilon_s[j] = exp(ap->stream_weights[j]) / denom;
printf("epsilon_s[%d]: %lf\n", j, epsilon_s[j]);
}
epsilon_b = 1.0 / denom;
printf("epsilon_b: %lf\n", epsilon_b);
r_point = (double*)malloc(sizeof(double) * ap->convolve);
qw_r3_N = (double*)malloc(sizeof(double) * ap->convolve);
init_constants(ap);
printf("initialized constants\n");
for (i = 0; i < sp->number_stars; i++)
{
MW_DEBUG("[%d/%d] setting star coords\n", i, sp->number_stars);
star_coords[0] = sp->stars[i][0];
star_coords[1] = sp->stars[i][1];
star_coords[2] = sp->stars[i][2];
MW_DEBUG("star_coords: %g %g %g\n", star_coords[0], star_coords[1], star_coords[2]);
MW_DEBUG("twoPanel: %d\n", twoPanel);
if (twoPanel == 1)
{
MW_DEBUG("setting probability constants\n");
set_probability_constants(ap->convolve, star_coords[2], r_point, qw_r3_N, &reff_xr_rp3);
MW_DEBUG("calculating probabilities\n");
calculate_probabilities(r_point, qw_r3_N, reff_xr_rp3, star_coords, ap, &prob_b, prob_s);
MW_DEBUG("calculated probabilities\n");
MW_DEBUG("prob_s: %lf\n", prob_s[0]);
MW_DEBU("prob_b: %lf\n", prob_b);
pbx = epsilon_b * prob_b / background_integral;
for (j = 0; j < ap->number_streams; j++)
{
psg[j] = epsilon_s[j] * prob_s[j] / stream_integrals[j];
}
MW_DEBUG("pbx: %g\n", pbx);
MW_DEBUG("psg: %g\n", psg[0]);
double psgSum = 0;
for (j = 0; j < ap->number_streams; j++)
{
psgSum += psg[j];
}
for (j = 0; j < ap->number_streams; j++)
{
sprob[j] = psg[j] / (psgSum + pbx);
}
MW_DEBUG("sprob: %g\n", sprob[0]);
for (j = 0; j < ap->number_streams; j++)
{
nstars[j] += sprob[j];
}
MW_DEBUG("nstars: %g\n", nstars[0]);
}
else
{
for (j = 0; j < ap->number_streams; j++)
{
sprob[j] = 1.0;
nstars[j] += 1.0;
}
}
/*determine if star with sprob should be put into stream*/
//for(j = 0; j < ap->number_streams; j++) {
s_ok = prob_ok(ap->number_streams, sprob);
// if (s_ok == 1) {
// s_ok += j;
// break;
// }
//}
MW_DEBUG("s_ok: %d\n", s_ok);
if (s_ok >= 1)
{
q[s_ok-1]++;
}
lbr2xyz(star_coords, starxyz);
transform_point(starxyz, cmatrix, xsun, starxyzTransform);
fprintf(file, "%d %lf %lf %lf\n", s_ok, starxyzTransform[0], starxyzTransform[1], starxyzTransform[2]);
//free(starxyz);
//free(starxyzTransform);
total += 1;
if ( (total % 10000) == 0 )
printf("%d\n", total);
}
printf("%d total stars\n", total);
for (j = 0; j < ap->number_streams; j++)
{
printf("%lf in stream[%d] (%lf%%)\n", nstars[j], j, (nstars[j] / total * 100));
}
for (j = 0; j < ap->number_streams; j++)
{
printf("%d stars separated into stream\n", q[j]);
}
fclose(file);
printf("Output written to: %s\n", filename);
free(r_point);
free(qw_r3_N);
free_constants(ap);
}
