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.";
}
float AssignEModulus::averageVoxels( const OBLelement3D* FEelement,
const McDArray<McVec3f>& vertices,
HxUniformScalarField3* field )
{
if ( !FEelement ) { return 0; }
McVec3f pcoords;
// Create an instance of HxLoc3Regular instead of HxLocation3,
// because we know we are dealing with a uniform scalar field and
// we want access to the voxelindices
//HxLoc3Regular* location = image->coords()->createLocation();
HxLoc3Regular* location = (HxLoc3Regular*)field->createLocation();
// determine bounding box of voxel indices within element
location->set( vertices[0][0], vertices[0][1], vertices[0][2] );
int indexbbox[6] = { location->ix, location->ix,
location->iy, location->iy,
location->iz, location->iz };
for ( int el = 1; el < vertices.size(); el++ )
{
location->move( vertices[el][0], vertices[el][1], vertices[el][2] );
if ( location->ix < indexbbox[0] ) { indexbbox[0] = location->ix; }
if ( location->ix > indexbbox[1] ) { indexbbox[1] = location->ix; }
if ( location->iy < indexbbox[2] ) { indexbbox[2] = location->iy; }
if ( location->iy > indexbbox[3] ) { indexbbox[3] = location->iy; }
if ( location->iz < indexbbox[4] ) { indexbbox[4] = location->iz; }
if ( location->iz > indexbbox[5] ) { indexbbox[5] = location->iz; }
}
float sum = 0.0;
int count = 0;
for ( int k = indexbbox[4]; k <= indexbbox[5]; k++ )
{
for ( int j = indexbbox[2]; j <= indexbbox[3]; j++ )
{
for ( int i = indexbbox[0]; i <= indexbbox[1]; i++ )
{
float point[3];
field->lattice.coords()->pos( i, j, k, point );
int inside = FEelement->getIsoParam( vertices,
McVec3f(point[0],point[1], point[2]), pcoords );
if ( inside == 1 )
{
location->move( point );
float value;
field->eval( location, &value );
sum += value;
count++;
}
}
}
}
delete location;
return ( count > 0 ) ? ( sum/count ) : 0.0;
}
int BruteForceOptMatching::getEvidenceAssignment(
const ConnectedFactorGraph& graph, const int varLabel,
int& evidenceAssignmentIndexInGraph) {
int evidenceAssignmentIndexInWholeModel = -1;
McVec2i evidence(-1, -1);
for (int i = 0; i < mEvidence.size(); i++) {
if (mEvidence[i].x == varLabel) {
evidence = mEvidence[i];
}
}
// get assignment
if (evidence.x == varLabel) {
evidenceAssignmentIndexInWholeModel = evidence.y;
} else {
// if there was no assignment, we return -1
evidenceAssignmentIndexInGraph = -1;
return evidenceAssignmentIndexInWholeModel;
}
// The varaible was assigned, let's see to which one.
McDArray<int> assignementsForVariable;
getAssignmentsForVariable(varLabel, assignementsForVariable);
if (evidenceAssignmentIndexInWholeModel == -1) {
// if the assigned vaue is the dummy value,
// assign the dummy value, but return -1 anyway, because there is no
// label for the assignment
evidenceAssignmentIndexInGraph = assignementsForVariable.size();
return evidenceAssignmentIndexInWholeModel;
}
for (int i = 0; i < assignementsForVariable.size(); i++) {
if (assignementsForVariable[i] == evidenceAssignmentIndexInWholeModel) {
// assign the index of assignment in the current graph but return
// the index in the full model
evidenceAssignmentIndexInGraph = i;
return evidenceAssignmentIndexInWholeModel;
}
}
// not OK
evidenceAssignmentIndexInGraph = -2;
return evidenceAssignmentIndexInWholeModel;
}
void MicrotubuleTransformOperation::undo() {
SbMatrix invMat = mMat.inverse();
SpatialGraphSelection::Iterator iter(mSelection);
iter.vertices.reset();
int vNum = iter.vertices.nextSelected();
while (vNum != -1) {
McVec3f c = graph->getVertexCoords(vNum);
SbVec3f t(c.x, c.y, c.z);
SbVec3f res;
invMat.multVecMatrix(t, res);
graph->setVertexCoords(vNum, McVec3f(res[0], res[1], res[2]));
vNum = iter.vertices.nextSelected();
}
iter.edges.reset();
int eNum = iter.edges.nextSelected();
while (eNum != -1) {
McDArray<McVec3f> points = graph->getEdgePoints(eNum);
for (int p = 0; p < points.size(); p++) {
SbVec3f t(points[p].x, points[p].y, points[p].z);
SbVec3f res;
invMat.multVecMatrix(t, res);
points[p] = McVec3f(res[0], res[1], res[2]);
}
graph->setEdgePoints(eNum, points);
eNum = iter.edges.nextSelected();
}
int numPoints = mSelection.getNumSelectedPoints();
for (int i = 0; i < numPoints; ++i) {
SpatialGraphPoint p = mSelection.getSelectedPoint(i);
McDArray<McVec3f> points = graph->getEdgePoints(p.edgeNum);
SbVec3f t(points[p.pointNum].x, points[p.pointNum].y,
points[p.pointNum].z);
SbVec3f res;
invMat.multVecMatrix(t, res);
points[p.pointNum] = McVec3f(res[0], res[1], res[2]);
graph->setEdgePoints(p.edgeNum, points);
}
// update the transform parameters
for (int i = 0; i < mTransParams.size(); ++i) {
appendTransform(mTransParams[i], invMat);
}
}
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);
}
}
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));
}
}
// creates a pairwise factor for each adjacent variables in one connected
// component
// does not set the values yet
void BruteForceOptMatching::createConnectionFactors(
const McDMatrix<int>& adjMat, const McDArray<int> connectedComp,
vector<Factor>& pairFactorList) {
for (int i = 0; i < connectedComp.size(); i++) {
int curVar = connectedComp[i];
McDArray<int> assigmentsForIthVariable;
getAssignmentsForVariable(curVar, assigmentsForIthVariable);
Var pi(curVar, assigmentsForIthVariable.size() + 1);
for (int j = curVar + 1; j < adjMat.nCols(); j++) {
if (adjMat[curVar][j] == 1) {
McDArray<int> assigmentsForJthVariable;
getAssignmentsForVariable(j, assigmentsForJthVariable);
Var pj(j, assigmentsForJthVariable.size() + 1);
Factor facij(VarSet(pi, pj));
pairFactorList.push_back(facij);
}
}
}
}
void HxCPDSpatialGraphWarp::resamplePairs(McDArray<McVec3f>& p1,
McDArray<McVec3f>& p2) {
int numPairs = p1.size();
for (int i = p1.size() - 1; i > -1; i--) {
bool resetI = false;
for (int j = i - 1; j > -1; j--) {
// std::cout<<"\nCompare "<<i<<" and "<<j;
McVec3f set1Coord = p1[i];
McVec3f set2Coord = p1[j];
float dist = (set1Coord - set2Coord).length();
if (dist < portSampleDist.getValue()) {
std::cout << "\ndist between " << i << " and " << j << " is "
<< dist;
}
if (dist < portSampleDist.getValue()) {
p1.remove(j, 1);
p2.remove(j, 1);
resetI = true;
}
}
if (resetI) {
i = p1.size() - 1;
}
}
std::cout << "\n" << p1.size() << " point from " << numPairs
<< " left after resampling.";
}
void ma::cpdElastic(const ma::FacingPointSets& points,
ma::WarpResult& warpResult, const ma::CPDParams& params,
ma::Context* ctx) {
if (!ctx) {
ctx = &defaultContext();
}
CPDElasticAligner cpd;
cpd.setContext(ctx);
cpd.params = params.elastic;
if (params.elastic.useDirections) {
FacingPointSets copy = points;
for (int i = 0; i < copy.trans.directions.size(); i++) {
copy.trans.directions[i] *= -1;
}
cpd.setPoints(copy);
} else {
cpd.setPoints(points);
}
// Solve.
AlignInfo info;
McDArray<McVec3f> transCoords = cpd.align(info);
McDArray<McVec3f> origCoords = points.trans.positions;
const int nbefore = origCoords.size();
resamplePairs(origCoords, transCoords,
params.elastic.sampleDistForWarpingLandmarks);
ctx->print(QString("%1 of %2 points left after resampling.")
.arg(nbefore)
.arg(origCoords.size()));
warpResult.type = WT_ELASTIC;
warpResult.mlsParams.alpha = params.alphaForMLS;
warpResult.mlsParams.ps = asVec2dArray(origCoords);
warpResult.mlsParams.qs = asVec2dArray(transCoords);
warpResult.alignInfo = info;
}
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);
}
}
}
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::get3dShiftProbs(
const McVec3f pointCoord1, const McVec3f pointCoord2,
const McDArray<McVec3f>& assignmentCoordsVar1,
const McDArray<McVec3f>& assignmentCoordsVar2, McDMatrix<float>& probs) {
float probParam = 50.0;
float dummyDistance = mDistanceThresholdProjected / 2.0;
for (int i = 0; i < assignmentCoordsVar1.size(); i++) {
for (int j = 0; j < assignmentCoordsVar2.size(); j++) {
McVec3f dir1 = pointCoord1 - assignmentCoordsVar1[i];
McVec3f dir2 = pointCoord2 - assignmentCoordsVar2[j];
float dist1 = dir1.length();
float dist2 = dir2.length();
dir1.normalize();
dir2.normalize();
dir1 *= -1;
dir2 *= -1;
McVec3f p2Projected = dir1 * dist2 + pointCoord2;
McVec3f p1Projected = dir2 * dist1 + pointCoord1;
float distP1Projected =
(assignmentCoordsVar1[i] - p1Projected).length();
float distP2Projected =
(assignmentCoordsVar2[j] - p2Projected).length();
float maxDist = distP1Projected > distP2Projected ? distP1Projected
: distP2Projected;
probs[i][j] = exp(-maxDist / probParam);
}
}
for (int i = 0; i <= assignmentCoordsVar1.size(); i++) {
probs[i][assignmentCoordsVar2.size()] = exp(-dummyDistance / probParam);
}
for (int i = 0; i <= assignmentCoordsVar2.size(); i++) {
probs[(int)(assignmentCoordsVar1.size())][i] =
exp(-dummyDistance / probParam);
}
// normalize
float sumDists = 0.0;
for (int i = 0; i < probs.nCols() * probs.nRows(); i++)
sumDists += probs.dataPtr()[i];
for (int i = 0; i < probs.nCols() * probs.nRows(); i++)
probs.dataPtr()[i] /= sumDists;
}
// `resamplePairs()` removes points that are closer than `sampleDist` in `p1`.
// The corresponding points are also removed from `p2`. The two arrays are
// required to have the same size.
static void resamplePairs(McDArray<McVec3f>& p1, McDArray<McVec3f>& p2,
const float sampleDist) {
mcrequire(p1.size() == p2.size());
const int numPairs = p1.size();
for (int i = p1.size() - 1; i > -1; i--) {
bool resetI = false;
for (int j = i - 1; j > -1; j--) {
const McVec3f set1Coord = p1[i];
const McVec3f set2Coord = p1[j];
const float dist = (set1Coord - set2Coord).length();
if (dist < sampleDist) {
p1.remove(j, 1);
p2.remove(j, 1);
resetI = true;
}
}
if (resetI)
i = p1.size() - 1;
}
}
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";
}
}
}
int HxIteratePointMatchingUntilConvergence::getNumberOfPointsToAssign() {
McDArray<int> nodesToAssign;
getListOfNeededEvidenceNodes(nodesToAssign);
return nodesToAssign.size();
}
// 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 HxCPDSpatialGraphWarp::preparePointsAndDirectionsRigid(
McDArray<McVec3f>& p1, McDArray<McVec3f>& p2, McDArray<McVec3f>& d1,
McDArray<McVec3f>& d2, SpatialGraphSelection& slice2,
const HxSpatialGraph* spatialGraph) {
ma::SliceSelector selectionHelper(spatialGraph, "TransformInfo");
ma::EndPointParams params;
params.endPointRegion = 30;
params.projectionPlane = selectionHelper.computeMidPlane(0, 1);
params.projectionType = ma::P_ORTHOGONAL;
params.refSliceNum = 0;
params.transSliceNum = 1;
params.useAbsoluteValueForEndPointRegion = false;
params.maxDistForAngle = 2000;
params.angleToPlaneFilter = 0.01;
SpatialGraphSelection slice1;
ma::FacingPointSets pr =
ma::projectEndPoints(spatialGraph, slice1, slice2, params);
McDArray<McVec3f> refCoords = pr.ref.positions;
McDArray<McVec3f> transCoords = pr.trans.positions;
McDArray<McVec3f> refDirs = pr.ref.directions;
McDArray<McVec3f> transDirs = pr.trans.directions;
mcassert(refCoords.size() == slice1.getNumSelectedVertices());
mcassert(transCoords.size() == slice2.getNumSelectedVertices());
p1.resize(refCoords.size());
d1.resize(refCoords.size());
for (int i = 0; i < refCoords.size(); i++) {
McVec3f coord = refCoords[i];
p1[i] = McVec3f(coord.x, coord.y, 0);
d1[i] = refDirs[i] * 2000;
}
p2.resize(transCoords.size());
d2.resize(transCoords.size());
for (int i = 0; i < transCoords.size(); i++) {
McVec3f coord = transCoords[i];
p2[i] = McVec3f(coord.x, coord.y, 0);
d2[i] = transDirs[i] * -2000;
}
mcassert(p2.size() == slice2.getNumSelectedVertices());
}
