int main(int argc, char** argv)
{
// try {
Dune::MPIHelper::instance(argc, argv);
typedef Dune::CpGrid Grid;
#if 0
std::string dgfString( "DGF\n\nInterval\n0 0 0\n1 1 1\n2 2 2\n#\n" );
std::stringstream dgfStream( dgfString );
Dune::GridPtr< Grid > gridPtr( dgfStream );
Grid &grid = *gridPtr;
#else
int refinement = 1;
Grid grid;
std::array<int , 3> dims;
std::fill(dims.begin(), dims.end(), 1 << refinement);
std::array<double, 3> cell_sz;
std::fill(cell_sz.begin(), cell_sz.end(), 1.0 / (1 << refinement));
grid.createCartesian(dims, cell_sz);
#endif
typedef Grid::LeafGridView GridView;
GridView gridView(grid.leafView());
typedef Dune::MultipleCodimMultipleGeomTypeMapper<GridView,ElementLayout> ElementMapper;
ElementMapper elementMapper(gridView);
typedef GridView::IndexSet IndexSet;
const IndexSet& indexSet(gridView.indexSet());
typedef GridView::Codim<0>::Iterator ElementIterator;
ElementIterator endEIt = gridView.end<0>();
for (ElementIterator eIt = gridView.begin<0>(); eIt != endEIt; ++eIt) {
int mapperIdx = elementMapper.map(*eIt);
int indexSetIdx = indexSet.index(*eIt);
if (mapperIdx != indexSetIdx) {
std::cerr << "Mismatched mapper and indexset indices: " << mapperIdx << " vs. " << indexSetIdx << '\n';
return EXIT_FAILURE;
}
// std::cout << "mapperIdx = " << mapperIdx << ", indexSetIdx = " << indexSetIdx << std::endl;
}
return EXIT_SUCCESS;
// }
// catch (Dune::Exception &e){
// std::cerr << "Dune reported error: " << e << std::endl;
// }
// catch (...){
// std::cerr << "Unknown exception thrown!" << std::endl;
// }
return EXIT_FAILURE;
}
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();
}
}
double trajOptimizerplus::eval(vector<double> ¶ms) {
cout << "IN EVAL "<<itrcount++<<" "<<params.size()<<endl;
for (int i=0; i < params.size(); i++)
cout << "PARAMS IN: "<<i<<" "<<params.at(i)<<endl;
int factor = evidence.getFactor();
pair<int, int> dims = grid.dims();
int v_dim = seqFeat.num_V();
/*
pair<int, int> lowDims((int)ceil((float)dims.first/factor),
(int)ceil((float)dims.second/factor));
*/
vector<vector<vector<double> > >
prior(dims.first, vector<vector<double> >(dims.second,
vector<double> (v_dim,-HUGE_VAL)));
double obj = 0.0;
vector<double> gradient(params.size(), 0.0);
vector<vector<vector<double> > > occupancy;
vector<vector<double> > layerOccupancy;
layerOccupancy.resize(dims.first,vector<double>(dims.second,-HUGE_VAL));
vector<double> modelFeats, pathFeats;
for (int i=0; i < evidence.size(); i++) {
for (int j=0; j < params.size(); j++) {
cout << " "<<j<<" "<<params.at(j);
}
cout<<endl;
cout << "Evidence #"<<i<<endl;
vector<pair<int, int> >& trajectory = evidence.at(i);
vector<double>& velocityseq = evidence.at_v(i);
pair<int,int>& bot = evidence.at_bot(i);
// robot local blurres features
for (int r=1; r <= NUMROBFEAT; r++) {
cout << "Adding Robot Feature "<<r<<endl;
RobotLocalBlurFeature robblurFeat(grid,bot,10*r);
// RobotGlobalFeature robFeat(grid,bot);
posFeatures.push_back(robblurFeat);
}
cout << " Creating feature array"<<endl;
FeatureArray featArray2(posFeatures);
FeatureArray featArray(featArray2, factor);
for (int rr=1; rr<= NUMROBFEAT; rr++)
posFeatures.pop_back();
// split different posfeatures and seqfeature weights
vector<double> p_weights,s_weights;
int itr = 0;
for (; itr<featArray.size(); itr++)
p_weights.push_back(params[itr]);
for (; itr<params.size(); itr++)
s_weights.push_back(params[itr]);
//cout<<"Params"<<endl;
Parameters p_parameters(p_weights), s_parameters(s_weights);
/* cout<<featArray.size()<<endl;
cout<<params.size()<<endl;
cout<<p_weights.size()<<endl;
cout<<s_weights.size()<<endl;
cout<<p_parameters.size()<<endl;
cout<<s_parameters.size()<<endl;
*/
//cout<<"Reward"<<endl;
RewardMap rewards(featArray,seqFeat,p_parameters,s_parameters);
DisSeqPredictor predictor(grid, rewards, engine);
// sum of reward along the trajectory
double cost = 0.0;
//cout<< trajectory.size()<<endl;
for (int j=0; j < trajectory.size(); j++) {
//cout<<j<<" "<<trajectory.at(j).first<<" "<< trajectory.at(j).second<< " "<< seqFeat.getFeat(velocityseq.at(j))<<endl;
cost+=rewards.at(trajectory.at(j).first, trajectory.at(j).second, seqFeat.getFeat(velocityseq.at(j)));
}
State initial(trajectory.front(),seqFeat.getFeat(velocityseq.front()));
State destination(trajectory.back(),seqFeat.getFeat(velocityseq.back()));
//for (int k=0;k<v_dim;k++)
prior.at(destination.x()).at(destination.y()).at(destination.disV) = 0.0;
cout << "Initial: "<<initial.x()<<" "<<initial.y()<<" "<<initial.disV<<endl;
cout << "Destination: "<<destination.x()<<" "
<<destination.y()<<" "<<destination.disV<<endl;
predictor.setStart(initial);
predictor.setPrior(prior);
double norm = predictor.forwardBackwardInference(initial, occupancy);
for (int l=0; l<v_dim; l++) {
BMPFile gridView(dims.first, dims.second);
for (int x= 0; x<dims.first; x++) {
for(int y=0; y<dims.second; y++) {
layerOccupancy.at(x).at(y) = occupancy.at(x).at(y).at(l);
}
}
char buf[1024];
/*
RobotGlobalFeature robblurFeat(grid,bot);
gridView.addBelief(robblurFeat.getMap(), 0.0, 25, white, red);
gridView.addVector(trajectory, blue, factor);
gridView.addLabel(bot,green);
sprintf(buf, "../figures/feat%04d_%d.bmp",i,l);
gridView.write(buf);
*/
gridView.addBelief(layerOccupancy, -300.0, 5.0, white, red);
//grid.addObstacles(gridView, black);
gridView.addLabel(bot,green);
gridView.addVector(trajectory, blue, factor);
sprintf(buf, "../figures/train%04d_%d.bmp",i,l);
gridView.write(buf);
}
/*
for (int i=0; i < occupancy.size(); i++)
for (int j=0; j < occupancy.at(i).size(); j++)
if (occupancy.at(i).at(j) > -10)
cout << i <<" "<<j<<" "<<occupancy.at(i).at(j)<<endl;
*/
featArray.featureCounts(occupancy, modelFeats);
featArray.featureCounts(trajectory, pathFeats);
seqFeat.featureCounts_vec(occupancy,modelFeats);
seqFeat.featureCounts_vec(velocityseq,pathFeats);
for (int k=0; k < params.size(); k++) {
double diff = pathFeats.at(k) - modelFeats.at(k);
gradient.at(k) -= diff;
cout <<" Gradient ("<< k << " -grad: "<< gradient.at(k) <<" -path: "<<
pathFeats.at(k)<<" -model: "<< modelFeats.at(k)<<")";
}
cout<<endl;
cout << "OBJ: "<<cost-norm<< " "<<cost<<" "<<norm<<endl;
obj += (cost - norm);
/* obj is the path probability
* cost is the sum of rewards: sum f(s,a)
* norm is V(s_1->G), since here s_T = G, V(s_T->G) = 0*/
prior.at(destination.x()).at(destination.y()).at(destination.disV)
= -HUGE_VAL;
}
cout << "RETURN OBJ: "<<-obj<<endl;
params = gradient;
return -obj;
}
double trajectoryOptimizer::eval(vector<double> ¶ms, vector<double> &gradient) {
cout << "IN EVAL "<<params.size()<<endl;
for (int i=0; i < params.size(); i++)
cout << "PARAMS IN: "<<i<<" "<<params.at(i)<<endl;
int factor = evidence.getFactor();
// cout << "FACTOR: "<<factor<<endl;
FeatureArray featArray2(features);
FeatureArray featArray(featArray2, factor);
//cout<<"Dims featarray "<<featArray.dims().first<<" "<<featArray.dims().second<<endl;
Parameters parameters(params);
//cout << "Calculating rewards"<<endl;
RewardMap rewards(featArray, parameters);
pair<int, int> dims = grid.dims();
BMPFile gridView(dims.first, dims.second);
pair<int, int> lowDims((int)ceil((float)dims.first/factor),
(int)ceil((float)dims.second/factor));
//cout << "Computing prior"<<endl;
vector<vector<double> > prior(lowDims.first, vector<double>(lowDims.second,
-HUGE_VAL));
double obj = 0.0;
gradient.clear();
gradient.resize(params.size(), 0.0);
for (int i=0; i < evidence.size(); i++) {
Predictor predictor(grid, rewards, engine);
cout << "Evidence #"<<i<<endl;
vector<pair<int, int> > trajectory = evidence.at(i);
double cost = 0.0;
for (int j=0; j < trajectory.size(); j++){
double temp = rewards.at(trajectory.at(j).first,
trajectory.at(j).second);
cost += temp;
}
pair<int, int> initial = trajectory.front();
pair<int, int> destination = trajectory.back();
prior.at(destination.first).at(destination.second) = 0.0;
#if 0
cout << "Initial: "<<initial.first<<" "<<initial.second<<endl;
cout << "Destination: "<<destination.first<<" "
<<destination.second<<endl;
#endif
predictor.setStart(initial);
predictor.setPrior(prior);
vector<vector<double> > occupancy;
double norm = predictor.predict(initial, occupancy);
gridView.addBelief(occupancy, -300.0, 0.0, white, red);
gridView.addVector(trajectory, blue, factor);
char buf[1024];
sprintf(buf, "../figures/train%04d.bmp", i);
gridView.write(buf);
vector<double> modelFeats, pathFeats;
//cout << "Computing feature counts"<<endl;
/*
for (int i=0; i < occupancy.size(); i++)
for (int j=0; j < occupancy.at(i).size(); j++)
if (occupancy.at(i).at(j) > -10)
cout << i <<" "<<j<<" "<<occupancy.at(i).at(j)<<endl;
*/
featArray.featureCounts(occupancy, modelFeats);
featArray.featureCounts(trajectory, pathFeats);
cout << "GRADIENT"<<endl;
for (int k=0; k < params.size(); k++) {
double diff = pathFeats.at(k) - modelFeats.at(k);
gradient.at(k) -= diff;
cout << k << ": " << gradient.at(k) << " " << pathFeats.at(k)
<< " " << modelFeats.at(k) <<endl;
}
cout << "OBJ: "<<cost-norm<<endl;
cout << " "<<cost<<" "<<norm<<endl;
obj += (cost - norm);
prior.at(destination.first).at(destination.second) = -HUGE_VAL;
}
cout << "RETURN OBJ: "<<-obj<<endl;
return -obj;
}
