bool VertexPointXYZCov::write(std::ostream& os) const
{
/// we store the id of the observation / parent vertex for faster lookups...
/// this information is also available through the connected edges
os << _parentVertex->id() << " ";
/// estimated pose of point (global coordinate frame):
/// this will be different from the measured depth after optimization!
for (int i=0; i<3; i++)
os << estimate()[i] << " ";
/// color information for the point (if available)
uint r,g,b;
r = (uint) cr;
g = (uint) cg;
b = (uint) cb;
os << r << " " << g << " " << b << " ";
///covariance of the point, computed from the neighborhood (do we really need to store in on the drive??)
for (int i=0; i<3; i++)
for (int j=i; j<3; j++){
os << _cov(i,j) << " ";
}
return os.good();
}
/* unconditional Monte Carlo simulation for correlation-based tests. */
SEXP gauss_mcarlo(SEXP x, SEXP y, SEXP samples, SEXP test, SEXP alpha) {
int j = 0, k = 0, num = LENGTH(x), *B = INTEGER(samples);
double *xx = REAL(x), *yy = REAL(y), *yperm = NULL, *res = NULL;
double observed = 0, enough = ceil(NUM(alpha) * (*B)) + 1, xm = 0, ym = 0;
int *perm = NULL, *work = NULL;
SEXP result;
/* allocate the arrays needed by RandomPermutation. */
perm = alloc1dcont(num);
work = alloc1dcont(num);
/* allocate the array for the pemutations. */
yperm = alloc1dreal(num);
/* cache the means of the two variables (they are invariant under permutation). */
for (j = 0; j < num; j++) {
xm += xx[j];
ym += yy[j];
}/*FOR*/
xm /= num;
ym /= num;
/* allocate the result. */
PROTECT(result = allocVector(REALSXP, 1));
res = REAL(result);
*res = 0;
/* initialize the random number generator. */
GetRNGstate();
/* pick up the observed value of the test statistic, then generate a set of
random permutations (all variable but the second are fixed) and check how
many tests are greater (in absolute value) than the original one.*/
switch(INT(test)) {
case GAUSSIAN_MUTUAL_INFORMATION:
case LINEAR_CORRELATION:
case FISHER_Z:
observed = _cov(xx, yy, &xm, &ym, &num);
for (j = 0; j < *B; j++) {
RandomPermutation(num, perm, work);
for (k = 0; k < num; k++)
yperm[k] = yy[perm[k]];
if (fabs(_cov(xx, yperm, &xm, &ym, &num)) > fabs(observed)) {
sequential_counter_check(*res);
}/*THEN*/
}/*FOR*/
break;
}/*SWITCH*/
PutRNGstate();
/* save the observed p-value. */
*res /= *B;
UNPROTECT(1);
return result;
}/*GAUSS_MCARLO*/
