Examples with KdTreePoint3D_F64 georegression.helper.KdTreePoint3D_F64 used on opensource projects

Example 1 with KdTreePoint3D_F64

use of georegression.helper.KdTreePoint3D_F64 in project BoofCV by lessthanoptimal.

the class PointCloudUtils_F64 method prune.

/**
 * Prunes points from the point cloud if they have very few neighbors
 *
 * @param cloud Point cloud
 * @param minNeighbors Minimum number of neighbors for it to not be pruned
 * @param radius search distance for neighbors
 */
public static void prune(List<Point3D_F64> cloud, int minNeighbors, double radius) {
    if (minNeighbors < 0)
        throw new IllegalArgumentException("minNeighbors must be >= 0");
    NearestNeighbor<Point3D_F64> nn = FactoryNearestNeighbor.kdtree(new KdTreePoint3D_F64());
    NearestNeighbor.Search<Point3D_F64> search = nn.createSearch();
    nn.setPoints(cloud, false);
    DogArray<NnData<Point3D_F64>> results = new DogArray<>(NnData::new);
    // It will always find itself
    minNeighbors += 1;
    // distance is Euclidean squared
    radius *= radius;
    for (int i = cloud.size() - 1; i >= 0; i--) {
        search.findNearest(cloud.get(i), radius, minNeighbors, results);
        if (results.size < minNeighbors) {
            cloud.remove(i);
        }
    }
}

Example 2 with KdTreePoint3D_F64

use of georegression.helper.KdTreePoint3D_F64 in project BoofCV by lessthanoptimal.

the class PointCloudUtils_F64 method prune.

/**
 * Prunes points from the point cloud if they have very few neighbors
 *
 * @param cloud Point cloud
 * @param colors Color of each point.
 * @param minNeighbors Minimum number of neighbors for it to not be pruned
 * @param radius search distance for neighbors
 */
public static void prune(List<Point3D_F64> cloud, DogArray_I32 colors, int minNeighbors, double radius) {
    if (minNeighbors < 0)
        throw new IllegalArgumentException("minNeighbors must be >= 0");
    NearestNeighbor<Point3D_F64> nn = FactoryNearestNeighbor.kdtree(new KdTreePoint3D_F64());
    NearestNeighbor.Search<Point3D_F64> search = nn.createSearch();
    nn.setPoints(cloud, false);
    DogArray<NnData<Point3D_F64>> results = new DogArray<>(NnData::new);
    // It will always find itself
    minNeighbors += 1;
    // distance is Euclidean squared
    radius *= radius;
    for (int i = cloud.size() - 1; i >= 0; i--) {
        search.findNearest(cloud.get(i), radius, minNeighbors, results);
        if (results.size < minNeighbors) {
            cloud.remove(i);
            colors.remove(i);
        }
    }
}

Example 3 with KdTreePoint3D_F64

use of georegression.helper.KdTreePoint3D_F64 in project BoofCV by lessthanoptimal.

the class PruneStructureFromSceneMetric method prunePoints.

/**
 * Prune a feature it has fewer than X neighbors within Y distance. Observations
 * associated with this feature are also pruned.
 *
 * Call {@link #pruneViews(int)} to makes sure the graph is valid.
 *
 * @param neighbors Number of other features which need to be near by
 * @param distance Maximum distance a point can be to be considered a feature
 */
public void prunePoints(int neighbors, double distance) {
    // Use a nearest neighbor search to find near by points
    Point3D_F64 worldX = new Point3D_F64();
    List<Point3D_F64> cloud = new ArrayList<>();
    for (int i = 0; i < structure.points.size; i++) {
        SceneStructureCommon.Point structureP = structure.points.data[i];
        structureP.get(worldX);
        cloud.add(worldX.copy());
    }
    NearestNeighbor<Point3D_F64> nn = FactoryNearestNeighbor.kdtree(new KdTreePoint3D_F64());
    NearestNeighbor.Search<Point3D_F64> search = nn.createSearch();
    nn.setPoints(cloud, false);
    DogArray<NnData<Point3D_F64>> resultsNN = new DogArray<>(NnData::new);
    // Create a look up table containing from old to new indexes for each point
    int[] oldToNew = new int[structure.points.size];
    // crash is bug
    Arrays.fill(oldToNew, -1);
    // List of point ID's which are to be removed.
    DogArray_I32 prunePointID = new DogArray_I32();
    // identify points which need to be pruned
    for (int pointId = 0; pointId < structure.points.size; pointId++) {
        SceneStructureCommon.Point structureP = structure.points.data[pointId];
        structureP.get(worldX);
        // distance is squared
        search.findNearest(cloud.get(pointId), distance * distance, neighbors + 1, resultsNN);
        // Don't prune if it has enough neighbors. Remember that it will always find itself.
        if (resultsNN.size() > neighbors) {
            oldToNew[pointId] = pointId - prunePointID.size;
            continue;
        }
        prunePointID.add(pointId);
        // Remove observations of this point
        for (int viewIdx = 0; viewIdx < structureP.views.size; viewIdx++) {
            SceneObservations.View v = observations.getView(structureP.views.data[viewIdx]);
            int pointIdx = v.point.indexOf(pointId);
            if (pointIdx < 0)
                throw new RuntimeException("Bad structure. Point not found in view's observation " + "which was in its structure");
            v.remove(pointIdx);
        }
    }
    pruneUpdatePointID(oldToNew, prunePointID);
}