static void trash_filter_sensor_update(carmen_dot_trash_filter_p f,
double x, double y) {
double pxx, pxy, pyx, pyy;
double mxx, mxy, myx, myy;
double kxx, kxy, kyx, kyy;
pxx = f->px;
pxy = f->pxy;
pyx = f->pxy;
pyy = f->py;
// kalman filter sensor update
mxx = pxx + f->rx;
mxy = pxy + f->rxy;
myx = pyx + f->rxy;
myy = pyy + f->ry;
if (invert2d(&mxx, &mxy, &myx, &myy) < 0) {
//carmen_warn("Error: can't invert matrix M");
return;
}
kxx = pxx*mxx+pxy*myx;
kxy = pxx*mxy+pxy*myy;
kyx = pyx*mxx+pyy*myx;
kyy = pyx*mxy+pyy*myy;
f->x = f->x + kxx*(x - f->x) + kxy*(y - f->y);
f->y = f->y + kyx*(x - f->x) + kyy*(y - f->y);
f->px = (1.0 - kxx)*pxx + (-kxy)*pyx;
f->pxy = (1.0 - kxx)*pxy + (-kxy)*pyy;
f->py = (-kyx)*pxy + (1.0 - kyy)*pyy;
}
static void person_filter_sensor_update(carmen_dot_person_filter_p f,
double x, double y) {
double pxx, pxy, pyx, pyy;
double mxx, mxy, myx, myy;
double kxx, kxy, kyx, kyy;
double x2, y2;
//printf("update_person_filter()\n");
pxx = f->px;
pxy = f->pxy;
pyx = f->pxy;
pyy = f->py;
//printf("pxx = %.4f, pxy = %.4f, pyx = %.4f, pyy = %.4f\n", pxx, pxy, pyx, pyy);
// kalman filter sensor update
mxx = pxx + f->rx;
mxy = pxy + f->rxy;
myx = pyx + f->rxy;
myy = pyy + f->ry;
//printf("mxx = %.4f, mxy = %.4f, myx = %.4f, myy = %.4f\n", mxx, mxy, myx, myy);
if (invert2d(&mxx, &mxy, &myx, &myy) < 0) {
//carmen_warn("Error: can't invert matrix M");
return;
}
//printf("mxx = %.4f, mxy = %.4f, myx = %.4f, myy = %.4f\n", mxx, mxy, myx, myy);
kxx = pxx*mxx+pxy*myx;
kxy = pxx*mxy+pxy*myy;
kyx = pyx*mxx+pyy*myx;
kyy = pyx*mxy+pyy*myy;
//printf("kxx = %.4f, kxy = %.4f, kyx = %.4f, kyy = %.4f\n", kxx, kxy, kyx, kyy);
x2 = f->x + kxx*(x - f->x) + kxy*(y - f->y);
y2 = f->y + kyx*(x - f->x) + kyy*(y - f->y);
if (f->hidden_cnt == 0) {
f->vx[f->vpos] = x2 - f->x;
f->vy[f->vpos] = y2 - f->y;
f->vpos = (f->vpos+1) % person_filter_velocity_window;
if (f->vlen < person_filter_velocity_window)
f->vlen++;
else
f->vlen = person_filter_velocity_window;
}
f->x = x2;
f->y = y2;
f->px = (1.0 - kxx)*pxx + (-kxy)*pyx;
f->pxy = (1.0 - kxx)*pxy + (-kxy)*pyy;
f->py = (-kyx)*pxy + (1.0 - kyy)*pyy;
}
static void door_filter_sensor_update(carmen_dot_door_filter_p f, double x, double y) {
double rx, ry, rxy, rt;
double pxx, pxy, pyx, pyy, pt;
double mxx, mxy, myx, myy, mt;
double kxx, kxy, kyx, kyy, kt;
pxx = f->px;
pxy = f->pxy;
pyx = f->pxy;
pyy = f->py;
pt = f->pt; //dbug: + f->qt ?
rx = default_door_filter_rx*cos(f->t)*cos(f->t) +
default_door_filter_ry*sin(f->t)*sin(f->t);
ry = default_door_filter_rx*sin(f->t)*sin(f->t) +
default_door_filter_ry*cos(f->t)*cos(f->t);
rxy = default_door_filter_rx*cos(f->t)*sin(f->t) -
default_door_filter_ry*cos(f->t)*sin(f->t);
rt = default_door_filter_rt;
// kalman filter sensor update
mxx = pxx + rx;
mxy = pxy + rxy;
myx = pyx + rxy;
myy = pyy + ry;
mt = pt + rt;
if (invert2d(&mxx, &mxy, &myx, &myy) < 0) {
carmen_die("Error: can't invert matrix M");
return;
}
mt = 1.0/mt;
kxx = pxx*mxx+pxy*myx;
kxy = pxx*mxy+pxy*myy;
kyx = pyx*mxx+pyy*myx;
kyy = pyx*mxy+pyy*myy;
kt = pt*mt;
f->x = f->x + kxx*(x - f->x) + kxy*(y - f->y);
f->y = f->y + kyx*(x - f->x) + kyy*(y - f->y);
f->px = (1.0 - kxx)*pxx + (-kxy)*pyx;
f->pxy = (1.0 - kxx)*pxy + (-kxy)*pyy;
f->py = (-kyx)*pxy + (1.0 - kyy)*pyy;
f->pt = (1.0 - kt)*pt;
}
MRI_IMAGE *mri_aff2d_byte( MRI_IMAGE *im, int flag ,
float axx, float axy, float ayx, float ayy )
{
float bxx,bxy,byx,byy , xbase,ybase , xx,yy , fx,fy ;
float f_j00,f_jp1 , wt_00,wt_p1 ;
int ii,jj , nx,ny , ix,jy ;
MRI_IMAGE *newImg ;
byte *far , *nar ;
ENTRY("mri_aff2d_byte") ;
if( im == NULL || !MRI_IS_2D(im) || im->kind != MRI_byte ){
fprintf(stderr,"*** mri_aff2d_byte only works on 2D byte images!\n");
RETURN( NULL );
}
if( flag == 0 ){
invert2d( axx,axy,ayx,ayy , &bxx,&bxy,&byx,&byy ) ;
} else {
bxx = axx ; bxy = axy ; byx = ayx ; byy = ayy ;
}
if( (bxx == 0.0 && bxy == 0.0) || (byx == 0.0 && byy == 0.0) ){
fprintf(stderr,"*** mri_aff2d_byte: input matrix is singular!\n") ;
RETURN( NULL );
}
nx = im->nx ; ny = im->ny ;
xbase = 0.5*nx*(1.0-bxx) - 0.5*ny*bxy ;
ybase = 0.5*ny*(1.0-byy) - 0.5*nx*byx ;
far = MRI_BYTE_PTR(im) ; /* input image data */
newImg = mri_new( nx , nx , MRI_byte ) ; /* output image */
nar = MRI_BYTE_PTR(newImg) ; /* output image data */
/*** loop over output points and warp to them ***/
for( jj=0 ; jj < nx ; jj++ ){
xx = xbase-bxx + bxy * jj ;
yy = ybase-byx + byy * jj ;
for( ii=0 ; ii < nx ; ii++ ){
xx += bxx ; /* get x,y in original image */
yy += byx ;
ix = (xx >= 0.0) ? ((int) xx) : ((int) xx)-1 ; /* floor */
jy = (yy >= 0.0) ? ((int) yy) : ((int) yy)-1 ;
fx = xx-ix ; wt_00 = 1.0 - fx ; wt_p1 = fx ;
if( ix >= 0 && ix < nx-1 && jy >= 0 && jy < ny-1 ){
byte *fy00 , *fyp1 ;
fy00 = far + (ix + jy*nx) ; fyp1 = fy00 + nx ;
f_j00 = wt_00 * fy00[0] + wt_p1 * fy00[1] ;
f_jp1 = wt_00 * fyp1[0] + wt_p1 * fyp1[1] ;
} else {
f_j00 = wt_00 * FINS(ix,jy ) + wt_p1 * FINS(ix+1,jy ) ;
f_jp1 = wt_00 * FINS(ix,jy+1) + wt_p1 * FINS(ix+1,jy+1) ;
}
fy = yy-jy ; nar[ii+jj*nx] = (1.0-fy) * f_j00 + fy * f_jp1 ;
}
}
MRI_COPY_AUX(newImg,im) ;
RETURN( newImg ) ;
}
