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