C++ McDArray::append方法代码示例

void BruteForceOptMatching::projectToPlaneApproxDirection(
    const McDArray<McVec3f>& vertices, const McDArray<McVec3f>& directions,
    const float planeZ, McDArray<McVec3f>& result) {

    for (int i = 0; i < vertices.size(); i++) {
        McPlane theZPlane(McVec3f(0, 0, 1), planeZ);
        McVec3f vertexCoord = vertices[i];
        McVec3f dir = directions[i] * -1;
        dir.normalize();
        float angle = dir.angle(McVec3f(0, 0, 1));
        if (angle > M_PI / 2.0)
            angle = M_PI - angle;

        McLine theLine(vertexCoord, vertexCoord + directions[i]);

        McVec3f intersectionPoint;
        bool intersected = theZPlane.intersect(theLine, intersectionPoint);
        // if(fabs(angle)<0.1)
        //   cout<<"\n Angle for vertex "<<i <<" too low: "<< angle;
        if (intersected && (fabs(angle) < (M_PI / 2.0 - M_PI / 8.0))) {
            result.append(intersectionPoint);
        } else {
            result.append(McVec3f(vertexCoord.x, vertexCoord.y, planeZ));
        }
    }
}

void BruteForceOptMatching::getAssignedValuesForVar(
    const McDMatrix<float>& allValues,
    const McDMatrix<int>& variableAssignmentMat,
    const McDArray<int>& possibleAssignments, const int label,
    const float zeroVal, McDArray<float>& values) {
    values.resize(0);
    for (int i = 0; i < possibleAssignments.size(); i++) {
        if (variableAssignmentMat[label][possibleAssignments[i]] > 1.e-6) {
            values.append(allValues[label][possibleAssignments[i]]);
        } else
            values.append(zeroVal);
    }
}

// Computes all variables that form a connected component in the
// adjacenceMatrix.
// The connected component chosen is arbitrary - it takes the first it finds.
bool BruteForceOptMatching::getConnectedComponent(
    const McDMatrix<int>& adjacenceMatrix,
    McDMatrix<int>& adjacenceMatrixWithoutConnctedComponent,
    McDArray<int>& connComp) {
    adjacenceMatrixWithoutConnctedComponent.resize(adjacenceMatrix.nRows(),
                                                   adjacenceMatrix.nCols());
    memcpy(adjacenceMatrixWithoutConnctedComponent.dataPtr(),
           adjacenceMatrix.dataPtr(),
           sizeof(int) * adjacenceMatrix.nRows() * adjacenceMatrix.nCols());

    // find first a startpoint
    int start = -1;
    connComp.resize(0);
    for (int i = 0; i < adjacenceMatrix.nRows(); i++) {
        for (int j = i; j < adjacenceMatrix.nCols(); j++) {
            if (adjacenceMatrix[i][j] == 1) {
                start = i;
                break;
            }
        }
    }
    if (start == -1)
        return false;

    McDArray<int> queue;
    queue.append(start);
    connComp.clear();
    while (queue.size() > 0) {
        int cur = queue.last();
        connComp.append(cur);
        queue.pop_back();
        for (int i = 0; i < adjacenceMatrixWithoutConnctedComponent.nCols();
             i++) {
            if (adjacenceMatrixWithoutConnctedComponent[cur][i] == 1) {
                queue.push(i);
                adjacenceMatrixWithoutConnctedComponent[cur][i] = 0;
            }
        }
    }
    // remove duplicates
    connComp.sort(&mcStandardCompare);
    int cur = connComp.last();
    for (int i = connComp.size() - 2; i >= 0; i--) {
        if (cur == connComp[i])
            connComp.remove(i, 1);
        else
            cur = connComp[i];
    }
    return true;
}

void BruteForceOptMatching::getMaxProbAssignments(
    const BP& ia, const FactorGraph& fg, const ConnectedFactorGraph& graph,
    McDArray<McVec2i>& pairs) {

    for (int i = 0; i < graph.variables.size(); i++) {

        McDArray<int> possibleAssignments;
        getAssignmentsForVariable(graph.variables[i], possibleAssignments);
        Factor belief =
            ia.belief(Var(graph.variables[i], possibleAssignments.size() + 1));
        float maxVal = -1 * FLT_MAX;
        int maxIdx = -1;

        for (int j = 0; j < possibleAssignments.size() + 1; j++) {
            if (belief.get(j) > maxVal) {
                maxVal = belief.get(j);
                maxIdx = j;
            }
        }
        int indexOfAssignmentInVertexList =
            mapVariableAssignmentToIndexInVertexList(graph.variables[i],
                                                     maxIdx);

        McVec2i pair =
            McVec2i(graph.variables[i], indexOfAssignmentInVertexList);

        pairs.append(pair);
    }
    outputSingleFactorValues(graph);

    //    std::vector<std::size_t> maxes= ia.findMaximum();
    //    vector<std::size_t>::iterator it=maxes.begin();
}

void HxCPDSpatialGraphWarp::applyRigidDeformationToSliceVanMises(
    SpatialGraphSelection& slice, HxSpatialGraph* spatialGraph,
    const ma::CPDLinearAligner& deformation, const McDMatrix<double>& R,
    const double s, const McDVector<double>& t) {

    McWatch watch;
    watch.start();

    ma::SliceSelector ssh(spatialGraph, "TransformInfo");
    SpatialGraphSelection fullSliceSelection;

    ssh.getSlice(ssh.getSliceAttributeValueFromIndex(1), fullSliceSelection);

    for (int i = 0; i < fullSliceSelection.getNumSelectedEdges(); i++) {
        int edge = fullSliceSelection.getSelectedEdge(i);
        McDArray<McVec3f> newEdgePoints;
        for (int j = 0; j < spatialGraph->getNumEdgePoints(edge); j++) {

            McVec3f edgePoint = spatialGraph->getEdgePoint(edge, j);
            const McVec3f edgePointWarped =
                deformation.warpPoint(edgePoint, R, s, t);
            newEdgePoints.append(edgePointWarped);
        }
        spatialGraph->setEdgePoints(edge, newEdgePoints);
    }

    for (int i = 0; i < fullSliceSelection.getNumSelectedVertices(); i++) {
        int curVertex = fullSliceSelection.getSelectedVertex(i);
        McVec3f curCoord = spatialGraph->getVertexCoords(curVertex);
        const McVec3f curCoordWarped = deformation.warpPoint(curCoord, R, s, t);
        spatialGraph->setVertexCoords(curVertex, curCoordWarped);
    }

    std::cout << "\n Apply deformation took " << watch.stop() << " seconds.";
}

void BruteForceOptMatching::getAssignmentsForVariable(
    const int variableLabel,
    McDArray<int>& indicesThisVariableCanBeAssignedTo) {
    indicesThisVariableCanBeAssignedTo.resize(0);
    for (int i = 0; i < mVariableAssignmentMat.nCols(); i++) {
        if (mVariableAssignmentMat[variableLabel][i] > 0)
            indicesThisVariableCanBeAssignedTo.append(i);
    }
}

void HxMovingLeastSquaresWarp::prepareLandmarks(McDArray<McVec2d>& p1,
                                               McDArray<McVec2d>& p2) {
    int set1 = 0;
    int set2 = 1;

    HxLandmarkSet* pointSet = hxconnection_cast<HxLandmarkSet>(portData);

    if (!pointSet)
        return;

    p1.resize(0);
    p2.resize(0);
    int nPoints = pointSet->getNumMarkers();
    for (int i = 0; i < nPoints; i++) {
        p1.append(McVec2d(pointSet->getCoords(set1)[i].x,
                          pointSet->getCoords(set1)[i].y));
        p2.append(McVec2d(pointSet->getCoords(set2)[i].x,
                          pointSet->getCoords(set2)[i].y));
    }
}

void BruteForceOptMatching::getSingletonProbs(
    const McDMatrix<float>& angleMatrix,
    const McDMatrix<float>& projDistanceMatrix,
    const McDMatrix<float>& distanceMatrix3d,
    const McDMatrix<int>& variableAssignmentMat,
    const McDArray<int>& assignments, const int varLabel,
    McDArray<float>& probs) {

    McDArray<float> angleValues;
    getAssignedValuesForVar(angleMatrix, variableAssignmentMat, assignments,
                            varLabel, 0, angleValues);
    mcassert(angleValues.size() == assignments.size());
    // add dummy
    angleValues.append(mAngleThreshold / 2.0);
    // compute actual prob representation
    computeAngleProbs(angleValues);

    McDArray<float> projDistValues;
    getAssignedValuesForVar(projDistanceMatrix, variableAssignmentMat,
                            assignments, varLabel, FLT_MAX, projDistValues);
    // add dummy
    projDistValues.append(mDistanceThresholdProjected / 2.0);
    // compute actual prob representation
    computeProjDistProbs(projDistValues);

    McDArray<float> distValues3d;
    getAssignedValuesForVar(distanceMatrix3d, variableAssignmentMat,
                            assignments, varLabel, FLT_MAX, distValues3d);
    // add dummy
    distValues3d.append(mDistanceThreshold3d / 2.0);
    // compute actual prob representation
    compute3dDistProbs(distValues3d);

    probs.resize(assignments.size() + 1);
    // set values of factors: Multiply angle and dist threshold
    for (int j = 0; j < probs.size(); j++) {
        probs[j] = projDistValues[j] * angleValues[j];
    }
}

void BruteForceOptMatching::checkAmbiguitiesInAssignments(
    const ConnectedFactorGraph& graph,
    const McDArray<McVec2i>& matchedPointPairs, McDArray<int>& ambiguities) {

    McBitfield assignedAlready(mCoords2.size());
    assignedAlready.unsetAll();

    for (int i = 0; i < matchedPointPairs.size(); i++) {
        if (matchedPointPairs[i].y < 0)
            continue;
        if (assignedAlready[matchedPointPairs[i].y])
            ambiguities.append(matchedPointPairs[i].x);
        assignedAlready.set(matchedPointPairs[i].y);
    }
}

void HxIteratePointMatchingUntilConvergence::getListOfNeededEvidenceNodes(
    McDArray<int>& nodesToAssign) {
    HxSpatialGraph* graph = hxconnection_cast<HxSpatialGraph>(portData);
    McString evidenceLabel = qPrintable(QString(
        portEvidenceHeuristic.getLabel(portEvidenceHeuristic.getValue())));
    const EdgeVertexAttribute* evidenceAttrib =
        dynamic_cast<const EdgeVertexAttribute*>(
            graph->findAttribute(HxSpatialGraph::VERTEX, evidenceLabel));
    for (int i = 0; i < graph->getNumVertices(); i++) {
        float test = evidenceAttrib->getIntDataAtIdx(i);
        if (test > 0) {
            nodesToAssign.append(i);
        }
    }
}

float BruteForceOptMatching::getMedianZ(const McDArray<McVec3f>& vertices) {

    if (!vertices.size())
        return -1 * FLT_MAX;
    McDArray<float> zs;
    float mean = 0.0;

    for (int i = 0; i < vertices.size(); i++) {
        zs.append(vertices[i].z);
        mean += vertices[i].z;
    }
    zs.sort(&mcStandardCompare);
    int medianIdx = zs.size() / 2.0;
    // cout <<"MeanZ: "<<mean/vertices.size();
    // return mean/vertices.size();
    return zs[medianIdx];
}

void BruteForceOptMatching::checkAmbiguities(const BP& ia,
                                             const FactorGraph& fg,
                                             const ConnectedFactorGraph& graph,
                                             McDArray<int>& ambiguities) {
    for (int h = 0; h < graph.variables.size(); h++) {
        McDArray<int> possibleAssignments;
        getAssignmentsForVariable(graph.variables[h], possibleAssignments);
        Factor belief =
            ia.belief(Var(graph.variables[h], possibleAssignments.size() + 1));

        float maxProb = belief.max();
        int countSame = 0;

        for (int k = 0; k < possibleAssignments.size() + 1; k++) {
            float curProb = belief.get(k);
            if (fabs(curProb - maxProb) < 0.1)
                countSame++;
        }

        /////
        cout << "\n Belief for var " << graph.variables[h] << "\n";
        for (int k = 0; k < possibleAssignments.size() + 1; k++) {
            float curProb = belief.get(k);
            cout << curProb << " ";
        }
        cout << "\n";

        ////

        if (countSame > 1) {
            // oh no! We found an ambiguos assignment!

            ambiguities.append(graph.variables[h]);

            // print it out:
            cout << "Found an ambiguous assignemnt to variable "
                 << graph.variables[h] << "\n";
            for (int k = 0; k < possibleAssignments.size() + 1; k++) {
                float curProb = belief.get(k);
                cout << curProb << " ";
            }
            cout << "\n";
        }
    }
}

void HxCPDSpatialGraphWarp::applyNLDeformationToSlice(
    SpatialGraphSelection& slice, HxSpatialGraph* spatialGraph,
    const McDArray<McVec3f>& origCoords,
    const McDArray<McVec3f>& shiftedCoords) {

    McWatch watch;
    watch.start();

    MovingLeastSquares mls;
    mls.setAlpha(portAlphaForMLS.getValue());
    mls.setLandmarks(origCoords, shiftedCoords);
    ma::SliceSelector ssh(spatialGraph, "TransformInfo");
    SpatialGraphSelection fullSliceSelection;
    ssh.getSlice(ssh.getSliceAttributeValueFromIndex(1), fullSliceSelection);

    for (int i = 0; i < fullSliceSelection.getNumSelectedEdges(); i++) {
        int edge = fullSliceSelection.getSelectedEdge(i);
        McDArray<McVec3f> newEdgePoints;
        for (int j = 0; j < spatialGraph->getNumEdgePoints(edge); j++) {

            McVec3f edgePoint = spatialGraph->getEdgePoint(edge, j);
            warpPoint(edgePoint, edgePoint, mls);
            newEdgePoints.append(edgePoint);
        }
        spatialGraph->setEdgePoints(edge, newEdgePoints);
    }

    for (int i = 0; i < fullSliceSelection.getNumSelectedVertices(); i++) {
        int curVertex = fullSliceSelection.getSelectedVertex(i);
        McVec3f curCoord = spatialGraph->getVertexCoords(curVertex);
        McVec3f curCoordWarped;
        warpPoint(curCoord, curCoordWarped, mls);
        spatialGraph->setVertexCoords(curVertex, curCoordWarped);
        // Add new segments that indicate shift.
        if (slice.isSelectedVertex(curVertex)) {
            int newVertex = spatialGraph->addVertex(curCoord);
            McDArray<McVec3f> edgePoints(2);
            edgePoints[0] = curCoord;
            edgePoints[1] = curCoordWarped;
            spatialGraph->addEdge(newVertex, curVertex, edgePoints);
        }
    }

    std::cout << "\n Apply deformation took " << watch.stop() << " seconds.";
}

void HxRotateSpatialGraphStackSliceAndCDP::rotateSlice(HxSpatialGraph* graph,
                                                       const double angle) {
    mtalign::SliceSelector ssh(graph, "TransformInfo");
    SpatialGraphSelection fullSliceSelection;
    ssh.getSlice(ssh.getSliceAttributeValueFromIndex(1), fullSliceSelection);
    McMat3f rotMat = McMat3f::IDENTITY;
    rotMat[0][0] = cos(angle);
    rotMat[0][1] = -sin(angle);
    rotMat[1][0] = sin(angle);
    rotMat[1][1] = cos(angle);
    // compute barycenter
    McVec3f barycenter(0, 0, 0);
    for (int i = 0; i < fullSliceSelection.getNumSelectedVertices(); i++) {
        barycenter +=
            graph->getVertexCoords(fullSliceSelection.getSelectedVertex(i));
    }
    barycenter /= fullSliceSelection.getNumSelectedVertices();
    McVec3f rotatedBarycenter;
    rotMat.multMatrixVec(barycenter, rotatedBarycenter);
    McVec3f translation = barycenter - rotatedBarycenter;

    for (int i = 0; i < fullSliceSelection.getNumSelectedEdges(); i++) {
        int edge = fullSliceSelection.getSelectedEdge(i);
        McDArray<McVec3f> newEdgePoints;
        for (int j = 0; j < graph->getNumEdgePoints(edge); j++) {
            McVec3f edgePoint = graph->getEdgePoint(edge, j);
            McVec3f rotatedEdgePoint;
            rotMat.multMatrixVec(edgePoint, rotatedEdgePoint);
            rotatedEdgePoint += translation;
            newEdgePoints.append(rotatedEdgePoint);
        }
        graph->setEdgePoints(edge, newEdgePoints);
    }

    for (int i = 0; i < fullSliceSelection.getNumSelectedVertices(); i++) {
        int curVertex = fullSliceSelection.getSelectedVertex(i);
        McVec3f curCoord = graph->getVertexCoords(curVertex);
        McVec3f rotatedVertex;
        rotMat.multMatrixVec(curCoord, rotatedVertex);
        rotatedVertex += translation;
        graph->setVertexCoords(curVertex, rotatedVertex);
    }
}