void plan(KoulesSetup& ks, double maxTime, const std::string& outputFile)
{
if (ks.solve(maxTime))
{
std::ofstream out(outputFile.c_str());
oc::PathControl path(ks.getSolutionPath());
path.interpolate();
if (!path.check())
OMPL_ERROR("Path is invalid");
writeParams(out);
path.printAsMatrix(out);
if (!ks.haveExactSolutionPath())
OMPL_INFORM("Solution is approximate. Distance to actual goal is %g",
ks.getProblemDefinition()->getSolutionDifference());
OMPL_INFORM("Output saved in %s", outputFile.c_str());
}
#if 0
// Get the planner data, save the ship's (x,y) coordinates to one file and
// the edge information to another file. This can be used for debugging
// purposes; plotting the tree of states might give you some idea of
// a planner's strategy.
ob::PlannerData pd(ks.getSpaceInformation());
ks.getPlannerData(pd);
std::ofstream vertexFile((outputFile + "-vertices").c_str()), edgeFile((outputFile + "-edges").c_str());
double* coords;
unsigned numVerts = pd.numVertices();
std::vector<unsigned int> edgeList;
for (unsigned int i = 0; i < numVerts; ++i)
{
coords = pd.getVertex(i).getState()->as<KoulesStateSpace::StateType>()->values;
vertexFile << coords[0] << ' ' << coords[1] << '\n';
pd.getEdges(i, edgeList);
for (unsigned int j = 0; j < edgeList.size(); ++j)
edgeFile << i << ' ' << edgeList[j] << '\n';
}
#endif
}
int main(int argc, char* argv[])
{
if(argc < 3)
{
std::cout << "Please provide all necessary parameters to convert opengm models." << std::endl;
std::cout << "convertHDF5ToCSV <opengm hdf5 file> <group name>" << std::endl;
return -1;
}
using Model = TST::GraphicalModel;
Model gm;
opengm::hdf5::load(gm, argv[1], argv[2]);
std::ofstream nrLabels("nrLabels.csv");
std::ofstream uFactors("uFactors.csv");
std::ofstream pwFactors("pwFactors.csv");
std::ofstream edgeFile("edgeListFile.txt");
bool notSaved = true;
size_t oneLabel[1];
size_t twoLabels[2];
for ( auto var=0; var < gm.numberOfVariables(); ++var)
{
for (auto factor = 0; factor < gm.numberOfFactors(var); ++factor)
{
auto facId = gm.factorOfVariable(var,factor);
if ( gm.numberOfVariables(facId) == 1) //unary Factor
{
for (auto label = 0; label < gm.numberOfLabels(var); ++label)
{
oneLabel[0] = label;
uFactors << gm[facId](oneLabel) << std::endl;
}
nrLabels << gm.numberOfLabels(var) << std::endl;
}
else
{
int var2;
for (auto nrVar = 0; nrVar < gm.numberOfVariables(facId); ++nrVar)
{
var2 = gm.variableOfFactor(facId,nrVar);
if (var != var2)
break;
}
if ( notSaved ) //only for equal pw functions, yet
{
for (auto label = 0; label < gm.numberOfLabels(var); ++label)
{
twoLabels[0] = label;
for (auto label2 = 0; label2 < gm.numberOfLabels(var2); ++label2)
{
twoLabels[1] = label2;
if ( label2 == (gm.numberOfLabels(var2)-1) )
pwFactors << gm[facId](twoLabels) << std::endl;
else
pwFactors << gm[facId](twoLabels) << ",";
}
}
notSaved = false;
}
edgeFile << var << " " << var2 << std::endl;
}
}
}
nrLabels.close();
uFactors.close();
pwFactors.close();
edgeFile.close();
}
