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