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