void dd (header *hd)
{ header *st=hd,*hd1,*result;
int c1,c2,i,j,r;
double *m1,*m2,*mr;
complex *mc1,*mc2,*mcr,hc1,hc2;
interval *mi1,*mi2,*mir,hi1,hi2;
hd1=next_param(st);
equal_params_2(&hd,&hd1); if (error) return;
getmatrix(hd,&r,&c1,&m1); if (r!=1) wrong_arg();
getmatrix(hd1,&r,&c2,&m2); if (r!=1) wrong_arg();
if (c1!=c2) wrong_arg();
if (iscomplex(hd)) /* complex values */
{ mc1=(complex *)m1; mc2=(complex *)m2;
result=new_cmatrix(1,c1,""); if (error) return;
mcr=(complex *)matrixof(result);
memmove((char *)mcr,(char *)mc2,c1*sizeof(complex));
for (i=1; i<c1; i++)
{ for (j=c1-1; j>=i; j--)
{ if (mc1[j][0]==mc1[j-i][0] &&
mc1[j][1]==mc1[j-i][1]) wrong_arg();
c_sub(mcr[j],mcr[j-1],hc1);
c_sub(mc1[j],mc1[j-i],hc2);
c_div(hc1,hc2,mcr[j]);
}
}
}
else if (isinterval(hd)) /* complex values */
{ mi1=(complex *)m1; mi2=(complex *)m2;
result=new_imatrix(1,c1,""); if (error) return;
mir=(interval *)matrixof(result);
memmove((char *)mir,(char *)mi2,c1*sizeof(interval));
for (i=1; i<c1; i++)
{ for (j=c1-1; j>=i; j--)
{ i_sub(mir[j],mir[j-1],hi1);
if (hi1[0]<=0 && hi1[1]>=0)
{ output("Interval points coincide\n");
error=1; return;
}
i_sub(mi1[j],mi1[j-i],hi2);
i_div(hi1,hi2,mir[j]);
}
}
}
else if (isreal(hd))
{ result=new_matrix(1,c1,""); if (error) return;
mr=matrixof(result);
memmove((char *)mr,(char *)m2,c1*sizeof(double));
for (i=1; i<c1; i++)
{ for (j=c1-1; j>=i; j--)
{ if (m1[j]==m1[j-i]) wrong_arg();
mr[j]=(mr[j]-mr[j-1])/(m1[j]-m1[j-i]);
}
}
}
else wrong_arg();
moveresult(st,result);
}
void polymult (header *hd)
{ header *st=hd,*hd1,*result;
int c,c1,c2,i,r,j,k;
double *m1,*m2,*mr,x;
complex *mc1,*mc2,*mcr,xc,hc;
interval *mi1,*mi2,*mir,xi,hi;
hd1=next_param(st);
equal_params_2(&hd,&hd1); if (error) return;
getmatrix(hd,&r,&c1,&m1); if (r!=1) wrong_arg();
getmatrix(hd1,&r,&c2,&m2); if (r!=1) wrong_arg();
if ((LONG)c1+c2-1>INT_MAX) wrong_arg();
c=c1+c2-1;
if (iscomplex(hd))
{ mc1=(complex *)m1; mc2=(complex *)m2;
result=new_cmatrix(1,c,""); if (error) return;
mcr=(complex *)matrixof(result);
c_copy(xc,*mc1); mc1++;
for (i=0; i<c2; i++) c_mult(xc,mc2[i],mcr[i]);
for (j=1; j<c1; j++)
{ c_copy(xc,*mc1); mc1++;
for (k=j,i=0; i<c2-1; i++,k++)
{ c_mult(xc,mc2[i],hc);
c_add(hc,mcr[k],mcr[k]);
}
c_mult(xc,mc2[i],mcr[k]);
}
}
else if (isinterval(hd))
{ mi1=(interval *)m1; mi2=(interval *)m2;
result=new_imatrix(1,c,""); if (error) return;
mir=(interval *)matrixof(result);
i_copy(xi,*mi1); mi1++;
for (i=0; i<c2; i++) i_mult(xi,mi2[i],mir[i]);
for (j=1; j<c1; j++)
{ i_copy(xi,*mi1); mi1++;
for (k=j,i=0; i<c2-1; i++,k++)
{ i_mult(xi,mi2[i],hi);
c_add(hi,mir[k],mir[k]);
}
c_mult(xi,mi2[i],mir[k]);
}
}
else if (isreal(hd))
{ result=new_matrix(1,c,""); if (error) return;
mr=matrixof(result);
x=*m1++;
for (i=0; i<c2; i++) mr[i]=x*m2[i];
for (j=1; j<c1; j++)
{ x=*m1++;
for (k=j,i=0; i<c2-1; i++,k++) mr[k]+=x*m2[i];
mr[k]=x*m2[i];
}
}
else wrong_arg();
moveresult(st,result);
}
void polyadd (header *hd)
{ header *st=hd,*hd1,*result;
int c,c1,c2,i,r;
double *m1,*m2,*mr;
complex *mc1,*mc2,*mcr;
interval *mi1,*mi2,*mir;
hd1=next_param(st);
equal_params_2(&hd,&hd1); if (error) return;
getmatrix(hd,&r,&c1,&m1); if (r!=1) wrong_arg();
getmatrix(hd1,&r,&c2,&m2); if (r!=1) wrong_arg();
c=max(c1,c2);
if (iscomplex(hd)) /* complex values */
{ mc1=(complex *)m1; mc2=(complex *)m2;
result=new_cmatrix(1,c,""); if (error) return;
mcr=(complex *)matrixof(result);
for (i=0; i<c; i++)
{ if (i>=c1) { c_copy(*mcr,*mc2); mcr++; mc2++; }
else if (i>=c2) { c_copy(*mcr,*mc1); mcr++; mc1++; }
else { c_add(*mc1,*mc2,*mcr); mc1++; mc2++; mcr++; }
}
}
else if (isinterval(hd))
{ mi1=(interval *)m1; mi2=(interval *)m2;
result=new_imatrix(1,c,""); if (error) return;
mir=(interval *)matrixof(result);
for (i=0; i<c; i++)
{ if (i>=c1) { i_copy(*mir,*mi2); mir++; mi2++; }
else if (i>=c2) { i_copy(*mir,*mi1); mir++; mi1++; }
else { i_add(*mi1,*mi2,*mir); mi1++; mi2++; mir++; }
}
}
else if (isreal(hd))
{ result=new_matrix(1,c,""); if (error) return;
mr=matrixof(result);
for (i=0; i<c; i++)
{ if (i>=c1) { *mr++ = *m2++; }
else if (i>=c2) { *mr++ = *m1++; }
else { *mr++ = *m1++ + *m2++; }
}
}
else wrong_arg();
moveresult(st,result);
}
void polydiv (header *hd)
{ header *st=hd,*hd1,*result,*rest;
int c1,c2,i,r,j;
double *m1,*m2,*mr,*mh,x,l;
complex *mc1,*mc2,*mcr,*mch,xc,lc,hc;
interval *mi1,*mi2,*mir,*mih,xi,li,hi;
hd1=next_param(st);
equal_params_2(&hd,&hd1); if (error) return;
getmatrix(hd,&r,&c1,&m1); if (r!=1) wrong_arg();
getmatrix(hd1,&r,&c2,&m2); if (r!=1) wrong_arg();
if (c1<c2)
{ result=new_real(0.0,"");
rest=(header *)newram;
moveresult(rest,hd1);
}
else if (iscomplex(hd))
{ mc1=(complex *)m1; mc2=(complex *)m2;
result=new_cmatrix(1,c1-c2+1,""); if (error) return;
mcr=(complex *)matrixof(result);
rest=new_cmatrix(1,c2,""); if (error) return;
mch=(complex *)newram;
if (!freeram(c1*sizeof(complex)))
{ output("Out of memory!\n"); error=190; return;
}
memmove((char *)mch,(char *)mc1,c1*sizeof(complex));
c_copy(lc,mc2[c2-1]);
if (lc[0]==0.0 && lc[1]==0.0) wrong_arg();
for (i=c1-c2; i>=0; i--)
{ c_div(mch[c2+i-1],lc,xc); c_copy(mcr[i],xc);
for(j=0; j<c2; j++)
{ c_mult(mc2[j],xc,hc);
c_sub(mch[i+j],hc,mch[i+j]);
}
}
memmove((char *)matrixof(rest),(char *)mch,c2*sizeof(complex));
}
else if (isinterval(hd))
{ mi1=(interval *)m1; mi2=(interval *)m2;
result=new_imatrix(1,c1-c2+1,""); if (error) return;
mir=(interval *)matrixof(result);
rest=new_imatrix(1,c2,""); if (error) return;
mih=(complex *)newram;
if (!freeram(c1*sizeof(complex)))
{ output("Out of memory!\n"); error=190; return;
}
memmove((char *)mih,(char *)mi1,c1*sizeof(interval));
i_copy(li,mi2[c2-1]);
if (li[0]<=0.0 && li[1]>=0.0) wrong_arg();
for (i=c1-c2; i>=0; i--)
{ i_div(mih[c2+i-1],li,xi); c_copy(mir[i],xi);
for(j=0; j<c2; j++)
{ i_mult(mi2[j],xi,hi);
i_sub(mih[i+j],hi,mih[i+j]);
}
}
memmove((char *)matrixof(rest),(char *)mih,c2*sizeof(interval));
}
else if (isreal(hd))
{ result=new_matrix(1,c1-c2+1,""); if (error) return;
mr=matrixof(result);
rest=new_matrix(1,c2,""); if (error) return;
mh=(double *)newram;
if (!freeram(c1*sizeof(double)))
{ output("Out of memory!\n"); error=190; return;
}
memmove((char *)mh,(char *)m1,c1*sizeof(double));
l=m2[c2-1];
if (l==0.0) wrong_arg();
for (i=c1-c2; i>=0; i--)
{ x=mh[c2+i-1]/l; mr[i]=x;
for(j=0; j<c2; j++) mh[i+j]-=m2[j]*x;
}
memmove((char *)matrixof(rest),(char *)mh,c2*sizeof(double));
}
else wrong_arg();
moveresult(st,result);
moveresult(nextof(st),rest);
}
static void update_evidence_grid(carmen_shape_p contour, int *laser_mask, double resolution, double laser_stdev) {
double xmin, xmax, ymin, ymax;
double dx, dy, dmax;
double *ux, *uy;
double **mask;
int i, j, x, y, x0, x1, y0, y1;
int new_x_size, new_y_size;
int mask_x_size, mask_y_size, mask_ux, mask_uy;
int mask_x0, mask_x1, mask_y0, mask_y1;
int x_offset, y_offset;
double ltheta;
printf("update_evidence_grid()\n");
ux = contour->x;
uy = contour->y;
// find bounds for contour's evidence grid
xmin = xmax = ux[0];
ymin = ymax = uy[0];
for (i = 1; i < contour->num_points; i++) {
if (ux[i] > xmax)
xmax = ux[i];
else if (ux[i] < xmin)
xmin = ux[i];
if (uy[i] > ymax)
ymax = uy[i];
else if (uy[i] < ymin)
ymin = uy[i];
}
// give a buffer for the egrid
xmin -= 2 * EGRID_MASK_STDEV_MULT * laser_stdev;
ymin -= 2 * EGRID_MASK_STDEV_MULT * laser_stdev;
xmax += 2 * EGRID_MASK_STDEV_MULT * laser_stdev;
ymax += 2 * EGRID_MASK_STDEV_MULT * laser_stdev;
new_x_size = floor((xmax-xmin) / resolution);
new_y_size = floor((ymax-ymin) / resolution);
//egrid_x_offset = -xmin;
//egrid_y_offset = -ymin;
if (egrid == NULL) { // new egrid
egrid = new_matrix(egrid_x_size, egrid_y_size);
egrid_counts = new_imatrix(egrid_x_size, egrid_y_size);
egrid_hits = new_imatrix(egrid_x_size, egrid_y_size);
for (i = 0; i < egrid_x_size; i++) {
for (j = 0; j < egrid_y_size; j++) {
egrid_counts[i][j] = egrid_hits[i][j] = 0;
egrid[i][j] = -1.0;
}
}
egrid_x_size = new_x_size;
egrid_y_size = new_y_size;
egrid_x_offset = -xmin;
egrid_y_offset = -ymin;
}
else { // resize
x0 = floor((xmin - egrid_x_offset)/resolution);
x1 = floor((xmax - egrid_x_offset)/resolution) + 1;
y0 = floor((ymin - egrid_y_offset)/resolution);
y1 = floor((ymax - egrid_y_offset)/resolution) + 1;
if (x0 < 0 || x1 > egrid_x_size || y0 < 0 || y1 > egrid_y_size) {
x0 = MAX(-x0, 0);
x1 = MAX(x1, egrid_x_size);
y0 = MAX(-y0, 0);
y1 = MAX(y1, egrid_y_size);
egrid = matrix_resize(egrid, x0, y0, egrid_x_size, egrid_y_size, x0+x1, y0+y1, -1.0);
egrid_counts = imatrix_resize(egrid_counts, x0, y0, egrid_x_size, egrid_y_size, x0+x1, y0+y1, 0);
egrid_hits = imatrix_resize(egrid_hits, x0, y0, egrid_x_size, egrid_y_size, x0+x1, y0+y1, 0);
egrid_x_size = x0 + x1;
egrid_y_size = y0 + y1;
egrid_x_offset -= x0*resolution;
egrid_y_offset -= y0*resolution;
}
}
// trace laser
for (i = 0; i < laser_msg.num_readings; i++) {
ltheta = carmen_normalize_theta(laser->theta + (i-90)*M_PI/180.0);
trace_laser_beam(laser->x, laser->y, ltheta, laser->range[i], laser_mask[i]);
}
/***************************** dbug **********************************
for (i = 0; i < egrid_x_size; i++)
for (j = 0; j < egrid_y_size; j++)
egrid[i][j] = LOW_LOG_PROB;
// create a log prob mask of odd dimensions (so there will be a unique center cell)
mask_x_size = floor(2 * EGRID_MASK_STDEV_MULT * laser_stdev / resolution);
if (mask_x_size % 2 == 0)
mask_x_size++;
mask_y_size = floor(2 * EGRID_MASK_STDEV_MULT * laser_stdev / resolution);
if (mask_y_size % 2 == 0)
mask_y_size++;
mask_ux = floor(mask_x_size / 2.0);
mask_uy = floor(mask_y_size / 2.0);
//printf("mask_size = (%d %d)\n", mask_x_size, mask_y_size);
mask = new_matrix(mask_x_size, mask_y_size);
dmax = mask_x_size*mask_x_size*resolution*resolution/4.0;
for (i = 0; i < mask_x_size; i++) {
dx = (i - mask_ux) * resolution;
for (j = 0; j < mask_y_size; j++) {
dy = (j - mask_uy) * resolution;
if (dx*dx+dy*dy <= dmax) // circular mask
mask[i][j] = -(dx*dx+dy*dy) / (2 * laser_stdev * laser_stdev);
else
mask[i][j] = LOW_LOG_PROB;
}
}
for (i = 0; i < contour->num_points; i++) {
x = floor((ux[i] - xmin) / resolution);
y = floor((uy[i] - ymin) / resolution);
if (x < 0 || x >= egrid_x_size || y < 0 || y >= egrid_y_size) {
carmen_warn("Warning: contour point out of evidence grid bounds in get_evidence_grid()\n");
continue;
}
mask_x0 = MAX(0, x + (mask_ux+1) - mask_x_size);
mask_x1 = MIN(egrid_x_size - 1, x - mask_ux + mask_x_size);
mask_y0 = MAX(0, y + (mask_uy+1) - mask_y_size);
mask_y1 = MIN(egrid_y_size - 1, y - mask_uy + mask_y_size);
x_offset = mask_ux - x;
y_offset = mask_uy - y;
for (x = mask_x0; x < mask_x1; x++)
for (y = mask_y0; y < mask_y1; y++)
if (egrid[x][y] < mask[x+x_offset][y+y_offset])
egrid[x][y] = mask[x+x_offset][y+y_offset];
}
free(mask);
//*egrid_x_offset = -xmin;
//*egrid_y_offset = -ymin;
//*egrid_x_size_ptr = egrid_x_size;
//*egrid_y_size_ptr = egrid_y_size;
***************************** end dbug *****************************/
return egrid;
}
