Example #1
0
    /*! \brief Helper function to write out a collection of pixels as
        a bmp file.
	
     */
    inline void writeBMPFile(const std::string& filename,
			     const std::vector<uint8_t>& image,
			     size_t width, size_t height,
			     size_t components = 4)
    {
      std::ofstream bmpFile(filename.c_str(), std::fstream::binary);
      bmpFile << detail::bitmap_information_header(width, height);
      
      if (components < 3)
	M_throw() << "Cannot write out bitmaps with less than three components";

      size_t rowpadding = ((width * 3 + 3) & 0xFFFC) - width * 3;
      for (size_t y(0); y < height; ++y)
	{
	  for (size_t x(0); x < width; ++x)
	    {
	      detail::write(bmpFile, image[components * (y * width + x) + 0]);
	      detail::write(bmpFile, image[components * (y * width + x) + 1]);
	      detail::write(bmpFile, image[components * (y * width + x) + 2]);
	    }
	  
	  for (size_t xpad(0); xpad < rowpadding; ++xpad)
	    detail::write(bmpFile, int8_t(0));
	}
    }
int ConvertMutToBmp(const string &fileName)
{
    DataReader mutFile(fileName);

    if (mutFile.IsOpen())
    {   
        //Check it's the right file type.
        char actualHeader[6];   
        memset(actualHeader,0,sizeof(actualHeader));

        for (int i = 0; i < 6; ++i)
        {
            actualHeader[i] = mutFile.ReadByte();
        }

        if (strcmp(actualHeader, siegeHeader) != 0) //The same header files - we're good to process them.
        {
            cout << "Error: not a valid MUT file\n";
            mutFile.Close();
            return -1;
        }

        char imageData[8192];
        memset(imageData,0,sizeof(imageData));
        for (int i = 8191; i >= 0; --i)  //Siege stores its images upside-down, so reverse it here.
        {
            imageData[i] = mutFile.ReadByte();
        }
        mutFile.Close();

        DataReader palFile("siege.pal");

        char* palette = NULL;
        if (palFile.IsOpen()) //Read our palette information
        {
            if (static_cast<int>(palFile.GetFileSize()) == 768)
            {
                palette = new char[palFile.GetFileSize()];
                for (int i = 0; i < palFile.GetFileSize(); ++i)
                {
                    palette[i] = palFile.ReadByte();
                }
                palFile.Close();
            }
            else
            {
                cout << "Error: Pallete incorrect size\n";
                return -1;
            }
        }
        else
        {
            cout << "Error: Failed reading Siege.pal\n";
            return -1;
        }

        //Start building a BMP file
        size_t lastBackslash = fileName.find_last_of("/\\");
        string newFileName = fileName.substr(lastBackslash+1, fileName.length() - lastBackslash - 4);
        newFileName+="bmp";

        if (DataFile::FileExists(newFileName))
        {
            cout << "Error: " << newFileName << " already exists - remove it before proceeding\n";
            return -1;
        }

        DataWriter bmpFile(newFileName);

        if (bmpFile.IsOpen())
        {
            bmpFile.Write("BM", 2); //BMP file format
            bmpFile.Write(9270,4);//File size
            bmpFile.Write(0,2); //Unused
            bmpFile.Write(0,2); //Unused
            bmpFile.Write(1078,4);//Offset
            //IMAGE HEADER
            bmpFile.Write(40,4);//header size
            bmpFile.Write(16,4);//Width
            bmpFile.Write(512,4);//Height
            bmpFile.Write(1,2);//Colour planes
            bmpFile.Write(8,2);//Colour depth - 256 colour
            bmpFile.Write(0,4);//Compression
            bmpFile.Write(8192,4); //Image size
            bmpFile.Write(0,4); //Horizontal resolution
            bmpFile.Write(0,4); //Vertical resolution
            bmpFile.Write(0,4); //colour pallette count - 0 means autodetect
            bmpFile.Write(0,4); //important colours
            //PALETTE
            for(int i = 0; i < 256; ++i)
            {
                bmpFile.Write(palette[(i*3)+2]); //Red   //Colours are stored as BGR in Siege's palette - backwards again
                bmpFile.Write(palette[(i*3)+1]); //Green
                bmpFile.Write(palette[(i*3)]); //Blue

                bmpFile.Write(0); //Unused - bitmaps pad the alpha channel to keep things in a nice DWORD size
            }
            //IMAGE DATA
            for (int j = 0; j < 8192; ++j)
            {
                bmpFile.Write(imageData[j]);
            }
        }
        else
        {
            cout << "Error: could not open " << newFileName << " for writing\n";      
            if (palette)
            {
                delete[] palette;
            }
            return -1;
        }

        if (palette)
        {
            delete[] palette;
        }
        cout << "Successfully created " << newFileName << "\n";
        return 0;
    }
    else
    {
        cout << "Error - could not open " << fileName << " for reading\n";
        return -1;
    }
}
int ConvertBmpToMut(const string &fileName)
{
    DataReader bmpFile(fileName);
    if (bmpFile.IsOpen())
    {
        short identifier = bmpFile.ReadShort();
        int fileSizePredicted = bmpFile.ReadInt();

        if (identifier != 0x4D42) //BM, as a little-endian DWORD
        {
            cout << "Error: Header incorrect - are you sure it's a valid bitmap?\n";
            bmpFile.Close();
            return -1;
        }
        if (static_cast<int>(bmpFile.GetFileSize()) != 9270 || fileSizePredicted != 9270) //256-colour bitmap files with Siege units in them will always be 9270 bytes.
        {
            cout << "Error: File size incorrect\n";
            bmpFile.Close();
            return -1;
        }

        bmpFile.SetPos(14); //Start of header file

        int headerSize = bmpFile.ReadInt();
        if (headerSize != 40) //If the header size isn't 40, something bad's happening.
        {
            cout << "Error: invalid header size\n";
            bmpFile.Close();
            return -1;
        }

        int width = bmpFile.ReadInt();
        int height = bmpFile.ReadInt();

        if (width != 16 || height != 512) //512/16 gives us 32 frames of 16x16 siege unit
        {
            cout << "Error: Image dimensions incorrect\n";
            bmpFile.Close();
            return -1;
        }

        bmpFile.SetPos(28);
        short colourDepth = bmpFile.ReadShort();

        if (colourDepth != 8)
        {
            cout << "Error: Must be a 256-colour image\n";
            bmpFile.Close();
            return -1;
        }

        bmpFile.SetPos(1078); //Jump to the start of image data.

        char imageData[8192];
        memset(imageData,0,sizeof(imageData));

        for (int i = 8191; i >= 0; --i) //Siege units are stored upside down, so we read our bitmap into the array upside down
        {
            imageData[i] = bmpFile.ReadByte();
        }
        bmpFile.Close();

        //We can start writing!
        size_t lastBackslash = fileName.find_last_of("/\\");
        string newFileName = fileName.substr(lastBackslash+1, fileName.length() - lastBackslash - 4);
        newFileName+="mut";

        if (DataFile::FileExists(newFileName))
        {
            cout << "Error: " << newFileName << " already exists - remove it before proceeding\n";
            return -1;
        }

        DataWriter mutFile(newFileName);
        if (mutFile.IsOpen())
        {
            for (int i = 0; i < sizeof(siegeHeader); ++i)
            {
                mutFile.Write(siegeHeader[i]); //We could have written this as a block, but it strangely nulls certain characters.
            }

            for (int i = 0; i < sizeof(imageData); ++i)
            {
                mutFile.Write(imageData[i]); //Finally write our image data
            }
        }
        else
        {
            cout << "Error: Could not open " << newFileName << " for writing\n";
            bmpFile.Close();
            return -1;
        }
        cout << "Successfully created " << newFileName << "\n";
    }
    else
    {
        cout << "Error: Could not open " << fileName << " for reading\n";
        return -1;
    }

    return 0;
}
Example #4
0
int main(int argc, char **argv) {
   OptionParser opts;

   string mapFile, evidFile;

   int factor;

   opts.addOption(new StringOption("map", 
            "--map <filename>                 : map file",
            "../input/grid.bmp", mapFile, false));

   opts.addOption(new StringOption("evidence", 
            "--evidence <filename>            : evidence file",
            "", evidFile, true));

   opts.addOption(new IntOption("factor",
            "--factor <int>                   : scaling factor",
            1, factor, true));

   opts.parse(argc,argv);
   JetColorMap jet;
   RGBTRIPLE black = {0,0,0};

   RGBTRIPLE white = {255,255,255};

   RGBTRIPLE red;
   red.R = 255;
   red.G = 0;
   red.B = 0;

   RGBTRIPLE blue;
   blue.R = 0;
   blue.G = 0;
   blue.B = 255;

   RGBTRIPLE green;
   green.R = 0;
   green.G = 255;
   green.B = 0; 

   RGBTRIPLE initialColor;
   initialColor.R = 111; 
   initialColor.G = 49;
   initialColor.B = 152;
//   initialColor.G = 152;
//   initialColor.B = 49;


   RGBTRIPLE currentColor;
   currentColor.R = 181;
   currentColor.G = 165;
   currentColor.B = 213;
//   currentColor.G = 213;
//   currentColor.B = 165;


   RGBTRIPLE magenta;
   magenta.R = 255;
   magenta.G = 0;
   magenta.B = 255;

   RGBTRIPLE cyan;
   cyan.R = 0;
   cyan.G = 255;
   cyan.B = 255;

   RGBTRIPLE yellow;
   yellow.R = 255;
   yellow.G = 255;
   yellow.B = 0;

   BMPFile bmpFile(mapFile);

   Grid grid(bmpFile, black);

   
   Evidence testSet(evidFile, grid, factor);
 /* 
   if (1) { 
	   evid.split(trainSet, testSet, 0.8);
   }else{
	   evid.deterministicsplit(trainSet, testSet);
   }*/

#if 0 
   cout << "Creating Markov Model"<<endl;
   MarkovModel markmodel(grid, trainSet);

   double totalObj = 0.0;

   for (int i=0; i < testSet.size(); i++) {
      vector<pair<int, int> > path = testSet.at(i);
      cout << "Calling eval"<<endl;
      double obj = markmodel.eval(path);
      cout << "OBJ: "<<i<<" "<<obj<<endl;
	
      totalObj += obj;
   }

   cout << "TOTAL OBJ: "<<totalObj<<endl;

   cout << "AVERAGE OBJ: "<<totalObj/testSet.size()<<endl;
   return 0;
#endif
   vector<PosFeature> features;

   cout << "Constant Feature"<<endl;

   ConstantFeature constFeat(grid);
   features.push_back(constFeat);

   cout << "Obstacle Feature"<<endl;

   ObstacleFeature obsFeat(grid);
   features.push_back(obsFeat);

   for (int i=1; i < 5; i++) {
      cout << "Blur Feature "<<i<<endl;
      ObstacleBlurFeature blurFeat(grid, 5*i);
      features.push_back(blurFeat);
   }

   cout << "Creating feature array"<<endl;
   FeatureArray featArray2(features);

   cout << "Creating lower resolution feature array"<<endl;
   FeatureArray featArray(featArray2, factor);

   pair<int, int> dims = grid.dims();
   pair<int, int> lowDims((int)ceil((float)dims.first/factor),
         (int)ceil((float)dims.second/factor));

   vector<double> weights(features.size(), -0.0);
   weights.at(1) = -6.2;
   //for (int i=2; i < weights.size(); i++)
   //   weights.at(i) = -1.0;
   weights.at(0) = -2.23;//-2.23
   weights.at(2) = -0.35;
   weights.at(3) = -2.73;
   weights.at(4) = -0.92;
   weights.at(5) = -0.26;
   Parameters params(weights);

   OrderedWaveInferenceEngine engine(InferenceEngine::GRID8);

   vector<vector<double> > prior(dims.first,vector<double> (dims.second,0.0));
/*
   double divide = 1.0;
   vector<double> radiusWeight;
   for (int i=0; i < 20; i++) {
      radiusWeight.push_back(1.0/divide);
      divide*=2;
   }
   generatePrior(grid, trainSet, priorOrig, radiusWeight, factor);
 
   reducePrior(priorOrig, prior, factor);
*/

   vector<vector<vector<double> > > partition, backpartition;

   int time0 = time(0);

   BMPFile gridView(dims.first, dims.second);



   RewardMap rewards(featArray, params); 

   vector<double> sums(params.size(),0.00001);
      
   vector<vector<double> > occupancy;

   Predictor predictor(grid, rewards, engine); 
   
   predictor.setPrior(prior);


   cout << testSet.size() <<" Examples"<<endl;

   for (int i=0; i < testSet.size(); i++) {

      int index = 0;


      vector<pair<int, int> > traj = testSet.at(i);
      vector<double> times = testSet.getTimes(i); 
      pair<int, int> initial = traj.front();
	  pair<int,int> & botinGrid = testSet.at_bot(i); 
	  pair<double,double>& botinPoint = testSet.at_rbot(i);
	  pair<double,double>& end = testSet.at_raw(i).back();

      predictor.setStart(initial); 

      double thresh = -20.0;
	  double startTime = times.front();

      char buf[1024];
      sprintf(buf, "../output/pppredict%03d.dat", i);
      ofstream file(buf);

      for (double tick = startTime; index < traj.size(); tick+=0.4) {

         for ( ; index < traj.size() && times.at(index) < tick; index++); 

         if (index == traj.size() ) break;
 
         cout << "Evidence: "<<i<<"  timestep: "<<tick
            <<"   index: "<<index<<endl;
         predictor.predict(traj.at(index), occupancy);

         cout << "SIZE: "<<prior.size()<<endl;
		 vector<vector<double> >  pos 
            = predictor.getPosterior();

         gridView.addBelief(pos, -30.0, 0.0,jet);

         grid.addObstacles(gridView, black);
         gridView.addLabel(botinGrid,green);
         vector<pair<int, int> > subTraj;

         subTraj.insert(subTraj.end(), traj.begin(), traj.begin()+index);

         gridView.addVector(subTraj, red, factor);

         sprintf(buf, "../compare/pp%03d-%03f.bmp", i, tick-startTime); 
         gridView.write(buf);
		 //pair<double,double> values = predictor.check(traj.back());
		 double cost = 0.0;
		 for(int itr = 0;itr<index;itr++)
		   cost +=rewards.at(traj[itr].first,traj[itr].second);

		 cout<<i<<" Normalizer: "<<predictor.getNormalizer(traj.back())<<
			 " path cost: "<<cost<<" Probability:  "<<cost+predictor.getNormalizer(traj.back())<<endl;

         vector<vector<vector<double> > > timeOcc 
            = predictor.getTimeOccupancy();

		 vector<vector<double > > posterior  = predictor.getPosterior();
		 double maxV = -HUGE_VAL;
		 pair<int,int> predestGrid;
		 pair<double,double> predestPoint;

         for (int ii=0; ii< dims.first; ii++) { 
            for (int jj=0; jj < dims.second; jj++) {
			   if(posterior[ii][jj]>maxV){
				   predestGrid.first = ii;
				   predestGrid.second = jj;
			   }
               maxV  = max(maxV, posterior.at(ii).at(jj));
            }
         }
		 predestPoint = grid.grid2Real(predestGrid.first,predestGrid.second);
		 double dist = sqrt((end.first-predestPoint.first)*(end.first-predestPoint.first)
			 +(end.second-predestPoint.second)*(end.second-predestPoint.second));

		 double logloss = entropy(posterior);

		 cout<<"final belief: "<<posterior.at(traj.back().first).at(traj.back().second)
			 <<" max: "<<maxV
			 <<" logloss: "<<logloss<<endl; 
		 cout<<botinGrid.first<<" "<<botinGrid.second
			 <<" "<<predestGrid.first<<" "<<predestGrid.second<<endl;
		 file<<tick-startTime
			 <<" "<<logloss
			 <<" "<<posterior.at(botinGrid.first).at(botinGrid.second)
			 <<" "<<posterior.at(traj.back().first).at(traj.back().second)
			 <<" "<<maxV<<" "<<dist<<endl;

      } 
      file.close();
   }

}
Example #5
0
int main(int argc, char **argv) {
   OptionParser opts;

   string mapFile,trainFile,testFile;

   int factor = 1;
   double step;

   opts.addOption(new StringOption("map", 
            "--map <filename>                 : map file",
            "../input/grid.bmp", mapFile, false));
   opts.addOption(new StringOption("evidence", 
            "--test evidence <filename>            : evidence file",
            "", testFile, true));

   opts.addOption(new DoubleOption("step",
            "--step <double>                   : inference interval",
            1.0, step, true));

   opts.parse(argc,argv);

   JetColorMap jet;
   RGBTRIPLE black = {0,0,0};
   RGBTRIPLE white = {255,255,255};
   RGBTRIPLE red;
   red.R = 255;
   red.G = 0;
   red.B = 0;
   RGBTRIPLE blue;
   blue.R = 0;
   blue.G = 0;
   blue.B = 255;
   RGBTRIPLE green;
   green.R = 0;
   green.G = 255;
   green.B = 0; 
   RGBTRIPLE initialColor;
   initialColor.R = 111; 
   initialColor.G = 49;
   initialColor.B = 152;
   RGBTRIPLE currentColor;
   currentColor.R = 181;
   currentColor.G = 165;
   currentColor.B = 213;
   RGBTRIPLE magenta;
   magenta.R = 255;
   magenta.G = 0;
   magenta.B = 255;
   RGBTRIPLE cyan;
   cyan.R = 0;
   cyan.G = 255;
   cyan.B = 255;
   RGBTRIPLE yellow;
   yellow.R = 255;
   yellow.G = 255;
   yellow.B = 0;

   BMPFile bmpFile(mapFile);
   Grid grid(bmpFile, black);

   
   Evidence testSet(testFile, grid, factor);
 //  Evidence trainSet(trainFile, grid, factor);

   pair<int, int> dims = grid.dims();
   
   cout << " Speed Feature"<<endl;
   vector<double> speedTable(VEL_DIM,0.0);
   speedTable.at(1) = 0.75;
   DisVecSeqFeature speedfeat(speedTable);

   vector<int> dimensions;
   dimensions.push_back(dims.first);
   dimensions.push_back(dims.second);
   dimensions.push_back(VEL_DIM);
   
   /* ****************************************
	*      INITIALIZE MARKOV DECESION PROCESS 
	*      BASED MODEL PARAMETERS
	* ****************************************/
   vector<double> p_weights(NUMPOSFEAT,-0.0);
   p_weights.at(0) = -2.23; //-2.23 for PPP forecast
   p_weights.at(1) = -6.2;
   p_weights.at(2) = -0.35;
   p_weights.at(3) = -2.73;
   p_weights.at(4) = -0.92;
   p_weights.at(5) = -0.26;
   vector<double> r_PosWeights(NUMPOSFEAT+NUMROBFEAT, -0.0);
   r_PosWeights.at(0) = -3.83;
   r_PosWeights.at(1) = -8.36;
   r_PosWeights.at(2) = -2.65;
   r_PosWeights.at(3) = -5.43;
   r_PosWeights.at(4) = -3.15;
   r_PosWeights.at(5) = -3.30;
   //r_PosWeights.at(6) =  0.60;
   //r_PosWeights.at(7) =  0.45;
   vector<double> nr_PosWeights(NUMPOSFEAT+NUMROBFEAT, -0.0);
   nr_PosWeights.at(0) = -4.51;
   nr_PosWeights.at(1) = -6.2;
   nr_PosWeights.at(2) = -0.35;
   nr_PosWeights.at(3) = -2.73;
   nr_PosWeights.at(4) = -0.93;
   nr_PosWeights.at(5) = -0.28;
   //nr_PosWeights.at(6) = -0.50;
   //nr_PosWeights.at(7) = -0.286;
   vector<double> r_SeqWeights(VEL_DIM, -0.0);
   r_SeqWeights.at(0) = 0.59;
   r_SeqWeights.at(1) = -0.83;
   vector<double> nr_SeqWeights(VEL_DIM, -0.0);
   nr_SeqWeights.at(0) = -1.21;
   nr_SeqWeights.at(1) = 0.49;

   Parameters p(p_weights);
   Parameters r_Pos(r_PosWeights);
   Parameters nr_Pos(nr_PosWeights);
   Parameters r_Seq(r_SeqWeights);
   Parameters nr_Seq(nr_SeqWeights);

   /* ****************************************
	*      INITIALIZE LINEAR QUADRATIC CONTROL 
	*      BASED MODEL PARAMETERS
	* ****************************************/
   M_6 A;
   A.setZero();
   A(0,0) = 1;
   A(1,1) = 1;
   A(4,2) = -1;
   A(5,3) = -1;
   M_6_2 B;
   B<<1,0,
	  0,1,
	  1,0,
	  0,1,
	  1,0,
	  0,1;
   M_6 costM;
   ifstream infile("../params/nonrob2000.dat");
   for(int row=0;row<costM.rows();row++){
	   for(int col=0;col<costM.cols();col++){
		   double temp;
		   infile>>temp;
		   costM(row,col) = temp;
	   }
   }
   infile.close();
   M_6 sigma;
   sigma<<0.001,0,0,0,0,0,
	      0,0.001,0,0,0,0,
		  0,0,0.005,0,0,0,
		  0,0,0,0.005,0,0,
		  0,0,0,0,0.005,0,
		  0,0,0,0,0,0.005;


   /* ****************************************
	*      DECLARATION OF INFERENCE ENGINES    
	* ****************************************/
   OrderedWaveInferenceEngine pp(InferenceEngine::GRID8);
   DisSeqOrderInferEngine mdpr(InferenceEngine::GRID8);
   DisSeqOrderInferEngine mdpnr(InferenceEngine::GRID8);
   ContinuousState cState;
   LQControlInference lq(A,B,sigma,costM,cState);
   lq.valueInference();


   IntentRecognizer IR(grid,p,r_Pos,r_Seq,nr_Pos,nr_Seq,
			   speedfeat,pp,mdpr,mdpnr,lq);

   cout << testSet.size() <<" Examples"<<endl;

   for (int i=0; i < testSet.size(); i++) {

      vector<pair<int, int> > & traj = testSet.at(i);
	  vector<double> & vels = testSet.at_v(i);
      vector<double> times = testSet.getTimes(i); 
	  pair<int,int> & botinGrid = testSet.at_bot(i);
	  vector<pair<double,double> > & obs = 
		  testSet.at_raw(i);
      vector<double> & rawTimes = testSet.at_rawTime(i);

      IR.combineForecast(traj,vels,obs,times,rawTimes,
				  botinGrid,i,step);
      
   }
}
Example #6
0
int main(int argc, char **argv) {
   OptionParser opts;

   string mapFile, evidFile;//interactFile,ignoreFile;

   int factor;

   opts.addOption(new StringOption("map", 
            "--map <filename>                 : map file",
            "../input/grid.bmp", mapFile, false));

   opts.addOption(new StringOption("evidence", 
            "--evidence <filename>            : evidence file",
            "", evidFile, true));
   opts.addOption(new IntOption("factor", 
            "--factor <int>                   : scaling factor",
            1, factor, true));


   opts.parse(argc,argv);

   cout << "Loading Map File"<<endl;
   BMPFile bmpFile(mapFile); 
   Grid grid(bmpFile, black);
//   cout << "xdim: "<<grid.dims().first<<" yDim: "<<grid.dims().second<<endl;
   cout << "Loading Evidence"<<endl;
   //Evidence trainSet(evidFile, grid, factor);
   /* used when need to train two seperate models
   Evidence evid_int(interactFile, grid, factor);
   Evidence evid_ig(ignoreFile, grid, factor);
   Evidence train_int(grid),test_int(grid),train_ig(grid), test_ig(grid);
   evid_int.split(train_int, test_int, 0.05);
   evid_ig.split(train_ig, test_ig, 0.05);
   */
   Evidence evid(evidFile,grid,factor);
   Evidence trainSet(grid),testSet(grid);
   evid.split(trainSet,testSet,0.05);
   cout<<"Optimize over "<<trainSet.size()<<" examples"<<endl;
#if 0 
   for (int i=0; i < evid.size(); i++) {
      cout << "Evid "<<i<<endl;
      vector<pair<int, int> > traj = evid.at(i);
      vector<double> timestamps = evid.getTimes(i);

      cout << timestamps.size()<<"  "<<traj.size()<<endl;

      for (int j=0; j < traj.size(); j++) {
         cout << timestamps.at(j)<<"  "<<traj.at(j).first
            << "  "<<traj.at(j).second<<endl;
      } 
   }
#endif
//   testSet.write("testTraj.data");

   cout << "Generating Feature Set"<<endl;

   vector<PosFeature> features;

   cout << "   Constant Feature"<<endl;

   ConstantFeature constFeat(grid);
   features.push_back(constFeat);

   cout << "   Obstacle Feature"<<endl;

   ObstacleFeature obsFeat(grid);
   features.push_back(obsFeat);
	

   for (int i=1; i < 5; i++) {
      cout << "   Blur Feature "<<i<<endl;
      ObstacleBlurFeature blurFeat(grid, 5*i);
      features.push_back(blurFeat);
   }

   /*
   cout << "    Robot Feature"<<endl;
   RobotGlobalFeature robglobal(grid,snackbot,factor);
   features.push_back(robglobal);
   //  robot local blurres features
   for (int i=1; i < 5; i++) {
      cout << "  RobotBlur Feature "<<i<<endl;
      RobotLocalBlurFeature robblur(grid,snackbot,5*i,factor);
      features.push_back(robblur);
   }
	
   */
 
   /* 
   cout << "   Creating feature array"<<endl;
   FeatureArray featArray2(features);

   cout << "   Creating lower resolution feature array"<<endl;
   FeatureArray featArray(featArray2, factor);
   */

   cout << " Speed Feature"<<endl;
   vector<double> speedTable(2,0.0);
   speedTable.at(1) = 0.75;
   //speedTable.at(2) = 1.1;
   DisVecSeqFeature speedfeat(speedTable);


   /* Robset training weights: 
	* -3.83 -8.35991 -2.6512 -5.43475 -3.15203 -3.29758
	*  0.596987 0.439284
	* 0.589445 -0.82448
	* Non-robot-ending trainng weights:
	* -4.57257  -6.2 -0.3537 -2.7385 -0.9357 -0.2797
	* -0.495205 -0.2863
	* -1.2225 0.43993
	*/
   vector<double> weights(6+2+2, -0.0);
   weights.at(0) = -25;	
   weights.at(1) = -8.36;
   weights.at(2) = -2.65;
   weights.at(3) = -5.43;
   weights.at(4) = -3.17;
   weights.at(5) = -3.34;
   
   weights.at(6) = 0.5; // robot feature
   weights.at(7) = 0.3; // robot feature
  
   weights.at(8) = -0.29;  // velocity feature
   weights.at(9) = -1.11; // velocity feature

   //weights.push_back(1.5);//the last parameter is for velocity feature
   Parameters params(weights);

   DisSeqOrderInferEngine engine(8,InferenceEngine::GRID8);

   trajOptimizerplus optimizer(grid,trainSet,features,speedfeat,engine);

   optimizer.optimize(params,0.005,1000,1.0,OPT_EXP);

   return 0;

}