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; }