int process_sse (Protein * protein, int type, double ** point, int number_of_points,
int first_res_index, int last_res_index, SSElement * element){
int point_ctr;
int number_of_avgd_points;
double ** point_avg = NULL;
Residue * residue;
if ( type != HELIX && type !=STRAND) {
fprintf (stderr, "Error in %s:%d: attempting a fit "
"on something that is neither strand nor helix.\n",
__FILE__, __LINE__);
exit (1);
}
/* the fields that are straightforward to fill: */
element->type = type;
residue = protein->sequence+first_res_index;
strncpy (element->begin_id, residue->pdb_id, (PDB_ATOM_RES_NO_LEN+2)*sizeof(char));
residue = protein->sequence+last_res_index;
strncpy (element->end_id, residue->pdb_id, (PDB_ATOM_RES_NO_LEN+2)*sizeof(char));
element->chain = residue->chain;
element->begin = first_res_index;
element->end = last_res_index;
element->length = last_res_index-first_res_index+1;
/* fit a line through the points --
fill in the center and direction fields of the element structure*/
if (type== STRAND) {
fit_line (point, number_of_points, element->cm, element->p);
} else {
number_of_avgd_points = number_of_points-2;
if ( ! (point_avg=dmatrix(number_of_avgd_points,3))) return 1;
for (point_ctr = 0; point_ctr < number_of_avgd_points; point_ctr++) {
int i;
for (i=0; i<3; i++) {
point_avg[point_ctr][i] = point[point_ctr][i]
+ point[point_ctr+1][i]
+ point[point_ctr+2][i];
point_avg[point_ctr][i] /= 3;
}
}
fit_line (point_avg, number_of_avgd_points, element->cm, element->p);
free_dmatrix (point_avg);
}
return 0;
}
int main(void)
{
srand(clock());
srand48(clock());
int num_in = 1;
int in_mfs[1] = {3};
int num_out = 1;
int out_mfs[1] = {3};
int num_rule = prod_i(in_mfs, num_in);
int consequents[num_rule * num_out];
int num_params = 3 * (sum_i(in_mfs,num_in) + sum_i(out_mfs,num_out));
double params[num_params];
init_params(params, num_in, in_mfs, num_out, out_mfs);
consequents[0] = 0;
consequents[1] = 1;
consequents[2] = 2;
// rand_params(params, num_in, in_mfs, num_out, out_mfs);
// rand_consequents(num_out, num_rule, consequents, out_mfs);
struct Specs * spcs = specs_set(num_in, in_mfs, num_out, out_mfs);
struct Individual * ind = individual_create(num_params, params, num_rule, num_out, consequents);
struct Fis * fis = individual_to_fis(ind, spcs);
// double x[2] = {0.2, 0.25};
double x[1] = {0.3};
double out[1];
evalfis(out,x,fis);
printf("%f\n",out[0]);
printf("fitness = %f\n",fit_line(fis));
plot_line(fis);
while (1) {
// scanf("%lf %lf", &x[0], &x[1]);
// printf("x = [%f, %f]\n",x[0],x[1]);
if (scanf("%lf", &x[0]) == 0){
printf("x = [%f]\n", x[0]);
}
evalfis(out,x,fis);
printf("%f\n",out[0]);
}
fis_destroy(fis);
specs_clear(spcs);
individual_destroy(ind);
return 0;
}
int *floor1_fit(vorbis_block *vb,vorbis_look_floor1 *look,
const float *logmdct, /* in */
const float *logmask){
long i,j;
vorbis_info_floor1 *info=look->vi;
long n=look->n;
long posts=look->posts;
long nonzero=0;
lsfit_acc fits[VIF_POSIT+1];
int fit_valueA[VIF_POSIT+2]; /* index by range list position */
int fit_valueB[VIF_POSIT+2]; /* index by range list position */
int loneighbor[VIF_POSIT+2]; /* sorted index of range list position (+2) */
int hineighbor[VIF_POSIT+2];
int *output=NULL;
int memo[VIF_POSIT+2];
for(i=0;i<posts;i++)fit_valueA[i]=-200; /* mark all unused */
for(i=0;i<posts;i++)fit_valueB[i]=-200; /* mark all unused */
for(i=0;i<posts;i++)loneighbor[i]=0; /* 0 for the implicit 0 post */
for(i=0;i<posts;i++)hineighbor[i]=1; /* 1 for the implicit post at n */
for(i=0;i<posts;i++)memo[i]=-1; /* no neighbor yet */
/* quantize the relevant floor points and collect them into line fit
structures (one per minimal division) at the same time */
if(posts==0){
nonzero+=accumulate_fit(logmask,logmdct,0,n,fits,n,info);
}else{
for(i=0;i<posts-1;i++)
nonzero+=accumulate_fit(logmask,logmdct,look->sorted_index[i],
look->sorted_index[i+1],fits+i,
n,info);
}
if(nonzero){
/* start by fitting the implicit base case.... */
int y0=-200;
int y1=-200;
fit_line(fits,posts-1,&y0,&y1);
fit_valueA[0]=y0;
fit_valueB[0]=y0;
fit_valueB[1]=y1;
fit_valueA[1]=y1;
/* Non degenerate case */
/* start progressive splitting. This is a greedy, non-optimal
algorithm, but simple and close enough to the best
answer. */
for(i=2;i<posts;i++){
int sortpos=look->reverse_index[i];
int ln=loneighbor[sortpos];
int hn=hineighbor[sortpos];
/* eliminate repeat searches of a particular range with a memo */
if(memo[ln]!=hn){
/* haven't performed this error search yet */
int lsortpos=look->reverse_index[ln];
int hsortpos=look->reverse_index[hn];
memo[ln]=hn;
{
/* A note: we want to bound/minimize *local*, not global, error */
int lx=info->postlist[ln];
int hx=info->postlist[hn];
int ly=post_Y(fit_valueA,fit_valueB,ln);
int hy=post_Y(fit_valueA,fit_valueB,hn);
if(ly==-1 || hy==-1){
exit(1);
}
if(inspect_error(lx,hx,ly,hy,logmask,logmdct,info)){
/* outside error bounds/begin search area. Split it. */
int ly0=-200;
int ly1=-200;
int hy0=-200;
int hy1=-200;
fit_line(fits+lsortpos,sortpos-lsortpos,&ly0,&ly1);
fit_line(fits+sortpos,hsortpos-sortpos,&hy0,&hy1);
/* store new edge values */
fit_valueB[ln]=ly0;
if(ln==0)fit_valueA[ln]=ly0;
fit_valueA[i]=ly1;
fit_valueB[i]=hy0;
fit_valueA[hn]=hy1;
if(hn==1)fit_valueB[hn]=hy1;
if(ly1>=0 || hy0>=0){
/* store new neighbor values */
for(j=sortpos-1;j>=0;j--)
if(hineighbor[j]==hn)
hineighbor[j]=i;
else
break;
for(j=sortpos+1;j<posts;j++)
if(loneighbor[j]==ln)
loneighbor[j]=i;
else
break;
}
}else{
fit_valueA[i]=-200;
fit_valueB[i]=-200;
}
}
}
}
output=_vorbis_block_alloc(vb,sizeof(*output)*posts);
output[0]=post_Y(fit_valueA,fit_valueB,0);
output[1]=post_Y(fit_valueA,fit_valueB,1);
/* fill in posts marked as not using a fit; we will zero
back out to 'unused' when encoding them so long as curve
interpolation doesn't force them into use */
for(i=2;i<posts;i++){
int ln=look->loneighbor[i-2];
int hn=look->hineighbor[i-2];
int x0=info->postlist[ln];
int x1=info->postlist[hn];
int y0=output[ln];
int y1=output[hn];
int predicted=render_point(x0,x1,y0,y1,info->postlist[i]);
int vx=post_Y(fit_valueA,fit_valueB,i);
if(vx>=0 && predicted!=vx){
output[i]=vx;
}else{
output[i]= predicted|0x8000;
}
}
}
return(output);
}
static int floor1_forward(vorbis_block *vb,vorbis_look_floor *in,
float *mdct, const float *logmdct, /* in */
const float *logmask, const float *logmax, /* in */
float *codedflr){ /* out */
static int seq=0;
long i,j,k,l;
vorbis_look_floor1 *look=(vorbis_look_floor1 *)in;
vorbis_info_floor1 *info=look->vi;
long n=info->n;
long posts=look->posts;
long nonzero=0;
lsfit_acc fits[VIF_POSIT+1];
int fit_valueA[VIF_POSIT+2]; /* index by range list position */
int fit_valueB[VIF_POSIT+2]; /* index by range list position */
int fit_flag[VIF_POSIT+2];
int loneighbor[VIF_POSIT+2]; /* sorted index of range list position (+2) */
int hineighbor[VIF_POSIT+2];
int memo[VIF_POSIT+2];
codec_setup_info *ci=vb->vd->vi->codec_setup;
static_codebook **sbooks=ci->book_param;
codebook *books=NULL;
int writeflag=0;
if(vb->vd->backend_state){
books=((backend_lookup_state *)(vb->vd->backend_state))->
fullbooks;
writeflag=1;
}
memset(fit_flag,0,sizeof(fit_flag));
for(i=0;i<posts;i++)loneighbor[i]=0; /* 0 for the implicit 0 post */
for(i=0;i<posts;i++)hineighbor[i]=1; /* 1 for the implicit post at n */
for(i=0;i<posts;i++)memo[i]=-1; /* no neighbor yet */
/* Scan back from high edge to first 'used' frequency */
for(;n>info->unusedmin_n;n--)
if(logmdct[n-1]>-floor1_rangedB &&
logmdct[n-1]+info->twofitatten>logmask[n-1])break;
/* quantize the relevant floor points and collect them into line fit
structures (one per minimal division) at the same time */
if(posts==0){
nonzero+=accumulate_fit(logmask,logmax,0,n,fits,n,info);
}else{
for(i=0;i<posts-1;i++)
nonzero+=accumulate_fit(logmask,logmax,look->sorted_index[i],
look->sorted_index[i+1],fits+i,
n,info);
}
if(nonzero){
/* start by fitting the implicit base case.... */
int y0=-200;
int y1=-200;
int mse=fit_line(fits,posts-1,&y0,&y1);
if(mse<0){
/* Only a single nonzero point */
y0=-200;
y1=0;
fit_line(fits,posts-1,&y0,&y1);
}
fit_flag[0]=1;
fit_flag[1]=1;
fit_valueA[0]=y0;
fit_valueB[0]=y0;
fit_valueB[1]=y1;
fit_valueA[1]=y1;
if(mse>=0){
/* Non degenerate case */
/* start progressive splitting. This is a greedy, non-optimal
algorithm, but simple and close enough to the best
answer. */
for(i=2;i<posts;i++){
int sortpos=look->reverse_index[i];
int ln=loneighbor[sortpos];
int hn=hineighbor[sortpos];
/* eliminate repeat searches of a particular range with a memo */
if(memo[ln]!=hn){
/* haven't performed this error search yet */
int lsortpos=look->reverse_index[ln];
int hsortpos=look->reverse_index[hn];
memo[ln]=hn;
/* if this is an empty segment, its endpoints don't matter.
Mark as such */
for(j=lsortpos;j<hsortpos;j++)
if(fits[j].un)break;
if(j==hsortpos){
/* empty segment; important to note that this does not
break 0/n post case */
fit_valueB[ln]=-200;
if(fit_valueA[ln]<0)
fit_flag[ln]=0;
fit_valueA[hn]=-200;
if(fit_valueB[hn]<0)
fit_flag[hn]=0;
}else{
/* A note: we want to bound/minimize *local*, not global, error */
int lx=info->postlist[ln];
int hx=info->postlist[hn];
int ly=post_Y(fit_valueA,fit_valueB,ln);
int hy=post_Y(fit_valueA,fit_valueB,hn);
if(inspect_error(lx,hx,ly,hy,logmask,logmdct,info)){
/* outside error bounds/begin search area. Split it. */
int ly0=-200;
int ly1=-200;
int hy0=-200;
int hy1=-200;
int lmse=fit_line(fits+lsortpos,sortpos-lsortpos,&ly0,&ly1);
int hmse=fit_line(fits+sortpos,hsortpos-sortpos,&hy0,&hy1);
/* the boundary/sparsity cases are the hard part. They
don't happen often given that we use the full mask
curve (weighted) now, but when they do happen they
can go boom. Pay them detailed attention */
/* cases for a segment:
>=0) normal fit (>=2 unique points)
-1) one point on x0;
one point on x1; <-- disallowed by fit_line
-2) one point in between x0 and x1
-3) no points */
switch(lmse){
case -2:
/* no points in the low segment */
break;
case -1:
ly0=fits[lsortpos].edgey0;
break;
/*default:
break;*/
}
switch(hmse){
case -2:
/* no points in the hi segment */
break;
case -1:
hy0=fits[sortpos].edgey0;
break;
}
/* store new edge values */
fit_valueB[ln]=ly0;
if(ln==0 && ly0>=0)fit_valueA[ln]=ly0;
fit_valueA[i]=ly1;
fit_valueB[i]=hy0;
fit_valueA[hn]=hy1;
if(hn==1 && hy1>=0)fit_valueB[hn]=hy1;
if(ly0<0 && fit_valueA[ln]<0)
fit_flag[ln]=0;
if(hy1<0 && fit_valueB[hn]<0)
fit_flag[hn]=0;
if(ly1>=0 || hy0>=0){
/* store new neighbor values */
for(j=sortpos-1;j>=0;j--)
if(hineighbor[j]==hn)
hineighbor[j]=i;
else
break;
for(j=sortpos+1;j<posts;j++)
if(loneighbor[j]==ln)
loneighbor[j]=i;
else
break;
/* store flag (set) */
fit_flag[i]=1;
}
}
}
}
}
}
/* quantize values to multiplier spec */
switch(info->mult){
case 1: /* 1024 -> 256 */
for(i=0;i<posts;i++)
if(fit_flag[i])
fit_valueA[i]=post_Y(fit_valueA,fit_valueB,i)>>2;
break;
case 2: /* 1024 -> 128 */
for(i=0;i<posts;i++)
if(fit_flag[i])
fit_valueA[i]=post_Y(fit_valueA,fit_valueB,i)>>3;
break;
case 3: /* 1024 -> 86 */
for(i=0;i<posts;i++)
if(fit_flag[i])
fit_valueA[i]=post_Y(fit_valueA,fit_valueB,i)/12;
break;
case 4: /* 1024 -> 64 */
for(i=0;i<posts;i++)
if(fit_flag[i])
fit_valueA[i]=post_Y(fit_valueA,fit_valueB,i)>>4;
break;
}
/* find prediction values for each post and subtract them */
for(i=2;i<posts;i++){
int sp=look->reverse_index[i];
int ln=look->loneighbor[i-2];
int hn=look->hineighbor[i-2];
int x0=info->postlist[ln];
int x1=info->postlist[hn];
int y0=fit_valueA[ln];
int y1=fit_valueA[hn];
int predicted=render_point(x0,x1,y0,y1,info->postlist[i]);
if(fit_flag[i]){
int headroom=(look->quant_q-predicted<predicted?
look->quant_q-predicted:predicted);
int val=fit_valueA[i]-predicted;
/* at this point the 'deviation' value is in the range +/- max
range, but the real, unique range can always be mapped to
only [0-maxrange). So we want to wrap the deviation into
this limited range, but do it in the way that least screws
an essentially gaussian probability distribution. */
if(val<0)
if(val<-headroom)
val=headroom-val-1;
else
val=-1-(val<<1);
else
if(val>=headroom)
val= val+headroom;
else
val<<=1;
fit_valueB[i]=val;
/* unroll the neighbor arrays */
for(j=sp+1;j<posts;j++)
if(loneighbor[j]==i)
loneighbor[j]=loneighbor[sp];
else
break;
for(j=sp-1;j>=0;j--)
if(hineighbor[j]==i)
hineighbor[j]=hineighbor[sp];
else
break;
}else{
fit_valueA[i]=predicted;
fit_valueB[i]=0;
}
if(fit_valueB[i]==0)
fit_valueA[i]|=0x8000;
else{
fit_valueA[look->loneighbor[i-2]]&=0x7fff;
fit_valueA[look->hineighbor[i-2]]&=0x7fff;
}
}
/* we have everything we need. pack it out */
/* mark nontrivial floor */
if(writeflag){
oggpack_write(&vb->opb,1,1);
/* beginning/end post */
look->frames++;
look->postbits+=ilog(look->quant_q-1)*2;
oggpack_write(&vb->opb,fit_valueA[0],ilog(look->quant_q-1));
oggpack_write(&vb->opb,fit_valueA[1],ilog(look->quant_q-1));
/* partition by partition */
for(i=0,j=2;i<info->partitions;i++){
int class=info->partitionclass[i];
int cdim=info->class_dim[class];
int csubbits=info->class_subs[class];
int csub=1<<csubbits;
int bookas[8]={0,0,0,0,0,0,0,0};
int cval=0;
int cshift=0;
/* generate the partition's first stage cascade value */
if(csubbits){
int maxval[8];
for(k=0;k<csub;k++){
int booknum=info->class_subbook[class][k];
if(booknum<0){
maxval[k]=1;
}else{
maxval[k]=sbooks[info->class_subbook[class][k]]->entries;
}
}
for(k=0;k<cdim;k++){
for(l=0;l<csub;l++){
int val=fit_valueB[j+k];
if(val<maxval[l]){
bookas[k]=l;
break;
}
}
cval|= bookas[k]<<cshift;
cshift+=csubbits;
}
/* write it */
look->phrasebits+=
vorbis_book_encode(books+info->class_book[class],cval,&vb->opb);
#ifdef TRAIN_FLOOR1
{
FILE *of;
char buffer[80];
sprintf(buffer,"line_%ldx%ld_class%d.vqd",
vb->pcmend/2,posts-2,class);
of=fopen(buffer,"a");
fprintf(of,"%d\n",cval);
fclose(of);
}
#endif
}
/* write post values */
for(k=0;k<cdim;k++){
int book=info->class_subbook[class][bookas[k]];
if(book>=0){
/* hack to allow training with 'bad' books */
if(fit_valueB[j+k]<(books+book)->entries)
look->postbits+=vorbis_book_encode(books+book,
fit_valueB[j+k],&vb->opb);
/*else
fprintf(stderr,"+!");*/
#ifdef TRAIN_FLOOR1
{
FILE *of;
char buffer[80];
sprintf(buffer,"line_%ldx%ld_%dsub%d.vqd",
vb->pcmend/2,posts-2,class,bookas[k]);
of=fopen(buffer,"a");
fprintf(of,"%d\n",fit_valueB[j+k]);
fclose(of);
}
#endif
}
}
j+=cdim;
}
}
{
/* generate quantized floor equivalent to what we'd unpack in decode */
int hx;
int lx=0;
int ly=fit_valueA[0]*info->mult;
for(j=1;j<posts;j++){
int current=look->forward_index[j];
if(!(fit_valueA[current]&0x8000)){
int hy=(fit_valueA[current]&0x7fff)*info->mult;
hx=info->postlist[current];
render_line0(lx,hx,ly,hy,codedflr);
lx=hx;
ly=hy;
}
}
for(j=lx;j<vb->pcmend/2;j++)codedflr[j]=codedflr[j-1]; /* be certain */
/* use it to create residue vector. Eliminate mdct elements
that were below the error training attenuation relative to
the original mask. This avoids portions of the floor fit
that were considered 'unused' in fitting from being used in
coding residue if the unfit values are significantly below
the original input mask */
for(j=0;j<n;j++)
if(logmdct[j]+info->twofitatten<logmask[j])
mdct[j]=0.f;
for(j=n;j<vb->pcmend/2;j++)mdct[j]=0.f;
}
}else{
if(writeflag)oggpack_write(&vb->opb,0,1);
// return 1 if the quad looks okay, 0 if it should be discarded
int fit_quad(apriltag_detector_t *td, image_u8_t *im, zarray_t *cluster, struct quad *quad)
{
int res = 0;
int sz = zarray_size(cluster);
if (sz < 4) // can't fit a quad to less than 4 points
return 0;
/////////////////////////////////////////////////////////////
// Step 1. Sort points so they wrap around the center of the
// quad. We will constrain our quad fit to simply partition this
// ordered set into 4 groups.
// compute a bounding box so that we can order the points
// according to their angle WRT the center.
int xmax = 0, xmin = 9999999, ymax = 0, ymin = 9999999;
for (int pidx = 0; pidx < zarray_size(cluster); pidx++) {
struct pt *p;
zarray_get_volatile(cluster, pidx, &p);
xmax = imax(xmax, p->x);
xmin = imin(xmin, p->x);
ymax = imax(ymax, p->y);
ymin = imin(ymin, p->y);
}
int cx = (xmin + xmax) / 2;
int cy = (ymin + ymax) / 2;
for (int pidx = 0; pidx < zarray_size(cluster); pidx++) {
struct pt *p;
zarray_get_volatile(cluster, pidx, &p);
p->theta = atan2f(p->y - cy, p->x - cx);
}
zarray_sort(cluster, pt_compare_theta);
// remove duplicate points. (A byproduct of our segmentation system.)
if (1) {
int outpos = 1;
struct pt *last;
zarray_get_volatile(cluster, 0, &last);
for (int i = 1; i < sz; i++) {
struct pt *p;
zarray_get_volatile(cluster, i, &p);
if (p->x != last->x || p->y != last->y) {
if (i != outpos) {
struct pt *out;
zarray_get_volatile(cluster, outpos, &out);
memcpy(out, p, sizeof(struct pt));
}
outpos++;
}
last = p;
}
cluster->size = outpos;
sz = outpos;
}
if (sz < 4)
return 0;
/////////////////////////////////////////////////////////////
// Step 2. Precompute statistics that allow line fit queries to be
// efficiently computed for any contiguous range of indices.
struct line_fit_pt *lfps = (struct line_fit_pt *)calloc(sz, sizeof(struct line_fit_pt));
for (int i = 0; i < sz; i++) {
struct pt *p;
zarray_get_volatile(cluster, i, &p);
if (i > 0) {
memcpy(&lfps[i], &lfps[i-1], sizeof(struct line_fit_pt));
}
double W = 1;
if (p->x > 0 && p->x+1 < im->width && p->y > 0 && p->y+1 < im->height) {
int grad_x = im->buf[p->y * im->stride + p->x + 1] -
im->buf[p->y * im->stride + p->x - 1];
int grad_y = im->buf[(p->y+1) * im->stride + p->x] -
im->buf[(p->y-1) * im->stride + p->x];
W = sqrtf(grad_x*grad_x + grad_y*grad_y) + 1;
}
lfps[i].Mx += W * p->x;
lfps[i].My += W * p->y;
lfps[i].Mxx += W * p->x * p->x;
lfps[i].Mxy += W * p->x * p->y;
lfps[i].Myy += W * p->y * p->y;
lfps[i].W += W;
}
int indices[4];
if (1) {
if (!quad_segment_maxima(td, cluster, lfps, indices))
goto finish;
} else {
if (!quad_segment_agg(td, cluster, lfps, indices))
goto finish;
}
// printf("%d %d %d %d\n", indices[0], indices[1], indices[2], indices[3]);
if (0) {
// no refitting here; just use those points as the vertices.
// Note, this is useful for debugging, but pretty bad in
// practice since this code path also omits several
// plausibility checks that save us tons of time in quad
// decoding.
for (int i = 0; i < 4; i++) {
struct pt *p;
zarray_get_volatile(cluster, indices[i], &p);
quad->p[i][0] = p->x;
quad->p[i][1] = p->y;
}
res = 1;
} else {
double lines[4][4];
for (int i = 0; i < 4; i++) {
int i0 = indices[i];
int i1 = indices[(i+1)&3];
if (0) {
// if there are enough points, skip the points near the corners
// (because those tend not to be very good.)
if (i1-i0 > 8) {
int t = (i1-i0)/6;
if (t < 0)
t = -t;
i0 = (i0 + t) % sz;
i1 = (i1 + sz - t) % sz;
}
}
double err;
fit_line(lfps, sz, i0, i1, lines[i], NULL, &err);
// XXX VALUE?
// printf("%f %d\n", err, i1-i0);
if (err > td->qtp.max_line_fit_mse) {
res = 0;
goto finish;
}
}
for (int i = 0; i < 4; i++) {
// solve for the intersection of lines (i) and (i+1)&3.
// p0 + lambda0*u0 = p1 + lambda1*u1, where u0 and u1
// are the line directions.
//
// lambda0*u0 - lambda1*u1 = (p1 - p0)
//
// rearrange (solve for lambdas)
//
// [u0_x -u1_x ] [lambda0] = [ p1_x - p0_x ]
// [u0_y -u1_y ] [lambda1] [ p1_y - p0_y ]
//
// remember that lines[i][0,1] = p, lines[i][2,3] = NORMAL vector.
// We want the unit vector, so we need the perpendiculars. Thus, below
// we have swapped the x and y components and flipped the y components.
double A00 = lines[i][3], A01 = -lines[(i+1)&3][3];
double A10 = -lines[i][2], A11 = lines[(i+1)&3][2];
double B0 = -lines[i][0] + lines[(i+1)&3][0];
double B1 = -lines[i][1] + lines[(i+1)&3][1];
double det = A00 * A11 - A10 * A01;
// inverse.
double W00 = A11 / det, W01 = -A01 / det;
if (fabs(det) < 0.001) {
res = 0;
goto finish;
}
// solve
double L0 = W00*B0 + W01*B1;
// compute intersection
quad->p[i][0] = lines[i][0] + L0*A00;
quad->p[i][1] = lines[i][1] + L0*A10;
if (0) {
// we should get the same intersection starting
// from point p1 and moving L1*u1.
double W10 = -A10 / det, W11 = A00 / det;
double L1 = W10*B0 + W11*B1;
double x = lines[(i+1)&3][0] - L1*A10;
double y = lines[(i+1)&3][1] - L1*A11;
assert(fabs(x - quad->p[i][0]) < 0.001 &&
fabs(y - quad->p[i][1]) < 0.001);
}
res = 1;
}
}
// reject quads with edges that are too short or long.
if (1) {
for (int i = 0; i < 3; i++) {
double dist2 = sq(quad->p[i][0] - quad->p[i+1][0]) +
sq(quad->p[i][1] - quad->p[i+1][1]);
if (dist2 < sq(6) || dist2 > sq(4096)) {
res = 0;
goto finish;
}
}
}
// reject quads whose cumulative angle change isn't equal to 2PI
if (1) {
double total = 0;
for (int i = 0; i < 4; i++) {
int i0 = i, i1 = (i+1)&3, i2 = (i+2)&3;
double theta0 = atan2f(quad->p[i0][1] - quad->p[i1][1],
quad->p[i0][0] - quad->p[i1][0]);
double theta1 = atan2f(quad->p[i2][1] - quad->p[i1][1],
quad->p[i2][0] - quad->p[i1][0]);
double dtheta = theta0 - theta1;
if (dtheta < 0)
dtheta += 2*M_PI;
if (dtheta < td->qtp.critical_rad || dtheta > (M_PI - td->qtp.critical_rad))
res = 0;
total += dtheta;
}
if (total < 6.2 || total > 6.4) {
res = 0;
goto finish;
}
}
// adjust pixel coordinates; all math up 'til now uses pixel
// coordinates in which (0,0) is the lower left corner. But each
// pixel actually spans from to [x, x+1), [y, y+1) the mean value of which
// is +.5 higher than x & y.
for (int i = 0; i < 4; i++) {
quad->p[i][0] += .5;
quad->p[i][1] += .5;
}
finish:
/*
if (res) {
static image_u8_t *im;
if (im == NULL)
im = image_u8_create(1920,1080);
for (int j = 0; j < 4; j++) {
int i0 = indices[j];
int i1 = indices[(j+1)&3];
if (i1 < i0)
i1 += sz;
for (int i = i0; i <= i1; i++) {
struct pt *p;
zarray_get_volatile(cluster, i % sz, &p);
im->buf[((int) p->y)*im->stride + ((int) p->x)] = 64 + 128*(j%2);
}
}
char name[1024];
sprintf(name, "debug_seg.pnm", utime_now());
image_u8_write_pnm(im, name);
}
*/
free(lfps);
return res;
}
// returns 0 if the cluster looks bad.
int quad_segment_agg(apriltag_detector_t *td, zarray_t *cluster, struct line_fit_pt *lfps, int indices[4])
{
int sz = zarray_size(cluster);
zmaxheap_t *heap = zmaxheap_create(sizeof(struct remove_vertex*));
// We will initially allocate sz rvs. We then have two types of
// iterations: some iterations that are no-ops in terms of
// allocations, and those that remove a vertex and allocate two
// more children. This will happen at most (sz-4) times. Thus we
// need: sz + 2*(sz-4) entries.
int rvalloc_pos = 0;
int rvalloc_size = 3*sz;
struct remove_vertex *rvalloc = (struct remove_vertex *) calloc(rvalloc_size, sizeof(struct remove_vertex));
struct segment *segs = (struct segment *)calloc(sz, sizeof(struct segment));
// populate with initial entries
for (int i = 0; i < sz; i++) {
struct remove_vertex *rv = &rvalloc[rvalloc_pos++];
rv->i = i;
if (i == 0) {
rv->left = sz-1;
rv->right = 1;
} else {
rv->left = i-1;
rv->right = (i+1) % sz;
}
fit_line(lfps, sz, rv->left, rv->right, NULL, NULL, &rv->err);
zmaxheap_add(heap, &rv, -rv->err);
segs[i].left = rv->left;
segs[i].right = rv->right;
segs[i].is_vertex = 1;
}
int nvertices = sz;
while (nvertices > 4) {
assert(rvalloc_pos < rvalloc_size);
struct remove_vertex *rv;
float err;
int res = zmaxheap_remove_max(heap, &rv, &err);
if (!res)
return 0;
assert(res);
// is this remove_vertex valid? (Or has one of the left/right
// vertices changes since we last looked?)
if (!segs[rv->i].is_vertex ||
!segs[rv->left].is_vertex ||
!segs[rv->right].is_vertex) {
continue;
}
// we now merge.
assert(segs[rv->i].is_vertex);
segs[rv->i].is_vertex = 0;
segs[rv->left].right = rv->right;
segs[rv->right].left = rv->left;
// create the join to the left
if (1) {
struct remove_vertex *child = &rvalloc[rvalloc_pos++];
child->i = rv->left;
child->left = segs[rv->left].left;
child->right = rv->right;
fit_line(lfps, sz, child->left, child->right, NULL, NULL, &child->err);
zmaxheap_add(heap, &child, -child->err);
}
// create the join to the right
if (1) {
struct remove_vertex *child = &rvalloc[rvalloc_pos++];
child->i = rv->right;
child->left = rv->left;
child->right = segs[rv->right].right;
fit_line(lfps, sz, child->left, child->right, NULL, NULL, &child->err);
zmaxheap_add(heap, &child, -child->err);
}
// we now have one less vertex
nvertices--;
}
free(rvalloc);
int idx = 0;
for (int i = 0; i < sz; i++) {
if (segs[i].is_vertex) {
indices[idx++] = i;
}
}
free(segs);
return 1;
}
/*
1. Identify A) white points near a black point and B) black points near a white point.
2. Find the connected components within each of the classes above,
yielding clusters of "white-near-black" and
"black-near-white". (These two classes are kept separate). Each
segment has a unique id.
3. For every pair of "white-near-black" and "black-near-white"
clusters, find the set of points that are in one and adjacent to the
other. In other words, a "boundary" layer between the two
clusters. (This is actually performed by iterating over the pixels,
rather than pairs of clusters.) Critically, this helps keep nearby
edges from becoming connected.
*/
int quad_segment_maxima(apriltag_detector_t *td, zarray_t *cluster, struct line_fit_pt *lfps, int indices[4])
{
int sz = zarray_size(cluster);
// ksz: when fitting points, how many points on either side do we consider?
// (actual "kernel" width is 2ksz).
//
// This value should be about: 0.5 * (points along shortest edge).
//
// If all edges were equally-sized, that would give a value of
// sz/8. We make it somewhat smaller to account for tags at high
// aspects.
// XXX Tunable
int ksz = imin(20, sz / 12);
// can't fit a quad if there are too few points.
if (ksz < 2)
return 0;
// printf("sz %5d, ksz %3d\n", sz, ksz);
double errs[sz];
for (int i = 0; i < sz; i++) {
fit_line(lfps, sz, (i + sz - ksz) % sz, (i + ksz) % sz, NULL, &errs[i], NULL);
}
// apply a low-pass filter to errs
if (1) {
double y[sz];
// how much filter to apply?
double sigma = 3;
// cutoff = exp(-j*j/(2*sigma*sigma));
// log(cutoff) = -j*j / (2*sigma*sigma)
// log(cutoff)*2*sigma*sigma = -j*j;
// how big a filter should we use? We make our kernel big
// enough such that we represent any values larger than
// 'cutoff'.
double cutoff = 0.05;
int fsz = sqrt(-log(cutoff)*2*sigma*sigma) + 1;
fsz = 2*fsz + 1;
// For default values of cutoff = 0.05, sigma = 3,
// we have fsz = 17.
float f[fsz];
for (int i = 0; i < fsz; i++) {
int j = i - fsz / 2;
f[i] = exp(-j*j/(2*sigma*sigma));
}
for (int iy = 0; iy < sz; iy++) {
double acc = 0;
for (int i = 0; i < fsz; i++) {
acc += errs[(iy + i - fsz / 2 + sz) % sz] * f[i];
}
y[iy] = acc;
}
memcpy(errs, y, sizeof(y));
}
int maxima[sz];
double maxima_errs[sz];
int nmaxima = 0;
for (int i = 0; i < sz; i++) {
if (errs[i] > errs[(i+1)%sz] && errs[i] > errs[(i+sz-1)%sz]) {
maxima[nmaxima] = i;
maxima_errs[nmaxima] = errs[i];
nmaxima++;
}
}
// if we didn't get at least 4 maxima, we can't fit a quad.
if (nmaxima < 4)
return 0;
// select only the best maxima if we have too many
int max_nmaxima = td->qtp.max_nmaxima;
if (nmaxima > max_nmaxima) {
double maxima_errs_copy[nmaxima];
memcpy(maxima_errs_copy, maxima_errs, sizeof(maxima_errs_copy));
// throw out all but the best handful of maxima. Sorts descending.
qsort(maxima_errs_copy, nmaxima, sizeof(double), err_compare_descending);
double maxima_thresh = maxima_errs_copy[max_nmaxima];
int out = 0;
for (int in = 0; in < nmaxima; in++) {
if (maxima_errs[in] <= maxima_thresh)
continue;
maxima[out++] = maxima[in];
}
nmaxima = out;
}
int best_indices[4];
double best_error = HUGE;
double err01, err12, err23, err30;
double mse01, mse12, mse23, mse30;
double params01[4], params12[4], params23[4], params30[4];
// disallow quads where the angle is less than a critical value.
double max_dot = cos(td->qtp.critical_rad); //25*M_PI/180);
for (int m0 = 0; m0 < nmaxima - 3; m0++) {
int i0 = maxima[m0];
for (int m1 = m0+1; m1 < nmaxima - 2; m1++) {
int i1 = maxima[m1];
fit_line(lfps, sz, i0, i1, params01, &err01, &mse01);
if (mse01 > td->qtp.max_line_fit_mse)
continue;
for (int m2 = m1+1; m2 < nmaxima - 1; m2++) {
int i2 = maxima[m2];
fit_line(lfps, sz, i1, i2, params12, &err12, &mse12);
if (mse12 > td->qtp.max_line_fit_mse)
continue;
double dot = params01[2]*params12[2] + params01[3]*params12[3];
if (fabs(dot) > max_dot)
continue;
for (int m3 = m2+1; m3 < nmaxima; m3++) {
int i3 = maxima[m3];
fit_line(lfps, sz, i2, i3, params23, &err23, &mse23);
if (mse23 > td->qtp.max_line_fit_mse)
continue;
fit_line(lfps, sz, i3, i0, params30, &err30, &mse30);
if (mse30 > td->qtp.max_line_fit_mse)
continue;
double err = err01 + err12 + err23 + err30;
if (err < best_error) {
best_error = err;
best_indices[0] = i0;
best_indices[1] = i1;
best_indices[2] = i2;
best_indices[3] = i3;
}
}
}
}
}
if (best_error == HUGE)
return 0;
for (int i = 0; i < 4; i++)
indices[i] = best_indices[i];
if (best_error / sz < td->qtp.max_line_fit_mse)
return 1;
return 0;
}
