pol_locator_gen.h

Go to the documentation of this file.

/*
 *
 *  A new locator which does not use Hough "images"
 *  should in the future be able to deal with position and angle and scale
 *  gen is for generic
 *
 *  stumeikle Wed Dec 11 16:15:21 2002
 *
 */

#ifndef POL_LOCATOR_GEN_H
#define POL_LOCATOR_GEN_H

#include <boost/numeric/ublas/vector.hpp>
#include <cmath>
#include <cstdarg>
#include "pol_match.h"

//not really true error values i think, but almost
#define DISTANCE_ERROR  5
#define ANGLE_ERROR 10.0 * 0.0174533
namespace mimas {
class poss_centroid
{
    public:
   typedef boost::numeric::ublas::vector< double > Vector;
  Vector posn;
  double    posn_weight;
  angle ang;
  double    angle_weight;
            //and scale to come of course !!
  double    entry_weight;

  static  double* posn_gauss;
  static  double* angle_gauss;
  static  int     posn_gauss_size;
  static  int     angle_gauss_size;

  poss_centroid()
  {
      posn_weight = angle_weight = 1.0;
      entry_weight=0.0;
  }

  double    consistent( poss_centroid *other )
  {
      // is the provided mment consistent with this value
      // (ie does it fall within the error bounds of this point)
      // for now this will mean, is its posn close to ours and
      // is its angle close to ours. both conditions must be satisfied
      // we'll return a match value now...
      double  dist;

      dist = norm_2(other->posn - posn);
      if (dist>DISTANCE_ERROR)
        return 0;

      //else
      //check the angles, and if they match too
      //then calculate a weight based on Gausssssss
      angle adiff;
      adiff = other->ang - ang;
      if (adiff> ANGLE_ERROR | adiff< -ANGLE_ERROR)
        return 0;

      //ok, so calculate a fitness measure.
      //distance is easy --> we can calculate the gaussian weight
      double  weight=0,w1,w2;

      getWeights( other, &w1, &w2 );
      weight = w1*w2;
       std::cout << "weight=" << weight<< std::endl;
      return weight;
  }

  void    getWeights( poss_centroid *other, double *w1, double *w2 )
  {
      //this could be elipsesified too but its internal so not so important
      double  posn_diff;
      int   ipd;

      posn_diff = norm_2(posn - other->posn);
      ipd = (int)posn_diff;
      if (ipd< posn_gauss_size)
    *w1 = posn_gauss[ipd];
    //*w1=1.0;
      else
        *w1 = 0;

      double  angle_diff;
      int   iad;

      angle_diff = (double)(ang - other->ang);
      if (angle_diff<0) angle_diff=-angle_diff;
      iad = (int)angle_diff;
      if (iad< angle_gauss_size)
        *w2 = angle_gauss[iad];
    //*w2=1.0;
      else
        *w2 = 0;

       std::cout << "ipd=" << ipd << " and iad=" << iad << std::endl;
       std::cout << "angle_gauss=" << angle_gauss[iad] << std::endl;
       std::cout << "dist_gauss="  << posn_gauss[ipd]  << std::endl;


  }

  void    update( poss_centroid *other )
  {
      //function should be called after it hsa been found that 'this'
      //and other are consistent. update 'this' with the new 'other' entry
      //this function updates the weight and centroid and angle position
      //associated with this function.
      double  w1,w2;
       std::cout << "Update" << std::endl;
      getWeights( other, &w1, &w2 );
      w1*=other->entry_weight;
      w2*=other->entry_weight;

      //we use distance and angle weights separately here. actually i dont
      //this this algorithm is much good (!!FIX!!)
      posn = (posn * posn_weight) + (other->posn*w1);
      posn_weight+=w1;
      posn= posn/ posn_weight;

      ang= (ang * angle_weight) + (other->ang*w2);
      angle_weight+= w2;
      ang= ang / angle_weight;

      //update the entry weight. for models this value will be used for
      //later thresholding, but NOT during the update process
      entry_weight= posn_weight* angle_weight;
  }
};

typedef std::list< poss_centroid * > poss_centroid_list;

class pol_locator_gen : public pol_locator
{
 public:
   typedef boost::numeric::ublas::vector< double > Vector;
    private:
      double      threshold;
  // list of possible centroids
     poss_centroid_list list_poss_centroids;
  // list of possible matches/found instances of the model
     poss_centroid_list list_poss_matches;

  double      *posn_gauss,*angle_gauss;
  int     posn_gauss_size, angle_gauss_size;

  int calcGaussBoxSize( double sigma, double accuracy )
  {
      //code largely taken from tina src.
      double n;

      sigma = 2.0 * sigma * sigma;

      for(n=1.0; ;n+=1.0)
      {
    if (exp( -n*n/sigma )< accuracy) break;
      }

      return (int) n;
  }

    public:
  //same interface as with the last one. i guess i'll create an
  //abstract interface file later..
  pol_locator_gen()
  {
      posn_gauss=angle_gauss=0;
      posn_gauss_size=angle_gauss_size=0;
      threshold=0;
  }
  pol_locator_gen(double t)
  {
      posn_gauss=angle_gauss=0;
      posn_gauss_size=angle_gauss_size=0;
      threshold=t;
  }
  ~pol_locator_gen()
  {
      if (posn_gauss) delete[] posn_gauss;
      if (angle_gauss)  delete[] angle_gauss;
//      list<poss_centroid*>::iterator  i;
//      for(i=list_poss_centroids.begin();
//          i!=list_poss_centroids.end(); ++i)
//      {
//        delete (*i);
//      }
//      list_poss_centroids.clear();

//      for(i=list_poss_matches.begin();
//          i!=list_poss_matches.end(); ++i)
//      {
//        delete (*i);
//      }
//      list_poss_matches.clear();
  }

  virtual void    init( int numdims, ...)
  {
      //set up the guassians. we'll need to use this for the position and angle
      //updates, so no harm in using this function again
      //i'll use a variable argument list here because the number of degrees of
      //freedom is likely to increase in the future
      va_list vlist;

      if (numdims!=2)
        return;
    //should throw an exception really

      //read the position and angle sigmas and precisions. create a gauss dist for each
      va_start(vlist, numdims);

      double  s,p;
      s=va_arg(vlist, double);
      p=va_arg(vlist, double);

      if (posn_gauss) delete posn_gauss;

      s = 2*s*s;
      int size = calcGaussBoxSize( s,p );
      int i;

      posn_gauss=new double[size];
      posn_gauss_size=size;

      //fill in the curve
      for(i=0;i<size;++i)
      {
    double d = (double)i*(double)i;
    double w = exp( -d/s );

        posn_gauss[i]=w;
      }

      //do the second ...
      s=va_arg(vlist, double);
      p=va_arg(vlist, double);

      if (angle_gauss)  delete angle_gauss;

      s = 2*s*s;
      size = calcGaussBoxSize( s,p );

      angle_gauss=new double[size];
      angle_gauss_size=size;

      //fill in the curve
      for(i=0;i<size;++i)
      {
    double d = (double)i*(double)i;
    double w = exp( -d/s );

        angle_gauss[i]=w;
      }

      //store the values in the poss_centroid so that all poss_centroids have access to them
      poss_centroid::posn_gauss= posn_gauss;
      poss_centroid::angle_gauss=angle_gauss;
      poss_centroid::angle_gauss_size=angle_gauss_size;
      poss_centroid::posn_gauss_size=posn_gauss_size;

      va_end(vlist);
  }

  virtual void    addEntry( double weight, ... )
  {
      //again we need ellipses to cover the n-dim case
      va_list vlist;
      va_start(vlist,weight);

       Vector posn( 2 );
       posn( 0 ) = va_arg( vlist, double );
       posn( 1 ) = va_arg( vlist, double );
      double  angle= va_arg( vlist, double );

      // just push the measurement onto the list. we'll analyse the results when
      // the user calls get matches
      poss_centroid *pc = new poss_centroid;
      pc->posn=posn;
      pc->ang=angle;
      pc->entry_weight=weight;
      list_poss_centroids.push_back(pc);

      va_end(vlist);
  }

  virtual pol_match_ptr_list getMatches()
  {
      // so now we have to look for consistencies in the measurements without
      // using a hough transform

      // maybe the first attempt will be to just try to replicate the results
      // that we get with the hough transform method.
      // Algo:
      // -- go through the list of poss centroids. compare each to the list of
      //    possible matches. if its consistent with one of them then update the
      //    match centroid and weight.
      // -- if not then , errr, in this simplist of cases, we will add it to thh
      //    list of possible matches anyway.
      //    [in future we can compare the centroid to all possible centroids and
      //     if it is consistent with at least one other, we can add it as a
      //     possible model.]
      // -- threshold the match list and return
       poss_centroid_list::iterator i_pc, i_pm;
       std::cout<<"hello1"<< std::endl;

      for(i_pc=list_poss_centroids.begin(); i_pc!=list_poss_centroids.end();++i_pc)
      {
    poss_centroid*  pc=(*i_pc);

    if  (!pc) continue;

    //we have a possible centroid position.
    //is it consistent with any of the matches found so far?
    //we'll take the best match to start with, though it may be too slow
    poss_centroid   *best_match=0;
    double      best_score=0;
    for(i_pm=list_poss_matches.begin(); i_pm!=list_poss_matches.end();++i_pm)
    {
        double    score;
        poss_centroid *match=(*i_pm);
        if (!match)   continue;

        score = pc->consistent( match );
          //std::cout << "score=" << score << std::endl;

          //if (score>best_score)
        if (score>0)
        {
          best_score=score;
      best_match=match;
      break;
        }
    }

    //if found a match
    if (best_match)
    {
        best_match->update( pc );
    }
    else
    {
        // if !found...
        // ... just add to the match list anyway
        list_poss_matches.push_back( pc );
    }
      }
       std::cout<<"hello2"<<std::endl;

      //threshold match list and return ...
      pol_match_ptr_list retval;
      double    max=0;
      for(i_pc=list_poss_matches.begin();
        i_pc!=list_poss_matches.end();  ++i_pc)
      {
    poss_centroid *match= (*i_pc);

      std::cout << "Found a possible entry, weight(" << threshold << ")= " << match->entry_weight<< std::endl;
    if (match->entry_weight> max)
      max = match->entry_weight;
    if (match->entry_weight > 5000) // threshold)
    {
        pol_match_ptr m( new pol_match );

      m->model_found = NULL;
      m->pos = match->posn;
      m->orient=match->ang;
      m->strength=match->entry_weight;
      retval.push_back(m);
    }
      }
       std::cout<<"hello3"<<std::endl;
      // delete the data structures which we're finished with now
//        poss_centroid_list::iterator  i;
//      for(i=list_poss_centroids.begin();
//          i!=list_poss_centroids.end(); ++i)
//      {
//        delete (*i);
//      }
//      list_poss_centroids.clear();

//      for(i=list_poss_matches.begin();
//          i!=list_poss_matches.end(); ++i)
//      {
//        delete (*i);
//      }
//      list_poss_matches.clear();
       std::cout<<"hello4"<<std::endl;
       std::cout << "Max val = " << max << std::endl;

      return retval;
  }
};


//yukky yuk
double* poss_centroid::angle_gauss = NULL;
double* poss_centroid::posn_gauss = NULL;
int     poss_centroid::angle_gauss_size = 0;
int poss_centroid::posn_gauss_size = 0;
}
#endif

---