Examples with SceneObservations boofcv.abst.geo.bundle.SceneObservations used on opensource projects

Example 1 with SceneObservations

use of boofcv.abst.geo.bundle.SceneObservations in project BoofCV by lessthanoptimal.

the class EstimateViewUtils method configureSbaStructure.

/**
 * Configures data structures for running SBA. Which observations are used is specified by the provided inliers.
 * By default all cameras and views are set to known. If these need to be optimized for a specific use case then
 * 'known' should be set to false.
 *
 * @param inliersThreeView Specifies the observations
 */
public void configureSbaStructure(List<AssociatedTriple> inliersThreeView) {
    final SceneStructureMetric structure = metricSba.structure;
    final SceneObservations observations = metricSba.observations;
    // Even if the cameras are all the same, we will tell that they are different just because the bookkeeping
    // is so much easier and results are the same
    structure.initialize(3, 3, usedThreeViewInliers.size);
    observations.initialize(3);
    // All cameras are known
    structure.setCamera(0, true, camera1);
    structure.setCamera(1, true, camera2);
    structure.setCamera(2, true, camera3);
    // All transforms are known but the target
    structure.setView(0, 0, true, view1_to_view1);
    structure.setView(1, 1, true, view1_to_view2);
    structure.setView(2, 2, true, view1_to_target);
    observations.getView(0).resize(usedThreeViewInliers.size());
    observations.getView(1).resize(usedThreeViewInliers.size());
    observations.getView(2).resize(usedThreeViewInliers.size());
    SceneObservations.View viewObs1 = observations.getView(0);
    SceneObservations.View viewObs2 = observations.getView(1);
    SceneObservations.View viewObs3 = observations.getView(2);
    final TriangulateNViewsMetricH triangulator = metricSba.triangulator;
    var foundX = new Point4D_F64();
    // Only use features that were in the inlier set for PnP
    for (int inlierCnt = 0; inlierCnt < usedThreeViewInliers.size(); inlierCnt++) {
        int threeViewInlierIndex = usedThreeViewInliers.get(inlierCnt);
        AssociatedTriple a = inliersThreeView.get(threeViewInlierIndex);
        // Pass in pixel observations for each view
        viewObs1.set(inlierCnt, inlierCnt, (float) a.p1.x, (float) a.p1.y);
        viewObs2.set(inlierCnt, inlierCnt, (float) a.p2.x, (float) a.p2.y);
        viewObs3.set(inlierCnt, inlierCnt, (float) a.p3.x, (float) a.p3.y);
        normalize1.compute(a.p1.x, a.p1.y, pixelNorms.get(0));
        normalize2.compute(a.p2.x, a.p2.y, pixelNorms.get(1));
        normalize3.compute(a.p3.x, a.p3.y, pixelNorms.get(2));
        if (!triangulator.triangulate(pixelNorms, listMotion, foundX)) {
            throw new RuntimeException("Triangulation failed. Possibly bad input. Handle this problem");
        }
        if (structure.isHomogenous())
            structure.setPoint(inlierCnt, foundX.x, foundX.y, foundX.z, foundX.w);
        else
            structure.setPoint(inlierCnt, foundX.x / foundX.w, foundX.y / foundX.w, foundX.z / foundX.w);
        structure.connectPointToView(inlierCnt, 0);
        structure.connectPointToView(inlierCnt, 1);
        structure.connectPointToView(inlierCnt, 2);
    }
}

Example 2 with SceneObservations

use of boofcv.abst.geo.bundle.SceneObservations in project BoofCV by lessthanoptimal.

the class RefineMetricWorkingGraph method initializeDataStructures.

/**
 * Initialized several data structures and resets it into the initial state
 */
void initializeDataStructures(LookUpSimilarImages dbSimilar, SceneWorkingGraph graph) {
    viewToIntegerID.clear();
    listPixelToNorm.clear();
    listNormToPixel.clear();
    final SceneStructureMetric structure = metricSba.structure;
    final SceneObservations observations = metricSba.observations;
    // Initialize the structure, but save initializing the points for later
    structure.initialize(graph.listCameras.size, graph.listViews.size(), 0);
    // Go through each view and load the observations then add them to the scene, but don't specify which
    // 3D point they are observing yet
    observations.initialize(graph.listViews.size());
    // First add cameras to the structure
    for (int cameraIdx = 0; cameraIdx < graph.listCameras.size; cameraIdx++) {
        SceneWorkingGraph.Camera wcam = graph.listCameras.get(cameraIdx);
        structure.setCamera(cameraIdx, false, wcam.intrinsic);
    }
    // add views next
    for (int viewIdx = 0; viewIdx < graph.listViews.size(); viewIdx++) {
        SceneWorkingGraph.View wview = graph.listViews.get(viewIdx);
        SceneObservations.View oview = observations.getView(viewIdx);
        viewToIntegerID.put(wview.pview.id, viewIdx);
        createProjectionModel(graph.getViewCamera(wview).intrinsic);
        // Add all observations in this view to the SBA observations.
        // Observations that are not assigned to a 3D point will be pruned later on. Much easier this way.
        oview.resize(wview.pview.totalObservations);
        dbSimilar.lookupPixelFeats(wview.pview.id, pixels);
        BoofMiscOps.checkEq(pixels.size, wview.pview.totalObservations);
        // The camera model assumes the principle point is (0,0) and this is done by assuming it's the image center
        SceneWorkingGraph.Camera camera = graph.getViewCamera(wview);
        float cx = (float) camera.prior.cx;
        float cy = (float) camera.prior.cy;
        // specify the observation pixel coordinates but not which 3D feature is matched to the observation
        for (int obsIdx = 0; obsIdx < pixels.size; obsIdx++) {
            Point2D_F64 p = pixels.get(obsIdx);
            oview.setPixel(obsIdx, (float) (p.x - cx), (float) (p.y - cy));
        }
        // Add this view to the graph and it's location
        structure.setView(viewIdx, camera.localIndex, viewIdx == 0, wview.world_to_view);
    }
}

Example 3 with SceneObservations

use of boofcv.abst.geo.bundle.SceneObservations in project BoofCV by lessthanoptimal.

the class RefineMetricWorkingGraph method assignKnown3DToUnassignedObs.

/**
 * Assigns world 3D features that were already matched to others in the inlier view set to the
 * unassigned observations.
 *
 * Part of the idea behind only associating an observation with a 3D feature if the preprojection error is less
 * than some value is that tracks can drift from one object to another, but it's useful to save both.
 *
 * @param inlierIdx Index of the observations. All observations with this index point to the same 3D feature
 */
void assignKnown3DToUnassignedObs(SceneWorkingGraph graph, SceneWorkingGraph.InlierInfo inliers, int inlierIdx) {
    final SceneStructureMetric structure = metricSba.structure;
    final SceneObservations observations = metricSba.observations;
    // Go through all the views/observations which have yet to be assigned a 3D feature
    for (int unassignedIdx = unassigned.size - 1; unassignedIdx >= 0; unassignedIdx--) {
        int whichViewInliers = unassigned.get(unassignedIdx);
        int whichViewID = sceneViewIntIds.get(whichViewInliers);
        // Lookup the pixel observation in the view
        int viewObsIdx = inliers.observations.get(whichViewInliers).get(inlierIdx);
        observations.getView(whichViewID).getPixel(viewObsIdx, pixelObserved);
        // look up scene information for this view
        SceneWorkingGraph.View wview = graph.listViews.get(whichViewID);
        Point2Transform2_F64 normToPixels = listNormToPixel.get(whichViewID);
        // See which 3D feature best matches this observation
        double bestScore = maxReprojectionErrorPixel * maxReprojectionErrorPixel;
        int bestId = -1;
        for (int knownIdx = 0; knownIdx < featureIdx3D.size; knownIdx++) {
            int featureId = featureIdx3D.get(knownIdx);
            // If this feature has already been assigned to this view skip over it
            if (structure.points.get(featureId).views.contains(whichViewID))
                continue;
            structure.getPoints().get(featureId).get(world3D);
            double error = computeReprojectionError(wview.world_to_view, normToPixels, pixelObserved, world3D);
            if (error <= bestScore) {
                bestScore = error;
                bestId = featureId;
            }
        }
        if (bestId == -1) {
            if (verbose != null)
                verbose.println("Not matching. Reprojection error too large view=" + whichViewID);
            continue;
        }
        // assign this scene feature to this observation
        observations.getView(whichViewID).safeAssignToFeature(viewObsIdx, bestId);
        structure.connectPointToView(bestId, whichViewID);
        // Remove it since it has been assigned. This is also why we iterate in reverse
        unassigned.removeSwap(unassignedIdx);
    }
}

Example 4 with SceneObservations

use of boofcv.abst.geo.bundle.SceneObservations in project BoofCV by lessthanoptimal.

the class RefineMetricWorkingGraph method pruneUnassignedObservations.

/**
 * Prunes unassigned observations from each view. This is done by swapping which is fast but does change the order
 */
void pruneUnassignedObservations() {
    final SceneObservations observations = metricSba.observations;
    for (int i = 0; i < observations.views.size; i++) {
        SceneObservations.View v = observations.views.get(i);
        for (int j = v.point.size() - 1; j >= 0; j--) {
            if (v.point.get(j) == -1) {
                // Remove it by swapping the last element. This is O(1) as compared to O(N), but changes the order
                v.point.data[j] = v.point.removeTail();
                v.observations.data[j * 2] = v.observations.getTail(1);
                v.observations.data[j * 2 + 1] = v.observations.getTail(0);
                v.observations.size -= 2;
            }
        }
    }
}

Example 5 with SceneObservations

use of boofcv.abst.geo.bundle.SceneObservations in project BoofCV by lessthanoptimal.

the class MetricSanityChecks method checkPhysicalConstraints.

public boolean checkPhysicalConstraints(SceneStructureMetric structure, SceneObservations observations, List<CameraPinholeBrown> listPriors) {
    BoofMiscOps.checkEq(listPriors.size(), structure.views.size);
    for (int i = 0; i < structure.cameras.size; i++) {
        BundlePinholeSimplified pinhole = (BundlePinholeSimplified) structure.cameras.get(i).model;
        if (pinhole.f < 0.0f) {
            if (verbose != null)
                verbose.println("Bad focal length. f=" + pinhole.f);
            return false;
        }
    }
    badFeatures.resetResize(structure.points.size, false);
    var worldP = new Point4D_F64(0, 0, 0, 1);
    var viewP = new Point4D_F64();
    var observedPixel = new Point2D_F64();
    var predictdPixel = new Point2D_F64();
    for (int viewIdx = 0; viewIdx < observations.views.size; viewIdx++) {
        int cameraIdx = structure.views.get(viewIdx).camera;
        BundlePinholeSimplified pinhole = (BundlePinholeSimplified) Objects.requireNonNull(structure.cameras.get(cameraIdx).model);
        CameraPinholeBrown priorCamera = listPriors.get(viewIdx);
        int width = priorCamera.width;
        int height = priorCamera.height;
        // Used to compensates for the lens model having its origin at the image center
        float cx = (float) priorCamera.cx;
        float cy = (float) priorCamera.cy;
        // Number of times each test failed in this particular view
        int failedBehind = 0;
        int failedImageBounds = 0;
        int failedReprojection = 0;
        Se3_F64 world_to_view = structure.getParentToView(viewIdx);
        SceneObservations.View oview = observations.views.get(viewIdx);
        for (int i = 0; i < oview.size(); i++) {
            // If true then this feature failed one of the constraints test in tis value
            boolean badObservation = false;
            oview.getPixel(i, observedPixel);
            SceneStructureCommon.Point p = structure.points.get(oview.getPointId(i));
            worldP.x = p.getX();
            worldP.y = p.getY();
            worldP.z = p.getZ();
            if (structure.isHomogenous()) {
                worldP.w = p.getW();
            }
            // worldP.w = 1 was already set for 3D points
            SePointOps_F64.transform(world_to_view, worldP, viewP);
            if (PerspectiveOps.isBehindCamera(viewP)) {
                badObservation = true;
                failedBehind++;
            }
            pinhole.project(viewP.x, viewP.y, viewP.z, predictdPixel);
            double reprojectionError = predictdPixel.distance2(observedPixel);
            if (reprojectionError > maxReprojectionErrorSq) {
                badObservation = true;
                failedReprojection++;
            }
            if (!BoofMiscOps.isInside(width, height, predictdPixel.x + cx, predictdPixel.y + cy)) {
                badObservation = true;
                failedImageBounds++;
            }
            if (badObservation) {
                badFeatures.set(oview.getPointId(i), true);
            }
        }
        if (verbose != null)
            verbose.printf("view[%d] errors: behind=%d bounds=%d reprojection=%d, obs=%d\n", viewIdx, failedBehind, failedImageBounds, failedReprojection, oview.size());
    }
    return true;
}