inline
void
reaccum_moments(accum &A, int *t, int n)
{
clear_accum(A);
for(int i=0; i<n; i++)
accum_moment(A, RAPID_moment[t[i]]);
}
int SelTempAvgMain::process_buffer(VFrame *frame,
int64_t start_position,
double frame_rate)
{
int h = frame->get_h();
int w = frame->get_w();
int color_model = frame->get_color_model();
load_configuration();
// Allocate accumulation
if(!accumulation)
{
accumulation = new unsigned char[w *
h *
BC_CModels::components(color_model) *
sizeof(float)];
accumulation_sq = new unsigned char[w *
h *
3 *
sizeof(float)];
clear_accum(w, h, color_model);
}
if(!config.nosubtract)
{
// Reallocate history
if(history)
{
if(config.frames != history_size)
{
VFrame **history2;
int64_t *history_frame2;
int *history_valid2;
history2 = new VFrame*[config.frames];
history_frame2 = new int64_t[config.frames];
history_valid2 = new int[config.frames];
// Copy existing frames over
int i, j;
for(i = 0, j = 0; i < config.frames && j < history_size; i++, j++)
{
history2[i] = history[j];
history_frame2[i] = history_frame[i];
history_valid2[i] = history_valid[i];
}
// Delete extra previous frames and subtract from accumulation
for( ; j < history_size; j++)
{
subtract_accum(history[j]);
delete history[j];
}
delete [] history;
delete [] history_frame;
delete [] history_valid;
// Create new frames
for( ; i < config.frames; i++)
{
history2[i] = new VFrame(0, -1, w, h, color_model, -1);
history_frame2[i] = -0x7fffffff;
history_valid2[i] = 0;
}
history = history2;
history_frame = history_frame2;
history_valid = history_valid2;
history_size = config.frames;
}
}
else
// Allocate history
{
history = new VFrame*[config.frames];
for(int i = 0; i < config.frames; i++)
history[i] = new VFrame(0, -1, w, h, color_model, -1);
history_size = config.frames;
history_frame = new int64_t[config.frames];
bzero(history_frame, sizeof(int64_t) * config.frames);
history_valid = new int[config.frames];
bzero(history_valid, sizeof(int) * config.frames);
}
// Create new history frames based on current frame
int64_t *new_history_frames = new int64_t[history_size];
int64_t theoffset = (int64_t) config.offset_fixed_value;
if (config.offsetmode == SelTempAvgConfig::OFFSETMODE_RESTARTMARKERSYS)
theoffset = (int64_t) restartoffset;
for(int i = 0; i < history_size; i++)
{
new_history_frames[history_size - i - 1] = start_position + theoffset + i;
}
// Subtract old history frames which are not in the new vector
int no_change = 1;
for(int i = 0; i < history_size; i++)
{
// Old frame is valid
if(history_valid[i])
{
int got_it = 0;
for(int j = 0; j < history_size; j++)
{
// Old frame is equal to a new frame
if(history_frame[i] == new_history_frames[j])
{
got_it = 1;
break;
}
}
// Didn't find old frame in new frames
if(!got_it)
{
subtract_accum(history[i]);
history_valid[i] = 0;
no_change = 0;
}
}
}
// If all frames are still valid, assume tweek occurred upstream and reload.
if(config.paranoid && no_change)
{
for(int i = 0; i < history_size; i++)
{
history_valid[i] = 0;
}
clear_accum(w, h, color_model);
}
// Add new history frames which are not in the old vector
for(int i = 0; i < history_size; i++)
{
// Find new frame in old vector
int got_it = 0;
for(int j = 0; j < history_size; j++)
{
if(history_valid[j] && history_frame[j] == new_history_frames[i])
{
got_it = 1;
break;
}
}
// Didn't find new frame in old vector
if(!got_it)
{
// Get first unused entry
for(int j = 0; j < history_size; j++)
{
if(!history_valid[j])
{
// Load new frame into it
history_frame[j] = new_history_frames[i];
history_valid[j] = 1;
read_frame(history[j],
0,
history_frame[j],
frame_rate);
add_accum(history[j]);
break;
}
}
}
}
delete [] new_history_frames;
}
else
// No subtraction
{
// Force reload if not repositioned or just started
if(config.paranoid && prev_frame == start_position ||
prev_frame < 0)
{
prev_frame = start_position - config.frames + 1;
prev_frame = MAX(0, prev_frame);
clear_accum(w, h, color_model);
}
for(int64_t i = prev_frame; i <= start_position; i++)
{
read_frame(frame,
0,
i,
frame_rate);
add_accum(frame);
//printf("SelTempAvgMain::process_buffer 1 %lld %lld %lld\n", prev_frame, start_position, i);
}
prev_frame = start_position;
}
// Read current frame into buffer (needed for the std-deviation tool)
read_frame(frame,
0,
start_position,
frame_rate);
// Transfer accumulation to output with division if average is desired.
transfer_accum(frame);
//printf("SelTempAvgMain::process_buffer 2\n");
return 0;
}
int
box::split_recurse(int *t, int n)
{
// The orientation for the parent box is already assigned to this->pR.
// The axis along which to split will be column 0 of this->pR.
// The mean point is passed in on this->pT.
// When this routine completes, the position and orientation in model
// space will be established, as well as its dimensions. Child boxes
// will be constructed and placed in the parent's CS.
if (n == 1)
{
return split_recurse(t);
}
// walk along the tris for the box, and do the following:
// 1. collect the max and min of the vertices along the axes of <or>.
// 2. decide which group the triangle goes in, performing appropriate swap.
// 3. accumulate the mean point and covariance data for that triangle.
accum M1, M2;
double C[3][3];
double c[3];
double minval[3], maxval[3];
int rc; // for return code on procedure calls.
int in;
tri *ptr;
int i;
double axdmp;
int n1 = 0; // The number of tris in group 1.
// Group 2 will have n - n1 tris.
// project approximate mean point onto splitting axis, and get coord.
axdmp = (pR[0][0] * pT[0] + pR[1][0] * pT[1] + pR[2][0] * pT[2]);
clear_accum(M1);
clear_accum(M2);
MTxV(c, pR, RAPID_tri[t[0]].p1);
minval[0] = maxval[0] = c[0];
minval[1] = maxval[1] = c[1];
minval[2] = maxval[2] = c[2];
for(i=0; i<n; i++)
{
in = t[i];
ptr = RAPID_tri + in;
MTxV(c, pR, ptr->p1);
minmax(minval[0], maxval[0], c[0]);
minmax(minval[1], maxval[1], c[1]);
minmax(minval[2], maxval[2], c[2]);
MTxV(c, pR, ptr->p2);
minmax(minval[0], maxval[0], c[0]);
minmax(minval[1], maxval[1], c[1]);
minmax(minval[2], maxval[2], c[2]);
MTxV(c, pR, ptr->p3);
minmax(minval[0], maxval[0], c[0]);
minmax(minval[1], maxval[1], c[1]);
minmax(minval[2], maxval[2], c[2]);
// grab the mean point of the in'th triangle, project
// it onto the splitting axis (1st column of pR) and
// see where it lies with respect to axdmp.
mean_from_moment(c, RAPID_moment[in]);
if (((pR[0][0]*c[0] + pR[1][0]*c[1] + pR[2][0]*c[2]) < axdmp)
&& ((n!=2)) || ((n==2) && (i==0)))
{
// accumulate first and second order moments for group 1
accum_moment(M1, RAPID_moment[in]);
// put it in group 1 by swapping t[i] with t[n1]
int temp = t[i];
t[i] = t[n1];
t[n1] = temp;
n1++;
}
else
{
// accumulate first and second order moments for group 2
accum_moment(M2, RAPID_moment[in]);
// leave it in group 2
// do nothing...it happens by default
}
}
// done using this->pT as a mean point.
// error check!
if ((n1 == 0) || (n1 == n))
{
// our partitioning has failed: all the triangles fell into just
// one of the groups. So, we arbitrarily partition them into
// equal parts, and proceed.
n1 = n/2;
// now recompute accumulated stuff
reaccum_moments(M1, t, n1);
reaccum_moments(M2, t + n1, n - n1);
}
// With the max and min data, determine the center point and dimensions
// of the parent box.
c[0] = (minval[0] + maxval[0])*0.5;
c[1] = (minval[1] + maxval[1])*0.5;
c[2] = (minval[2] + maxval[2])*0.5;
pT[0] = c[0] * pR[0][0] + c[1] * pR[0][1] + c[2] * pR[0][2];
pT[1] = c[0] * pR[1][0] + c[1] * pR[1][1] + c[2] * pR[1][2];
pT[2] = c[0] * pR[2][0] + c[1] * pR[2][1] + c[2] * pR[2][2];
d[0] = (maxval[0] - minval[0])*0.5;
d[1] = (maxval[1] - minval[1])*0.5;
d[2] = (maxval[2] - minval[2])*0.5;
// allocate new boxes
P = RAPID_boxes + RAPID_boxes_inited++;
N = RAPID_boxes + RAPID_boxes_inited++;
// Compute the orienations for the child boxes (eigenvectors of
// covariance matrix). Select the direction of maximum spread to be
// the split axis for each child.
double tR[3][3];
if (n1 > 1)
{
mean_from_accum(P->pT, M1);
covariance_from_accum(C, M1);
if (eigen_and_sort1(tR, C) > 30)
{
// unable to find an orientation. We'll just pick identity.
Midentity(tR);
}
McM(P->pR, tR);
if ((rc = P->split_recurse(t, n1)) != RAPID_OK) return rc;
}
else
{
if ((rc = P->split_recurse(t)) != RAPID_OK) return rc;
}
McM(C, P->pR); MTxM(P->pR, pR, C); // and F1
VmV(c, P->pT, pT); MTxV(P->pT, pR, c);
if ((n-n1) > 1)
{
mean_from_accum(N->pT, M2);
covariance_from_accum (C, M2);
if (eigen_and_sort1(tR, C) > 30)
{
// unable to find an orientation. We'll just pick identity.
Midentity(tR);
}
McM(N->pR, tR);
if ((rc = N->split_recurse(t + n1, n - n1)) != RAPID_OK) return rc;
}
else
{
if ((rc = N->split_recurse(t+n1)) != RAPID_OK) return rc;
}
McM(C, N->pR); MTxM(N->pR, pR, C);
VmV(c, N->pT, pT); MTxV(N->pT, pR, c);
return RAPID_OK;
}
int
RAPID_model::build_hierarchy()
{
// allocate the boxes and set the box list globals
num_boxes_alloced = num_tris * 2;
b = new box[num_boxes_alloced];
if (b == 0) return RAPID_ERR_MODEL_OUT_OF_MEMORY;
RAPID_boxes = b;
RAPID_boxes_inited = 1; // we are in process of initializing b[0].
// Determine initial orientation, mean point, and splitting axis.
int i;
accum M;
// double F1[3];
// double S1[6];
double C[3][3];
RAPID_moment = new moment[num_tris];
if (RAPID_moment == 0)
{
delete [] b;
return RAPID_ERR_MODEL_OUT_OF_MEMORY;
}
compute_moments(RAPID_moment, tris, num_tris);
clear_accum(M);
for(i=0; i<num_tris; i++)
accum_moment(M, RAPID_moment[i]);
mean_from_accum(b[0].pT, M);
covariance_from_accum(C, M);
eigen_and_sort1(b[0].pR, C);
// create the index list
int *t = new int[num_tris];
if (t == 0)
{
delete [] b;
delete [] RAPID_moment;
return RAPID_ERR_MODEL_OUT_OF_MEMORY;
}
for(i=0; i<num_tris; i++) t[i] = i;
// set the tri pointer
RAPID_tri = tris;
// do the build
int rc = b[0].split_recurse(t, num_tris);
if (rc != RAPID_OK)
{
delete [] b;
delete [] RAPID_moment;
delete [] t;
return RAPID_ERR_MODEL_OUT_OF_MEMORY;
}
// free the moment list
delete [] RAPID_moment; RAPID_moment = 0;
// null the tri pointer
RAPID_tri = 0;
// free the index list
delete [] t;
return RAPID_OK;
}
int
build_recurse(PQP_Model *m, int bn, int first_tri, int num_tris)
{
BV *b = m->child(bn);
// compute a rotation matrix
PQP_REAL C[3][3], E[3][3], R[3][3], s[3], axis[3], mean[3], coord;
#if RAPID2_FIT
moment *tri_moment = new moment[num_tris];
compute_moments(tri_moment, &(m->tris[first_tri]), num_tris);
accum acc;
clear_accum(acc);
for(int i = 0; i < num_tris; i++) accum_moment(acc, tri_moment[i]);
delete [] tri_moment;
covariance_from_accum(C,acc);
#else
get_covariance_triverts(C,&m->tris[first_tri],num_tris);
#endif
Meigen(E, s, C);
// place axes of E in order of increasing s
int min, mid, max;
if (s[0] > s[1]) { max = 0; min = 1; }
else { min = 0; max = 1; }
if (s[2] < s[min]) { mid = min; min = 2; }
else if (s[2] > s[max]) { mid = max; max = 2; }
else { mid = 2; }
McolcMcol(R,0,E,max);
McolcMcol(R,1,E,mid);
R[0][2] = E[1][max]*E[2][mid] - E[1][mid]*E[2][max];
R[1][2] = E[0][mid]*E[2][max] - E[0][max]*E[2][mid];
R[2][2] = E[0][max]*E[1][mid] - E[0][mid]*E[1][max];
// fit the BV
b->FitToTris(R, &m->tris[first_tri], num_tris);
if (num_tris == 1)
{
// BV is a leaf BV - first_child will index a triangle
b->first_child = -(first_tri + 1);
}
else if (num_tris > 1)
{
// BV not a leaf - first_child will index a BV
b->first_child = m->num_bvs;
m->num_bvs+=2;
// choose splitting axis and splitting coord
McolcV(axis,R,0);
#if RAPID2_FIT
mean_from_accum(mean,acc);
#else
get_centroid_triverts(mean,&m->tris[first_tri],num_tris);
#endif
coord = VdotV(axis, mean);
// now split
int num_first_half = split_tris(&m->tris[first_tri], num_tris,
axis, coord);
// recursively build the children
build_recurse(m, m->child(bn)->first_child, first_tri, num_first_half);
build_recurse(m, m->child(bn)->first_child + 1,
first_tri + num_first_half, num_tris - num_first_half);
}
return PQP_OK;
}
