ompl::geometric::BasicPRMmodif::Milestone* ompl::geometric::BasicPRMmodif::addMilestone(base::State *state) { Milestone *m = new Milestone(); m->state = state; m->component = componentCount_; componentSizes_[m->component] = 1; // connect to nearest neighbors std::vector<Milestone*> nbh; nearestNeighbors(m, nbh); for (unsigned int i = 0 ; i < nbh.size() ; ++i) if (si_->checkMotion(m->state, nbh[i]->state)) { m->adjacent.push_back(nbh[i]); nbh[i]->adjacent.push_back(m); m->costs.push_back(si_->distance(m->state, nbh[i]->state)); // std::cout << si_->distance(m->state, nbh[i]->state) << std::endl; nbh[i]->costs.push_back(m->costs.back()); uniteComponents(m, nbh[i]); } // if the new milestone was no absorbed in an existing component, // increase the number of components if (m->component == componentCount_) componentCount_++; m->index = milestones_.size(); milestones_.push_back(m); nn_->add(m); return m; }
/** * Given a kd-tree and a point p, the function stores the nearest neighbors of p to bpq * * @param curr - the kd-tree containing the points * @param bpq - the bounded priority queue to store the nearest neighbors in * @param p - the point to find the nearest neighbors to * * does nothing if curr == NULL or bpq == NULL or p == NULL */ void nearestNeighbors(KDTreeNode curr, SPBPQueue bpq, SPPoint p) { SPListElement node; SPPoint q; bool isLeft; double coorDis; if (curr == NULL || bpq == NULL || p == NULL ) return; q = curr->data; /* Add the current point to the BPQ. Note that this is a no-op if the * point is not as good as the points we've seen so far.*/ if (curr->dim == -1) { int index; double dis; index = spPointGetIndex(q); dis = spPointL2SquaredDistance(p, curr->data); node = spListElementCreate(index, dis); spBPQueueEnqueue(bpq, node); spListElementDestroy(node); return; } /* Recursively search the half of the tree that contains the test point. */ if (spPointGetAxisCoor(p, curr->dim) <= curr->val) { nearestNeighbors(curr->left, bpq, p); isLeft = true; } else { nearestNeighbors(curr->right, bpq, p); isLeft = false; } /* If the candidate hypersphere crosses this splitting plane, look on the * other side of the plane by examining the other subtree*/ coorDis = abs(spPointGetAxisCoor(p, curr->dim) - curr->val); if (!spBPQueueIsFull(bpq) || coorDis*coorDis < spBPQueueMaxValue(bpq)) { if (isLeft) nearestNeighbors(curr->right, bpq, p); else nearestNeighbors(curr->left, bpq, p); } }
int main(int argc, char *argv[]) { Pooma::initialize(argc, argv); Pooma::Tester tester(argc, argv); const double eps = 1.0e-08; // checking tolerance const int Dim = 2; std::vector<FieldOffsetList<Dim> > nn; std::vector<FieldOffsetList<3> > nn3; Centering<2> inputCenteringTwo, outputCenteringTwo; Centering<3> inputCenteringThree, outputCenteringThree; Interval<Dim> physicalVertexDomain(4, 4); DomainLayout<Dim> layout(physicalVertexDomain, GuardLayers<Dim>(1)); typedef Field<UniformRectilinearMesh<Dim>, double, Brick> Field_t; // Test 2D Discontinuous Vertex -> Continuous Vertex. inputCenteringTwo = canonicalCentering<2>(VertexType, Discontinuous, AllDim); outputCenteringTwo = canonicalCentering<2>(VertexType, Continuous, AllDim); nn = nearestNeighbors(inputCenteringTwo, outputCenteringTwo); Field_t g(inputCenteringTwo, layout, Vector<Dim>(0.0), Vector<Dim>(1.0, 2.0)); g.all() = 2.0; g = -1.0; Pooma::blockAndEvaluate(); g(FieldOffset<Dim>(Loc<Dim>(1,1), 0), Loc<Dim>(0,0)) = 17.0; tester.check("discontinuous vertex->continuous vertex", checkFieldPosition(g, nn[0], Loc<Dim>(1), 14.0, 3.5, -1.0, 17.0, eps)); // Test 2D Continuous Cell -> Continuous Cell. inputCenteringTwo = canonicalCentering<2>(CellType, Continuous, AllDim); outputCenteringTwo = canonicalCentering<2>(CellType, Continuous, AllDim); nn = nearestNeighbors(inputCenteringTwo, outputCenteringTwo); Field_t f(inputCenteringTwo, layout, Vector<Dim>(0.0), Vector<Dim>(1.0, 2.0)); f.all() = 2.0; f = -1.0; Pooma::blockAndEvaluate(); f(FieldOffset<Dim>(Loc<Dim>(1,1), 0), Loc<Dim>(0,0)) = 17.0; tester.check("cell->cell", checkFieldPosition(f, nn[0], Loc<Dim>(1,1), 17.0, 17.0, 17.0, 17.0, eps)); // Test 2D Discontinuous Face -> Continuous Edge. inputCenteringTwo = canonicalCentering<2>(FaceType, Discontinuous, AllDim); outputCenteringTwo = canonicalCentering<2>(EdgeType, Continuous, AllDim); nn = nearestNeighbors(inputCenteringTwo, outputCenteringTwo); Field_t h(inputCenteringTwo, layout, Vector<Dim>(0.0), Vector<Dim>(1.0, 2.0)); h.all() = 2.0; h = -1.0; Pooma::blockAndEvaluate(); h(FieldOffset<Dim>(Loc<Dim>(1), 0), Loc<Dim>(0)) = 17.0; tester.check("discontinuous face->edge", checkFieldPosition(h, nn[0], Loc<Dim>(1), -2.0, -1.0, -1.0, -1.0, eps)); // Test 3D Discontinuous Vertex -> Continuous Cell. inputCenteringThree = canonicalCentering<3>(VertexType, Discontinuous, AllDim); outputCenteringThree = canonicalCentering<3>(CellType, Continuous, AllDim); nn3 = nearestNeighbors(inputCenteringThree, outputCenteringThree); Interval<3> physicalVertexDomain3(4, 4, 4); DomainLayout<3> layout3(physicalVertexDomain3, GuardLayers<3>(1)); Field<UniformRectilinearMesh<3>, double, Brick> G(inputCenteringThree, layout3, Vector<3>(0.0), Vector<3>(1.0, 2.0, 0.0)); G.all() = 2.0; G = -1.0; Pooma::blockAndEvaluate(); G(FieldOffset<3>(Loc<3>(1), 0), Loc<3>(0)) = 17.0; tester.check("discontinuous vertex->cell", checkFieldPosition(G, nn3[0], Loc<3>(1), -46.0, -46.0/64.0, -1.0, 17.0, eps)); int ret = tester.results("FieldReductions"); Pooma::finalize(); return ret; }
void locallyLinearEmbeddings(const Mat_<float> &samples, int outDim, Mat_<float> &embeddings, int k) { assert(outDim < samples.cols); assert(k >= 1); Mat_<int> nearestNeighbors(samples.rows, k); // determining k nearest neighbors for each sample flann::Index flannIndex(samples, flann::LinearIndexParams()); for (int i = 0; i < samples.rows; i++) { Mat_<int> nearest; Mat dists; flannIndex.knnSearch(samples.row(i), nearest, dists, k + 1); nearest.colRange(1, nearest.cols).copyTo(nearestNeighbors.row(i)); } // determining weights for each sample vector<Triplet<double> > tripletList; tripletList.reserve(samples.rows * k); for (int i = 0; i < samples.rows; i++) { Mat_<double> C(k,k); for (int u = 0; u < k; u++) { for (int v = u; v < k; v++) { C(u,v) = (samples.row(i) - samples.row(nearestNeighbors(i,u))).dot(samples.row(i) - samples.row(nearestNeighbors(i,v))); C(v,u) = C(u,v); } } // regularize when the number of neighbors is greater than the input // dimension if (k > samples.cols) { C = C + Mat_<double>::eye(k,k) * 10E-3 * trace(C)[0]; } Map<MatrixXd,RowMajor> eigC((double*)C.data, C.rows, C.cols); LDLT<MatrixXd> solver(eigC); Mat_<double> weights(k,1); Map<MatrixXd,RowMajor> eigWeights((double*)weights.data, weights.rows, weights.cols); eigWeights = solver.solve(MatrixXd::Ones(k,1)); Mat_<double> normWeights; double weightsSum = sum(weights)[0]; if (weightsSum == 0) { cout<<"error: cannot reconstruct point "<<i<<" from its neighbors"<<endl; exit(EXIT_FAILURE); } normWeights = weights / weightsSum; for (int j = 0; j < k; j++) { tripletList.push_back(Triplet<double>(nearestNeighbors(i,j), i, normWeights(j))); } } SparseMatrix<double> W(samples.rows, samples.rows); W.setFromTriplets(tripletList.begin(), tripletList.end()); // constructing vectors in lower dimensional space from the weights VectorXd eigenvalues; MatrixXd eigenvectors; LLEMult mult(&W); symmetricSparseEigenSolver(samples.rows, "SM", outDim + 1, samples.rows, eigenvalues, eigenvectors, mult); embeddings = Mat_<double>(samples.rows, outDim); if (DEBUG_LLE) { cout<<"actual eigenvalues : "<<eigenvalues<<endl; cout<<"actual : "<<endl<<eigenvectors<<endl; MatrixXd denseW(W); MatrixXd tmp = MatrixXd::Identity(W.rows(), W.cols()) - denseW; MatrixXd M = tmp.transpose() * tmp; SelfAdjointEigenSolver<MatrixXd> eigenSolver(M); MatrixXd expectedEigenvectors = eigenSolver.eigenvectors(); cout<<"expected eigenvalues : "<<eigenSolver.eigenvalues()<<endl; cout<<"expected : "<<endl<<expectedEigenvectors<<endl; } for (int i = 0; i < samples.rows; i++) { for (int j = 0; j < outDim; j++) { embeddings(i,j) = eigenvectors(i, j + 1); } } }
void TileGrid::recursivePathfind(TilePos point, int maxCost, PathMap& pmap, DistanceMap& dmap, bool diagonal) { // Get a list of the nearest neighbours and cache the current fastest // path to this point TilePosList nneighbor = nearestNeighbors(point); auto partialPath = pmap[point]; auto partialCost = dmap[point]; // Repeat the process for each of the nearest-neighbours of this point for (auto const &p : nneighbor) { // Is the candidate point diagonal? (diagonal paths take longer to walk) bool diagCopy = diagonal; int pathCost = 1; if (!point.isInLine(p)) { if (diagCopy) { pathCost++; } diagCopy = !diagCopy; } int adjustedCost = partialCost + pathCost; if (adjustedCost > maxCost) { // We can't get here taking this route - stop. continue; } // Do we already have this point in our map? auto it = dmap.find(p); if (it == dmap.end()) { // This point is not in our map - this is the fastest route here automatically pmap[p] = partialPath; pmap[p].push_back(p); dmap[p] = adjustedCost; } else { // This point is already in our map - is it faster? if (adjustedCost > it->second) { continue; } else { // Replace pmap[p] = partialPath; pmap[p].push_back(p); dmap[p] = adjustedCost; } } // RECURSE recursivePathfind(p, maxCost, pmap, dmap, diagCopy); } }