/*! \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; }
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(); } }
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); } }
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; }