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 ) ; }