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."; }
// `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; } }
// 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; }