Uint32 _vmem_get_next_4MB_address(void)
{
int i;
int memory_size = BITMAP_MAX/32;
//make sure the we check every 32 index because that is how many word reprsenting 4KB pages it take to make 4MB
for( i =(BITMAP_NORMAL/32); i < memory_size; i++ )
{
int l;
int end = i * 32 + 32;
Uint8 flag = TRUE;
//loop over all 32 words
for ( l=i*32; l < end; l++ )
{
if( 0xFFFFFFFF != _vmem_bitmap_start[l] )
{
flag = FALSE;
break;
}
}
//if the flag is still true we found an empty 4MB chunk
if( flag )
{
return _vmem_get_4MB_address(i);
}
}
__panic( "vmem: OUT OF PHYISCAL MEMORY :(" );
return 0x00;
}
Thread threadGetCurrent(void)
{
ThreadVars* tv = getThreadVars();
if (tv->magic != THREADVARS_MAGIC)
__panic();
return tv->thread_ptr;
}
void _vmem_clear_address( Uint32 address )
{
Uint32 index[1];
Uint8 index2[1];
_vmem_address_calc( address, index, index2);
if ( _vmem_read_bit( _vmem_bitmap_start, *index, *index2) == 1)
{
__panic( "Vmem: address already cleared");
}
_vmem_set_bit( _vmem_bitmap_start, *index, *index2 );
}
void _vmem_set_address( Uint32 address )
{
Uint32 index[1];
Uint8 index2[1];
_vmem_address_calc( address, index, index2);
if ( _vmem_read_bit( _vmem_bitmap_start, *index, *index2) == 0)
{
__panic( "Vmem: address already in use");
}
_vmem_clear_bit( _vmem_bitmap_start, *index, *index2);
//c_printf("%x %d %d ", address, *index, *index2);
}
void threadExit(int rc)
{
Thread t = threadGetCurrent();
if (!t)
__panic();
t->finished = true;
if (t->detached)
threadFree(t);
else
t->rc = rc;
svcExitThread();
}
/*
** Name: __default_expected_handler
**
** Arguments: The usual ISR arguments
**
** Returns: The usual ISR return value
**
** Description: Default handler for interrupts we expect may occur but
** are not handling (yet). Just reset the PIC and return.
*/
static void __default_expected_handler( int vector, int code ){
if( vector >= 0x20 && vector < 0x30 ){
__outb( PIC_MASTER_CMD_PORT, PIC_EOI );
if( vector > 0x28 ){
__outb( PIC_SLAVE_CMD_PORT, PIC_EOI );
}
}
else {
/*
** All the "expected" interrupts will be handled by the
** code above. If we get down here, the isr table may
** have been corrupted. Print message and don't return.
*/
__panic( "Unexpected \"expected\" interrupt!" );
}
}
/**
* Asynchronously writes the specified byte sequence to the serial console.
* @param buffer the data to write from
* @param nbytes the number of bytes to write
*/
void sio_write(char *buffer, unsigned int nbytes) {
//c_printf("Writing:%s:%d %d-%d\n", buffer, nbytes, write_buf_start, write_buf_end);
int spaceleft = WRITE_BUF_LEN - ((write_buf_end - write_buf_start) %
WRITE_BUF_LEN + WRITE_BUF_LEN) % WRITE_BUF_LEN;
if (spaceleft < nbytes) {
__panic("No space left in serial output buffer\n");
}
int index = write_buf_end;
unsigned int count;
for (count = 0; count < nbytes; ++count) {
write_buf[index] = buffer[count];
++write_buf_end;
write_buf_end %= WRITE_BUF_LEN;
index = (index + 1) % WRITE_BUF_LEN;
}
write_start();
}
Uint32 _vmem_get_next_reserve_address(void)
{
Uint32 i;
Uint32 memory_size = BITMAP_NORMAL;
Uint32 start = 0;
for( i = start; i < memory_size; i++ )
{
//if it is any other thing but zero one bit must be on and therfore free
if( 0 != _vmem_bitmap_start[i] )
{
//find the sub index for the given word
return _vmem_get_address(i,_vmem_bsf(_vmem_bitmap_start[i]));
}
}
__panic( "vmem: OUT OF RESERVE ADDRESSES");
return 0x0;
}
Uint32 _vmem_get_next_address(void)
{
int i;
int memory_size = BITMAP_MAX;
for( i = BITMAP_NORMAL; i < memory_size; i++ )
{
//if it is any other thing but zero one bit must be on and therfore free
if( 0 != _vmem_bitmap_start[i] )
{
#ifdef _VMEM_DEBUG
c_printf( "%x %d\n", _vmem_bitmap_start[i],_vmem_bsf(_vmem_bitmap_start[i]));
#endif
//find the sub index for the given word
return _vmem_get_address(i,_vmem_bsf(_vmem_bitmap_start[i]));
}
}
__panic( "vmem: OUT OF PHYISCAL MEMORY :(" );
return 0x0;
}
void _kpanic( char *mod, char *msg, Status code ) {
c_puts( "\n\n***** KERNEL PANIC *****\n\n" );
c_printf( "Module: %s\n", mod );
if( msg != NULL ) {
c_printf( msg, _kstatus(code) );
c_putchar( '\n' );
}
if( code >= STATUS_SENTINEL ) {
c_printf( "*** bad code %d\n", code );
}
//
// This might be a good place to do a stack frame
// traceback
//
__panic( "KERNEL PANIC" );
}
/*!
Implements the estimation of a 2D parametric motion model.
This method differs from estimate(const CMotion2DPyramid &, const
CMotion2DPyramid &, const short *, short, CMotion2DModel &) only in the type
of the estimation support \f$R_t\f$ which is here \e unsigned \e char.
Return true if the estimation process succeeds, false otherwise.
\sa estimate(const CMotion2DPyramid &, const CMotion2DPyramid &, const short *, short, CMotion2DModel &)
*/
bool CMotion2DEstimator::estimate(const CMotion2DPyramid & pyramid1,
const CMotion2DPyramid & pyramid2,
const unsigned char *support,
unsigned char label,
CMotion2DModel & model,
bool useModelAsInitialization)
{
int nrows_pyr1, ncols_pyr1;
int nrows_pyr2, ncols_pyr2;
if (pyramid1.getNumberOfRows(nrows_pyr1) != PYR_NO_ERROR)
return false;
if (pyramid1.getNumberOfCols(ncols_pyr1) != PYR_NO_ERROR)
return false;
if (pyramid2.getNumberOfRows(nrows_pyr2) != PYR_NO_ERROR)
return false;
if (pyramid2.getNumberOfCols(ncols_pyr2) != PYR_NO_ERROR)
return false;
if ((nrows_pyr1 != nrows_pyr2) || (ncols_pyr1 != ncols_pyr2)) {
char message[FILENAME_MAX];
sprintf(message, "CMotion2DEstimator::estimate() \n\tError: Pyramid size differ");
__panic(message);
return false;
}
int size = nrows_pyr1 * ncols_pyr1;
short *s_support = new short [ size ];
bool state;
// Initialize the estimation support
cast_uchar_short(support, s_support, size);
state = estimate(pyramid1, pyramid2, s_support, (short) label,
model, useModelAsInitialization);
delete [] s_support;
return (state);
}
void _kpanic( char *mod, char *msg, status_t code ) {
c_puts( "\n\n***** KERNEL PANIC *****\n\n" );
c_printf( "Module: %s\n", mod );
if( msg != NULL ) {
c_printf( msg, _kstatus(code) );
c_putchar( '\n' );
}
if( code >= N_STATUS ) {
c_printf( "*** bad code %d\n", code );
}
//
// This might be a good place to do a stack frame
// traceback
//
// dump out all the queues
_q_dump_all();
__panic( "KERNEL PANIC" );
}
Uint32 _vmem_first_4MB( void )
{
if( __get_end() >= PAGE_TABLE_SIZE - sizeof(Uint32) ){
__panic( "Houston we have a problem, the kernel is too fat.");
}
//get the end of memory and get the next aligned address
_vmem_page_dir = (Uint32*)((__get_end() & PAGE_ADDRESS_LOC) + PAGE_SIZE);
#ifdef _VMEM_DEBUG
c_printf( "%x\n", (__get_end() & PAGE_ADDRESS_LOC) + PAGE_SIZE ) ;
#endif
//initialize the directory so no pages are prsent
int i;
for(i = 0; i < 1024; i++ )
{
_vmem_page_dir[i] = PAGE_DIR_WRITE;
}
//maps the first 4 MBs 0x400000
_vmem_page_dir[0] = (Uint32) 0x00 | PAGE_DIR_PRESENT | PAGE_DIR_WRITE | PAGE_DIR_SIZE;
//send the pdt to be placed in the correct stop and pagging turned on
_vmem_turnon((Uint32)_vmem_page_dir);
#ifdef _VMEM_DEBUG
c_printf( "\n\nEnd is at position %x \n", __get_end() );
c_printf( "Aligned address is %x \n", _vmem_page_dir);
//c_printf( "First page table address is %x \n", pageTableStart);
c_printf( "Cr0 is %x \n", _vmem_getcr0());
c_printf( "Cr0 High is %x \n", ( (_vmem_getcr0()>>31) & 0x01));
#endif
//return address;
return PAGE_TABLE_SIZE;
}
void _vmem_init_bitmap( Uint32 addr )
{
//make sure we start the bitmap where we think we should
if ( addr != PAGE_TABLE_SIZE )
{
__panic( "vmem: bitmap start is not where expected" );
}
//maps the next 4 MBs
_vmem_page_dir[1] = (Uint32) PAGE_TABLE_SIZE | PAGE_DIR_PRESENT | PAGE_DIR_WRITE | PAGE_DIR_SIZE;
//initilize all bits to be set 1 in the bitmap (aka the address is free)
int i;
for( i = 0; i < (PAGE_TABLE_SIZE/32); i++ )
{
_vmem_bitmap_start[i] = 0xFFFFFFFF;
}
//set the first 8MB are in use
_vmem_set_4MB_address( 0x00 );
_vmem_set_4MB_address( PAGE_TABLE_SIZE );
#ifdef _VMEM_DEBUG
c_printf("Bitmap ending init addr: %x \n", _vmem_bitmap_start );
c_printf(" %x : ", PAGE_TABLE_SIZE);
c_printf(" %x : ", PAGE_SIZE);
c_printf( "Ending of bitmap table is %x \n", addr);
c_printf("0: %x \n", _vmem_page_dir[0] );
c_printf("1: %x \n", _vmem_page_dir[1] );
c_printf( "Bitmap entry 0 is %x \n", _vmem_bitmap_start[0]);
c_printf( "Bitmap entry 0 is %x \n", _vmem_read_bit( _vmem_bitmap_start, 0, 0));
c_printf( "Bitmap entry 1 is %x \n", _vmem_read_bit( _vmem_bitmap_start, 0, 1));
c_printf( "Bitmap entry 2 is %x \n", _vmem_read_bit( _vmem_bitmap_start, 0, 2));
c_printf( "Bitmap entry n is %x \n", _vmem_read_bit( _vmem_bitmap_start, 63, 32));
c_printf( "Bitmap entry n is %x \n", _vmem_read_bit( _vmem_bitmap_start, 64, 0));
#endif
}
/*!
Implements the estimation of a 2D parametric motion model.
Depending on setRobustEstimator() two different schemes can be used: either
involving a robust multiresolution estimation implementing the IRLS
technique, or involving a multiresolution least-mean-square estimation.
The estimation process is performed between images \f$I_t\f$ and
\f$I_{t+1}\f$ for which the Gaussian and image gradient pyramids, denoted
respectively \e pyramid1 and \e pyramid2, must be built using
CMotion2DPyramid.
The parameter \e support refers to the estimation support \f$R_t\f$. In other
terms, \e support indicates which pixels of the image \f$I_t\f$ are taken
into account in the estimation process. The size of \e support must be equal
to the size of the finest level in the pyramids (\e pyramid1 or \e
pyramid2). It usually consists of the whole image. In that case, all \e
support must be set to \e label. But, if required, it can also be restricted
to a specific area of the image. In that case, all the pixels belonging to
this area must be set to \e label. The other pixels must be set to another
value than \e label.
An initialization of the motion \e model parameters does not affect the
results. That means that the parameters of the motion model given as input
are not used to initialize the estimator.
This function returns the estimated 2D parametric motion model in \e
model. The returned parameters have to be considered for the highest
resolution in the pyramids (level = 0) even if the last level considered in
the estimation process is different when specified for example with
setLastEstimationLevel(1). If the estimation process is stopped at a pyramid
level different from 0, a projection of the motion model parameter to level 0
is performed. For the projection from one given level to the next finer one,
the constant parameters of the model are multiplied by 2, the affine
parameters are unchanged and the quadratic parameters are divided by 2.
The weight map can be obtained by getWeights().
Returns true if the estimation process succeeds, false otherwise.
\sa estimate(const CMotion2DPyramid &, const CMotion2DPyramid &, const unsigned char *, unsigned char, CMotion2DModel &), setRobustEstimator(), setFirstEstimationLevel(),
setLastEstimationLevel(), getWeights(),
*/
bool CMotion2DEstimator::estimate(const CMotion2DPyramid & pyramid1,
const CMotion2DPyramid & pyramid2,
const short *support, short label,
CMotion2DModel & model,
bool useModelAsInitialization)
{
if (DEBUG_LEVEL1)
cout << "Begin CMotion2DEstimator::estimate()" << endl;
Para paramet;
float ligravbase = 0.; /* Coordonnees du centre de gravite des regions.*/
float cogravbase = 0.; /* Coordonnees du centre de gravite des regions.*/
int i;
int Mode_Niv_Init; /* estime para cst, lin... avec niveaux par defaut */
int para_init = 0; /* 108: au depart on n'a pas d'estimee des parametres */
/* //€ changer !! bug */
bool state = false;
// Check the pyramid size, if differ, return false
int nrows_pyr1, ncols_pyr1;
int nrows_pyr2, ncols_pyr2;
if (pyramid1.getNumberOfRows(nrows_pyr1) != PYR_NO_ERROR)
return false;
if (pyramid1.getNumberOfCols(ncols_pyr1) != PYR_NO_ERROR)
return false;
if (pyramid2.getNumberOfRows(nrows_pyr2) != PYR_NO_ERROR)
return false;
if (pyramid2.getNumberOfCols(ncols_pyr2) != PYR_NO_ERROR)
return false;
if ((nrows_pyr1 != nrows_pyr2) || (ncols_pyr1 != ncols_pyr2)) {
char message[FILENAME_MAX];
sprintf(message, "CMotion2DEstimator::estimate() \n\tError: Pyramid size differ");
__panic(message);
return false;
}
if (DEBUG_LEVEL2) {
cout << "Pyr 1: " << nrows_pyr1 << " x " << ncols_pyr1 << endl;
cout << "Pyr 2: " << nrows_pyr2 << " x " << ncols_pyr2 << endl;
}
// Check if the memory initialization was done for the weights pyramid
if (init_mem_weightsIsDone == true) {
// Destruction of the weights pyramid if size differ
if ((nrows != nrows_pyr1) || (ncols != ncols_pyr1)) {
if (DEBUG_LEVEL3)
cout << "free pyr_weights[] nlevels: " << pyr_level_max << endl;
free_p_fl(&pyr_weights[0], pyr_level_max + 1);
init_mem_weightsIsDone = false;
}
}
// Check if the memory initialization was done for the internal pyramids
if (init_mem_estIsDone == true) {
// Destruction of the internal pyramids if size differ
if ((nrows != nrows_pyr1) || (ncols != ncols_pyr1)) {
if (DEBUG_LEVEL3)
cout << "free mem_est nlevels: " << pyr_level_max << endl;
efface_memoire_pyr_est(pyr_level_max + 1, &init_mem_estIsDone,
pyr_fl1, pyr_fl2, pyr_fl3, pyr_support);
init_mem_estIsDone = false;
}
}
// Initialisation:
rapport_poids = 0.0;
sigma2res = 0.0;
// Update the image size
nrows = nrows_pyr1;
ncols = ncols_pyr1;
// Warning: To compute the covariance matrix we have to compute the residual
// variance. Thus, if compute_covariance is true, we set compute_sigma2res to
// true.
if (compute_covariance)
compute_sigma2res = true;
// Initialize the covariance matrix to infinity
init_covariance(¶met);
paramet.compute_covariance = compute_covariance;
paramet.compute_sigma2res = compute_sigma2res;
paramet.sigma2res = 0.0;
/* on prend les valeurs par d‰faut */
if( level_ct==-1 && level_lin==-1 && level_quad==-1 )
Mode_Niv_Init=0;
else {
Mode_Niv_Init=1;
if (level_quad >= level_lin) {
char message[FILENAME_MAX];
sprintf(message, "CMotion2DEstimator::estimate() \n\tBad level number %d for starting the estimation of quadratic parameters while affine parameters are estimate at level %d...", level_quad, level_lin);
__panic(message);
return false;
}
if (level_lin >= level_ct ) {
char message[FILENAME_MAX];
sprintf(message, "CMotion2DEstimator::estimate() \n\tBad level number %d for starting the estimation of linear parameters while constant parameters are estimate at level %d...", level_lin, level_ct);
__panic(message);
return false;
}
}
if (first_est_level < final_est_level) {
char message[FILENAME_MAX];
sprintf(message, "CMotion2DEstimator::estimate() \n\tBad initial and final estimation levels...");
__panic(message);
return false;
}
// Modifcation of the first level
int pyr1_level_max = pyramid1.getNumberOfLevels() - 1;
int pyr2_level_max = pyramid2.getNumberOfLevels() - 1;
pyr_level_max = Min(pyr1_level_max, pyr2_level_max);
if (pyr_level_max < 0) {
// The pyramid was not build
return false;
}
// Modification of the first estimation level, if this level is not
// built in the pyramids
if (pyr_level_max > (int) first_est_level)
pyr_level_max = first_est_level;
// Allocate memory for the weights pyramid
if (init_mem_weightsIsDone == false) {
if (DEBUG_LEVEL3) {
cout << "alloc pyr_weights[] nlevels: " << pyr_level_max << endl;
cout << "nrows: " << nrows << " ncols: " << ncols << endl;
}
if (Mem_pyramide_float(&pyr_weights[0], nrows, ncols, pyr_level_max)
== false)
return false;
init_mem_weightsIsDone = true;
}
// Initialize the memory used by the estimator for "pyr_fl1", "pyr_fl2" which
// contains the displaced spatial gradients, for "pyr_fl3" which contains the
// DFD ie I(pi+depl,t+1)-I(pi,t), and for "pyr_support" which contains the
// estimator support.
if (init_mem_est(nrows, ncols, pyr_level_max, &init_mem_estIsDone,
pyr_fl1, pyr_fl2, pyr_fl3, pyr_support) == false) {
char message[FILENAME_MAX];
sprintf(message, "CMotion2DEstimator::estimate() \n\tCan not allocate memory...");
__panic(message);
return false;
}
// Initialize the estimation support
memcpy(pyr_support[0].ad, support, nrows * ncols*sizeof (short));
paramet.n_points = 0;
paramet.nb_para = Nb_Para_Modele(model.getIdModel());
// Test if the motion model exists
if (paramet.nb_para==0)
{
__panic("CMotion2DEstimator::estimate() \n\tMotion model not implemented.\n");
return false;
}
paramet.var_light = model.getVarLight();
paramet.id_model = model.getIdModel();
if (useModelAsInitialization == true) {
para_init = 1;
model.getParameters(paramet.thet); // set the MDL_NMAX_COEF parameters
model.getVarLight(paramet.thet[12]);
}
else {
para_init = 0;
for(i=0;i<MAXCOEFSMODEL;i++) {
paramet.thet[i] = 0.0;
}
}
// Used to compute the window caracteristics
center_of_gravity(&pyr_support[0], label,
&ligravbase, &cogravbase, ¶met.fen, 0);
double row, col;
model.getParameters(paramet.thet);
model.getOrigin(row, col);
if ((row == -1.f) && (col == -1.f)) { // Origin not specified.
// Origin set to the center of gravity
paramet.li_c = ligravbase;
paramet.co_c = cogravbase;
model.setOrigin(paramet.li_c, paramet.co_c);
}
else {
paramet.li_c = row;
paramet.co_c = col;
}
if (DEBUG_LEVEL2) printf("avant appel RMRmod() label: %d\n", label);
// Estimation robuste
state = RMRmod(label, ¶met.fen, pyr_level_max,
pyramid1.pyr_ima, pyramid2.pyr_ima,
pyramid1.pyr_gx, pyramid1.pyr_gy,
pyramid2.pyr_gx, pyramid2.pyr_gy,
max_it_irls, ty_pond, least_mean_square,
type_variance, max_it_stab,
¶met, para_init, &variance, Mode_Niv_Init,
level_ct, level_lin, level_quad,
final_est_level, &pyr_weights[0], tx_pts_min,
&pyr_fl1[0], &pyr_fl2[0],
&pyr_fl3[0], &pyr_support[0], verbose,
&ct_level_intro, &lin_level_intro, &quad_level_intro);
// Test si l'estimation s'est bien deroulee
if (state == false) return false;
// Warning:
// pyr_weights[final_est_level].ad contains the weights
//
// Compute the support size: conform pixels / pixels zone de recouvrement.
//
if (compute_support_size == 1) {
double tx=0.0, ty=0.0;
double a=0.0, b=0.0, c=0.0, d=0.0;
double q1=0.0, q2=0.0, q3=0.0, q4=0.0, q5=0.0, q6=0.0; // Modele de mvt.
int xsize = pyr_weights[final_est_level].nbco;
int ysize = pyr_weights[final_est_level].nbli;
int x, y; // Coordonnees d'un pixel.
int px, py; // Coordonnees du pixel (x,y) deplace.
float xg, yg; // Origine du modele de mvt.
int inter_size = 0; // Taille de la zone commune entre t et t+1.
float nb_pt_inter= 0.0; // Nbre de points utiles dans la zone commune.
float *pond = pyr_weights[final_est_level].ad;
short *support = pyr_support[final_est_level].ad;
int support_size = 0;
// On ramene si necessaire les parametres du modele de mvt du niveau 0
// correspondant a la resolution la plus fine, au niveau final de
// l'estimation (correspondant a l'imagette a un niveau plus grossier de la
// pyramide).
Para tmp_modele;
change_level(¶met, &tmp_modele, final_est_level);
// The parameters
tx = tmp_modele.thet[0];
ty = tmp_modele.thet[1];
a = tmp_modele.thet[2];
b = tmp_modele.thet[3];
c = tmp_modele.thet[4];
d = tmp_modele.thet[5];
q1 = tmp_modele.thet[6];
q2 = tmp_modele.thet[7];
q3 = tmp_modele.thet[8];
q4 = tmp_modele.thet[9];
q5 = tmp_modele.thet[10];
q6 = tmp_modele.thet[11];
// Balayage de l'imagette au niveau final de l'estimation
xg = tmp_modele.co_c;
yg = tmp_modele.li_c;
// Pour optimiser legerement cette partie, 3 cas sont envisages en fonction
// du degre du modele
if ( model_degree(model.getIdModel()) == 2 ) {
// Cas des modeles de mouvement quadratique
for (y = 0; y < ysize; y++) {
float y_yg = y - yg;
float y_yg2 = y_yg * y_yg;
float y_yg_b = y_yg * b;
float y_yg_d = y_yg * d;
float ty_y = ty + y;
int y_xsize = y * xsize;
for (x = 0; x < xsize; x++) {
if (support[x + y_xsize] == label) {
float x_xg = x - xg;
float x_xg2 = x_xg * x_xg;
float x_xg_y_yg = x_xg * y_yg;
support_size ++;
// Calcul du pixel deplace. Le calcul a effectuer est le suivant:
// px = (int) (tx + (x-xg)*a + (y-yg)*b
// + (x-xg)*(x-xg)*q1 + (x-xg)*(y-yg)*q2 + (y-yg)*(y-yg)*q3 + x);
// py = (int) (ty + (x-xg)*c + (y-yg)*d
// + (x-xg)*(x-xg)*q4 + (x-xg)*(y-yg)*q5 + (y-yg)*(y-yg)*q6 + y);
px = (int) (tx + (x_xg)*a + y_yg_b
+ (x_xg2)*q1 + (x_xg_y_yg)*q2 + (y_yg2)*q3 + x);
py = (int) (ty_y + (x_xg)*c + y_yg_d
+ (x_xg2)*q4 + (x_xg_y_yg)*q5 + (y_yg2)*q6);
// Test si pixel deplace de t a t+1 reste a l'interieur de l'image
if ( (px >= 0) && (px < xsize) && (py >= 0) && (py < ysize) ) {
// Le pixel deplace restant dans l'imagette, incrementation de
// la taille de la zone commune entre t et t+1.
inter_size ++;
// Test si le pixel (x,y) participe au mvt dominant. En fait,
// on teste si la ponderation appliquee au pixel (x, y)
// est superieure a un seuil.
if (pond[x + y_xsize] > seuil_poids) {
nb_pt_inter += 1.0;
}
}
}
}
}
}
else if ( model_degree(model.getIdModel()) == 1){
// Cas des modeles de mouvement affine
for (y = 0; y < ysize; y++) {
float y_yg = y - yg;
float y_yg_b = y_yg * b;
float y_yg_d = y_yg * d;
float ty_y = ty + y;
int y_xsize = y * xsize;
for (x = 0; x < xsize; x++) {
if (support[x + y_xsize] == label) {
float x_xg = x - xg;
support_size ++;
// Calcul du pixel deplace. Le calcul a effectuer est le suivant:
// px = (int) (tx + (x-xg)*a + (y-yg)*b + x);
// py = (int) (ty + (x-xg)*c + (y-yg)*d + y);
px = (int) (tx + (x_xg)*a + y_yg_b + x);
py = (int) (ty_y + (x_xg)*c + y_yg_d);
// Test si pixel deplace de t a t+1 reste a l'interieur de l'image
if ( (px >= 0) && (px < xsize) && (py >= 0) && (py < ysize) ) {
// Le pixel deplace restant dans l'imagette, incrementation de
// la taille de la zone commune entre t et t+1.
inter_size ++;
// Test si le pixel (x,y) participe au mvt dominant. En fait,
// on teste si la ponderation appliquee au pixel (x, y)
// est superieure a un seuil.
if (pond[x + y_xsize] > seuil_poids) {
nb_pt_inter += 1.0;
}
}
}
}
}
}
else if ( model_degree(model.getIdModel()) == 0) {
// Cas des modeles de mouvement constant
for (y = 0; y < ysize; y++) {
float ty_y = ty + y;
int y_xsize = y * xsize;
for (x = 0; x < xsize; x++) {
if (support[x + y_xsize] == label) {
support_size ++;
// Calcul du pixel deplace. Le calcul a effectuer est le suivant:
// px = (int) (tx + x);
// py = (int) (ty + y);
px = (int) (tx + x);
py = (int) (ty_y );
// Test si pixel deplace de t a t+1 reste a l'interieur de l'image
if ( (px >= 0) && (px < xsize) && (py >= 0) && (py < ysize) ) {
// Le pixel deplace restant dans l'imagette, incrementation de
// la taille de la zone commune entre t et t+1.
inter_size ++;
// Test si le pixel (x,y) participe au mvt dominant. En fait,
// on teste si la ponderation appliquee au pixel (x, y)
// est superieure a un seuil.
if (pond[x + y_xsize] > seuil_poids) {
nb_pt_inter += 1.0;
}
}
}
}
}
} // fin else degre modele
// Calcul du taux de points conformes dans la zone de recouvrement.
// Si le support est inferieur a 1/8 de la taille de l'image, on met
// le rapport a 0.
if (inter_size < ( (int) (support_size) / 8) )
rapport_poids = 0.0;
else
rapport_poids = nb_pt_inter / inter_size;
} // Fin du calcul du taux de points dans la zone de recouvrement.
// Mise a jour de la variance du residuel.
sigma2res = paramet.sigma2res;
if (compute_covariance) {
// Updates the covariance matrix
memcpy(covariance, paramet.covariance,
MAXCOEFSMODEL * MAXCOEFSMODEL * sizeof(double));
}
double coefs[MAXCOEFSMODEL-1];
for (i=0; i < MAXCOEFSMODEL-1; i++)
coefs[i] = paramet.thet[i];
model.setOrigin(paramet.li_c, paramet.co_c);
model.setParameters(coefs);
model.setVarLight(model.getVarLight(), paramet.thet[12]);
if (DEBUG_LEVEL1)
cout << "Fin CMotion2DEstimator::estimate()" << endl;
return true;
}
/*
** Name: __default_unexpected_handler
**
** Arguments: The usual ISR arguments
**
** Returns: Nothing; it never returns (though we must declare it
** in the usual way to avoid compilation errors).
**
** Description: This routine catches interrupts that we do not expect
** to ever occur. It handles them by calling panic.
*/
static void __default_unexpected_handler( int vector, int code ){
c_printf( "\nVector=0x%02x, code=%d\n", vector, code );
__panic( "Unexpected interrupt" );
}
void _ps2_nonack( Uint resp ){
c_printf( "Did not recieve ACK (0xFF) from MOUSE, instead got: '0x%x'.\n",
resp );
__panic( "YOU SHALL NOT CYCLE!!!\n" );
}
void _vmem_ref_inc_count( Uint32 address, Uint8 is4Mb )
{
//loop over all entries
Uint32 addr;
Uint32 temp;
Uint32 mb;
Uint32 value;
int i;
for( i =0; i < REF_SIZE; i++ )
{
//skip if empty
if( ref[i] == 0 )
{
continue;
}
#ifdef _VMEM_REF_DEBUG
c_printf("Found non-empty index %d %x \n", i, ref[i] );
#endif
temp = ref[i];
addr = temp & REF_ADDRESS;
mb = temp & REF_4MB;
//if the address and the correct size is found then
if( addr == address && mb == is4Mb )
{
//get the reference count
value = temp & REF_COUNT;
if( value == REF_COUNT )
{
__panic("Vmem Ref page shared to many times");
}
//increase the reference count by 1
value = value + REF_COUNT_ADD;
//reassemble and put it back in the index
ref[i] = addr | value | REF_VALID | mb;
return;
}
}
//if the address is not found make a new entry for it
for( i =0; i < REF_SIZE; i++ )
{
//skip if there is already an entry
if( ref[i] != 0 )
{
continue;
}
//calulate values and store
ref[i] = ( address & REF_ADDRESS) | (is4Mb & REF_4MB) | REF_COUNT_ADD | REF_VALID;
#ifdef _VMEM_REF_DEBUG
c_printf("Make new entry for addr %x %x\n", address, ref[i] );
#endif
return;
}
__panic( "Vmem Ref out of places for shared pages");
}
status_t i8255x_driver_setup_irq(void){
status_t status;
int i = 0;
for(; i < MAX_IRQ_FIND_TRIES; i++){
c_printf("INFO: i8255x_driver_setup_irq - Try %d\n", i);
//Initialize IRQ related variables
_i8255x_device.irq_vector = -1;
_i8255x_device.wrong_irq = false;
asm("cli"); //Turn off maskable interrupts
_i8255x_device.csr_bar->status |= INTEL_ETH_SCB_STATUS_ACK_MASK;
//Trigger software interrupt
_i8255x_device.csr_bar->command |= INTEL_ETH_SCB_CMD_TRIGGER_SI;
status = _interrupt_wait(1000, &i8255x_driver_isr);
if(status != E_SUCCESS){
c_printf("ERROR: i8255x_driver_setup_irq - Wait was inconclusive\n");
//i=0;
__delay_ms(1000);
continue;
}
//c_printf("Initial Status: 0x%x\n", _i8255x_device.csr_bar->status);
//Setup our ISR
status = _interrupt_add_isr(&i8255x_driver_isr, _i8255x_device.irq_vector);
if(status != E_SUCCESS){
c_printf("ERROR: i8255x_driver_setup_irq - Failed to add ISR with status=0x%x\n", status);
__panic("CRITICAL");
}
//Make sure we didn't get the wrong IRQ
//We don't expect an interrupt since we didn't trigger one
__delay_ms(100);
status = _interrupt_wait_for_irq(1000, _i8255x_device.irq_vector);
if(_i8255x_device.wrong_irq != false || status != E_TIMEOUT){
//Remove installed ISR
status = _interrupt_del_isr(&i8255x_driver_isr, _i8255x_device.irq_vector);
if(status != E_SUCCESS){
__panic("ERROR: i8255x_driver_setup_irq - Failed to remove ISR\n");
}
continue;
}
//Test out our ISR
_i8255x_device.csr_bar->command |= INTEL_ETH_SCB_CMD_TRIGGER_SI;
__delay_ms(10);
//c_printf("Final Status: 0x%x\n", _i8255x_device.csr_bar->status);
if(_i8255x_device.csr_bar->status == 0x0){
return E_SUCCESS;
}
}
if(i == 3){
//Try using the default IRQ
_i8255x_device.irq_vector = I8255X_DEFAULT_IRQ;
status = _interrupt_add_isr(&i8255x_driver_isr, I8255X_DEFAULT_IRQ);
if(status != E_SUCCESS){
c_printf("ERROR: i8255x_driver_setup_irq - Failed to add ISR with status=0x%x\n", status);
__panic("CRITICAL");
}
//Test out our ISR
_i8255x_device.csr_bar->command |= INTEL_ETH_SCB_CMD_TRIGGER_SI;
__delay_ms(10);
//c_printf("Final Status: 0x%x\n", _i8255x_device.csr_bar->status);
if(_i8255x_device.csr_bar->status == 0x0){
c_printf("INFO: i8255x_driver_setup_irq - Using default IRQ\n");
return E_SUCCESS;
}
_interrupt_del_isr(&i8255x_driver_isr, I8255X_DEFAULT_IRQ);
return E_TOO_MANY_TRIES;
}
return status;
}