bool GlobFit::solve(std::vector<RelationEdge>& vecRelationEdge, RelationEdge::RelationEdgeType currentStage, const std::string& stageName)
{
// dump data to file for debugging in matlab
dumpData(vecRelationEdge, stageName);
size_t nConstraintNum = vecRelationEdge.size();
std::string optimization;
if (currentStage < RelationEdge::RET_COAXIAL) {
optimization = "OptimizeNormal";
std::cout << "Optimize Normal..." << std::endl;
} else if (currentStage < RelationEdge::RET_COPLANAR) {
optimization = "OptimizePoint";
std::cout << "Optimize Point..." << std::endl;
} else if (currentStage < RelationEdge::RET_EQUAL_RADIUS) {
optimization = "OptimizeDistance";
std::cout << "Optimize Distance..." << std::endl;
} else {
optimization = "OptimizeRadius";
std::cout << "Optimize Radius..." << std::endl;
}
if (nConstraintNum == 0)
{
std::cout << "Empty constraint set." << std::endl;
return true;
}
size_t numPrimitives = _vecPrimitive.size();
mxArray* inputParameters = mxCreateDoubleMatrix(numPrimitives, Primitive::getNumParameter(), mxREAL);
double* pInputParameters = mxGetPr(inputParameters);
for (size_t i = 0; i < numPrimitives; ++i) {
Primitive* pPrimitive = _vecPrimitive[i];
pPrimitive->prepareParameters();
for (size_t j = 0; j < Primitive::getNumParameter(); ++ j) {
pInputParameters[j*numPrimitives+i] = pPrimitive->getParameter(j);
}
}
engPutVariable(matlabEngine, "inputParameters", inputParameters);
mxArray* constraints = mxCreateNumericMatrix(nConstraintNum, RelationEdge::getNumParameter(), mxINT32_CLASS, mxREAL);
int* pConstraints = (int*)mxGetData(constraints);
for (size_t i = 0; i < nConstraintNum; ++i) {
vecRelationEdge[i].dumpData(pConstraints, nConstraintNum, i);
}
engPutVariable(matlabEngine, "constraints", constraints);
std::string path = boost::filesystem::current_path().string();
path = "cd "+path+"/matlab;";
engEvalString(matlabEngine, path.c_str());
size_t szOutputBuffer = 65536;
char* matlabOutputBuffer = new char[szOutputBuffer];
engOutputBuffer(matlabEngine, matlabOutputBuffer, szOutputBuffer);
std::string output = "[outputParameters, initialFittingError, exitFittingError, exitFlag]";
std::string input = "(inputParameters, maxIterNum, numVertices, primitiveType, coordX, coordY, coordZ, normalX, normalY, normalZ, confVertices, constraints);";
std::string command = output+"="+optimization+input;
engEvalString(matlabEngine, command.c_str());
matlabOutputBuffer[szOutputBuffer - 1] = '\0';
printf("%s\n", matlabOutputBuffer);
engOutputBuffer(matlabEngine, NULL, 0);
delete[] matlabOutputBuffer;
mxArray* outputParameters = engGetVariable(matlabEngine, "outputParameters");
double *pOutputParameters = mxGetPr(outputParameters);
mxArray* initialFittingError = engGetVariable(matlabEngine, "initialFittingError");
double *pInitialFittingError = mxGetPr(initialFittingError);
mxArray* exitFittingError = engGetVariable(matlabEngine, "exitFittingError");
double *pExitFittingError = mxGetPr(exitFittingError);
mxArray* exitFlag = engGetVariable(matlabEngine, "exitFlag");
double *pExitFlag = mxGetPr(exitFlag);
bool bValidOptimization = (*pExitFlag >= 0);
// posterior check: consider invalid if fitting error increased too much
// however, if the threshold is very big, the fitting error may increase a lot
// so, be careful with this
bValidOptimization &= (*pExitFittingError < 10*(*pInitialFittingError));
if (!bValidOptimization) {
mxDestroyArray(constraints);
mxDestroyArray(inputParameters);
mxDestroyArray(outputParameters);
mxDestroyArray(initialFittingError);
mxDestroyArray(exitFittingError);
mxDestroyArray(exitFlag);
std::cout << "No feasible solution found." << std::endl;
return false;
}
// update primitives
for (size_t i = 0; i < numPrimitives; ++ i) {
Primitive* pPrimitive = _vecPrimitive[i];
for (size_t j = 0; j < Primitive::getNumParameter(); ++ j) {
pPrimitive->setParameter(j, pOutputParameters[j*numPrimitives+i]);
}
pPrimitive->applyParameters();
}
// destroy matrix
mxDestroyArray(constraints);
mxDestroyArray(inputParameters);
mxDestroyArray(outputParameters);
mxDestroyArray(initialFittingError);
mxDestroyArray(exitFittingError);
mxDestroyArray(exitFlag);
return true;
}
void GlobFit::dumpData(const std::vector<RelationEdge>& vecRelationEdge, const std::string& stageName)
{
size_t maxVerticesNum = 0;
for (size_t i = 0, iEnd = _vecPrimitive.size(); i < iEnd; ++ i) {
Primitive* pPrimitive = _vecPrimitive[i];
pPrimitive->prepareParameters();
maxVerticesNum = std::max(pPrimitive->getPointIdx().size(), maxVerticesNum);
}
std::locale loc("C");
std::string path = boost::filesystem::current_path().string();
path = path+"/matlab/data/"+stageName+"/";
boost::filesystem::create_directories(path);
std::ofstream constraints((path+"constraints.dat").c_str());
std::ofstream primitiveType((path+"primitiveType.dat").c_str());
std::ofstream inputParameters((path+"inputParameters.dat").c_str());
std::ofstream numVertices((path+"numVertices.dat").c_str());
std::ofstream coordX((path+"coordX.dat").c_str());
std::ofstream coordY((path+"coordY.dat").c_str());
std::ofstream coordZ((path+"coordZ.dat").c_str());
std::ofstream normalX((path+"normalX.dat").c_str());
std::ofstream normalY((path+"normalY.dat").c_str());
std::ofstream normalZ((path+"normalZ.dat").c_str());
std::ofstream confVertices((path+"confVertices.dat").c_str());
constraints.imbue(loc);
primitiveType.imbue(loc);
inputParameters.imbue(loc);
inputParameters.precision(16);
numVertices.imbue(loc);
coordX.imbue(loc);
coordX.precision(16);
coordY.imbue(loc);
coordY.precision(16);
coordZ.imbue(loc);
coordZ.precision(16);
normalX.imbue(loc);
normalX.precision(16);
normalY.imbue(loc);
normalY.precision(16);
normalZ.imbue(loc);
normalZ.precision(16);
confVertices.imbue(loc);
confVertices.precision(16);
for (size_t i = 0, iEnd = vecRelationEdge.size(); i < iEnd; ++ i) {
const RelationEdge& relationEdge = vecRelationEdge[i];
constraints << relationEdge << std::endl;
}
for (size_t i = 0, iEnd = _vecPrimitive.size(); i < iEnd; ++ i) {
Primitive* pPrimitive = _vecPrimitive[i];
primitiveType << pPrimitive->getType() << std::endl;
pPrimitive->prepareParameters();
for (size_t j = 0, jEnd = Primitive::getNumParameter(); j < jEnd; ++ j) {
inputParameters << pPrimitive->getParameter(j) << " ";
}
inputParameters << std::endl;
numVertices << pPrimitive->getPointIdx().size() << std::endl;
for (size_t j = 0, jEnd = pPrimitive->getPointIdx().size(); j < jEnd; ++ j) {
const RichPoint* richPoint = _vecPointSet[pPrimitive->getPointIdx()[j]];
const Point& point = richPoint->point;
const Vector& normal = richPoint->normal;
coordX << point.x() << " ";
coordY << point.y() << " ";
coordZ << point.z() << " ";
normalX << normal.x() << " ";
normalY << normal.y() << " ";
normalZ << normal.z() << " ";
confVertices << richPoint->confidence << " ";
}
for (size_t j = pPrimitive->getPointIdx().size(); j < maxVerticesNum; ++ j) {
coordX << 0 << " ";
coordY << 0 << " ";
coordZ << 0 << " ";
normalX << 0 << " ";
normalY << 0 << " ";
normalZ << 0 << " ";
confVertices << 0 << " ";
}
coordX << std::endl;
coordY << std::endl;
coordZ << std::endl;
normalX << std::endl;
normalY << std::endl;
normalZ << std::endl;
confVertices << std::endl;
}
return;
}
