C++ Primitive::prepareParameters方法代码示例

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

//.........这里部分代码省略.........

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