C++ Factor::set方法代码示例

void BruteForceOptMatching::fillSingletonVals(
    const McDMatrix<float>& angleMatrix,
    const McDMatrix<float>& projDistanceMatrix,
    const McDMatrix<float>& distanceMatrix3d,
    const McDMatrix<int>& variableAssignmentMat,
    vector<Factor>& singletonFactors, ConnectedFactorGraph& graph) {

    while (!singletonFactors.empty()) {

        Factor curFac = singletonFactors.back();
        singletonFactors.pop_back();

        int varLabel = curFac.vars().front().label();
        int assignmentInEvidence;
        int evidenceAssgnmentInWholeModel =
            getEvidenceAssignment(graph, varLabel, assignmentInEvidence);
        handleEvidenceAssignment(varLabel, assignmentInEvidence,
                                 evidenceAssgnmentInWholeModel);
        // Get all possible assignments for variable
        McDArray<int> possibleAssignmentsForVariable;
        getAssignmentsForVariable(varLabel, possibleAssignmentsForVariable);
        if (assignmentInEvidence < 0) {
            McDArray<float> singletonProbs;
            getSingletonProbs(angleMatrix, projDistanceMatrix, distanceMatrix3d,
                              variableAssignmentMat,
                              possibleAssignmentsForVariable, varLabel,
                              singletonProbs);

            // set values of factors: Multiply angle and dist threshold
            for (int j = 0; j < curFac.vars().front().states(); j++) {
                curFac.set(j, singletonProbs[j]);
            }
        } else {
            for (int j = 0; j < curFac.vars().front().states(); j++) {
                if (j == assignmentInEvidence) {
                    curFac.set(j, 1);
                } else {
                    curFac.set(j, 0);
                }
            }
        }
        graph.factors.push_back(curFac);
    }
}

void mexFunction( int nlhs, mxArray *plhs[], int nrhs, const mxArray*prhs[] ) {
    // Check for proper number of arguments
    if( ((nrhs < NR_IN) || (nrhs > NR_IN + NR_IN_OPT)) || ((nlhs < NR_OUT) || (nlhs > NR_OUT + NR_OUT_OPT)) ) {
        mexErrMsgTxt("Usage: [logZ,q,qv,qf,qmap,margs] = dai_jtree(psi,varsets,opts)\n\n"
        "\n"
        "INPUT:  psi        = linear cell array containing the factors\n"
        "                     (psi{i} should be a structure with a Member field\n"
        "                     and a P field).\n"
        "        varsets    = linear cell array containing varsets for which marginals\n"
        "                     are requested.\n"
        "        opts       = string of options.\n"
        "\n"
        "OUTPUT: logZ       = logarithm of the partition sum.\n"
        "        q          = linear cell array containing all calculated marginals.\n"
        "        qv         = linear cell array containing all variable marginals.\n"
        "        qf         = linear cell array containing all factor marginals.\n"
        "        qmap       = linear array containing the MAP state.\n"
        "        margs      = linear cell array containing all requested marginals.\n");
    }

    // Get psi and construct factorgraph
    vector<Factor> factors = mx2Factors(PSI_IN, 0);
    FactorGraph fg(factors);

    // Get varsets
    vector<Permute> perms;
    vector<VarSet> varsets = mx2VarSets(VARSETS_IN,fg,0,perms);

    // Get options string
    char *opts;
    size_t buflen = mxGetN( OPTS_IN ) + 1;
    opts = (char *)mxCalloc( buflen, sizeof(char) );
    mxGetString( OPTS_IN, opts, buflen );
    // Convert to options object props
    stringstream ss;
    ss << opts;
    PropertySet props;
    ss >> props;

    // Construct InfAlg object, init and run
    JTree jt = JTree( fg, props );
    jt.init();
    jt.run();

    // Save logZ
	double logZ = NAN;
    logZ = jt.logZ();

    // Hand over results to MATLAB
    LOGZ_OUT = mxCreateDoubleMatrix(1,1,mxREAL);
    *(mxGetPr(LOGZ_OUT)) = logZ;

    Q_OUT = Factors2mx(jt.beliefs());

    if( nlhs >= 3 ) {
        vector<Factor> qv;
        qv.reserve( fg.nrVars() );
        for( size_t i = 0; i < fg.nrVars(); i++ )
            qv.push_back( jt.belief( fg.var(i) ) );
        QV_OUT = Factors2mx( qv );
    }

    if( nlhs >= 4 ) {
        vector<Factor> qf;
        qf.reserve( fg.nrFactors() );
        for( size_t I = 0; I < fg.nrFactors(); I++ )
            qf.push_back( jt.belief( fg.factor(I).vars() ) );
        QF_OUT = Factors2mx( qf );
    }

    if( nlhs >= 5 ) {
        std::vector<size_t> map_state;
        bool supported = true;
        try {
            map_state = jt.findMaximum();
        } catch( Exception &e ) {
            if( e.getCode() == Exception::NOT_IMPLEMENTED )
                supported = false;
            else
                throw;
        }
        if( supported ) {
            QMAP_OUT = mxCreateNumericMatrix(map_state.size(), 1, mxUINT32_CLASS, mxREAL);
            uint32_T* qmap_p = reinterpret_cast<uint32_T *>(mxGetPr(QMAP_OUT));
            for (size_t n = 0; n < map_state.size(); ++n)
                qmap_p[n] = map_state[n];
        } else {
            mexErrMsgTxt("Calculating a MAP state is not supported by this inference algorithm.");
        }
    }

    if( nlhs >= 6 ) {
        vector<Factor> margs;
        margs.reserve( varsets.size() );
        for( size_t s = 0; s < varsets.size(); s++ ) {
            Factor marg;
            jt.init();
            jt.run();
            marg = jt.calcMarginal( varsets[s] );

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

// This method sets the mutual exclusive constraint for each pair factor
void
BruteForceOptMatching::fillPairVals(const McDMatrix<int>& variableAssignmentMat,
                                    const McDMatrix<float>& projDistanceMatrix,
                                    vector<Factor>& pairFactors,
                                    ConnectedFactorGraph& graph) {
    while (!pairFactors.empty()) {
        Factor curFac = pairFactors.back();
        pairFactors.pop_back();
        int numStatesVar1 = curFac.vars().front().states();
        int numStatesVar2 = curFac.vars().back().states();

        // get the evidence assignment for the two vars, to make sure that the
        // evidence was assigned correctly!
        int var1 = curFac.vars().elements()[0].label();
        int var2 = curFac.vars().elements()[1].label();
        int assignmentForVar1, assignmentForVar2;
        int assignmentIndexInModelForVar1 =
            getEvidenceAssignment(graph, var1, assignmentForVar1);
        int assignmentIndexInModelForVar2 =
            getEvidenceAssignment(graph, var2, assignmentForVar2);

        // create shift matrix, ensuring same shift for vertices are weighted
        // higher
        McDMatrix<float> sameShift(numStatesVar1, numStatesVar2);
        createSameShiftMatrix(graph, curFac, projDistanceMatrix, sameShift);
        // we must check, if the hard coded assignemnts in the evidence do not
        // conflict with the pair factors.
        // this can happen, if evidence was given, that would result in 0
        // probability according to parameters
        if (assignmentForVar1 > -1 && assignmentForVar2 > -1) {
            // both were assigned. Now, check if pair entry is 0
            if (sameShift[assignmentForVar1][assignmentForVar2] == 0.0) {
                // add the assignments to the queer evidence
                handleEvidenceAssignment(var1, -3,
                                         assignmentIndexInModelForVar1);
                handleEvidenceAssignment(var2, -3,
                                         assignmentIndexInModelForVar2);
                // set the shift probability to some value >0, the value does
                // not matter
                sameShift[assignmentForVar1][assignmentForVar2] = 1.e-5;
            }
        }

        // create the actual probability matrix

        McDMatrix<float> finalProb = sameShift;  // here woe could multiply
                                                 // other probability factors as
                                                 // well....

        for (int i = 0; i < numStatesVar1 - 1; i++) {
            for (int j = 0; j < numStatesVar2 - 1; j++) {
                int assignmentIndexInArrayForVar1 =
                    mapVariableAssignmentToIndexInVertexList(
                        curFac.vars().front().label(), i);
                int assignmentIndexInArrayForVar2 =
                    mapVariableAssignmentToIndexInVertexList(
                        curFac.vars().back().label(), j);
                if (assignmentIndexInArrayForVar1 ==
                    assignmentIndexInArrayForVar2)
                    finalProb[i][j] = 0;
            }
        }

        // set values for factor

        finalProb = finalProb.transpose();

        for (int i = 0; i < numStatesVar1 * numStatesVar2; i++) {
            curFac.set(i, finalProb.dataPtr()[i]);
        }
        graph.factors.push_back(curFac);
    }
}