void HxCPDSpatialGraphWarp::preparePoints(McDArray<McVec3f>& p1,
McDArray<McVec3f>& p2,
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;
mcassert(refCoords.size() == slice1.getNumSelectedVertices());
mcassert(transCoords.size() == slice2.getNumSelectedVertices());
p1.resize(refCoords.size());
for (int i = 0; i < refCoords.size(); i++) {
McVec3f coord = refCoords[i];
p1[i] = McVec3f(coord.x, coord.y, 0);
}
p2.resize(transCoords.size());
for (int i = 0; i < transCoords.size(); i++) {
McVec3f coord = transCoords[i];
p2[i] = McVec3f(coord.x, coord.y, 0);
}
mcassert(p2.size() == slice2.getNumSelectedVertices());
}
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);
}
}
static McDArray<McVec2d> asVec2dArray(const McDArray<McVec3f>& a) {
McDArray<McVec2d> b;
b.resize(a.size());
for (long i = 0; i < a.size(); i++) {
b[i].x = a[i].x;
b[i].y = a[i].y;
}
return b;
}
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::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::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<double>& probs) {
McDArray<float> floatProbs;
getSingletonProbs(angleMatrix, projDistanceMatrix, distanceMatrix3d,
variableAssignmentMat, assignments, varLabel, floatProbs);
probs.resize(floatProbs.size());
for (int i = 0; i < floatProbs.size(); ++i)
probs[i] = floatProbs[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];
}
}
