Examples with Point4D_F64 georegression.struct.point.Point4D_F64 used on opensource projects

Example 1 with Point4D_F64

use of georegression.struct.point.Point4D_F64 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 Point4D_F64

use of georegression.struct.point.Point4D_F64 in project BoofCV by lessthanoptimal.

the class TestRefineMetricWorkingGraph method computeReprojectionError.

/**
 * Render a known object and see if expected reprojection error is returned.
 */
@Test
void computeReprojectionError() {
    var X = new Point4D_F64(0.01, 0.02, 0.2, 1.0);
    var intrinsic = new CameraPinhole(400, 410, 0, 420, 420, 800, 800);
    var world_to_vew = SpecialEuclideanOps_F64.eulerXyz(0.1, -0.2, 2, 0, 0.05, -0.05, null);
    var normToPixels = new LensDistortionPinhole(intrinsic).distort_F64(false, true);
    var observed = PerspectiveOps.renderPixel(world_to_vew, intrinsic, X, null);
    Objects.ensureNotNull(observed);
    // homogenous coordinates are scale invariant
    X.scale(-5.0);
    var alg = new RefineMetricWorkingGraph();
    assertEquals(0.0, alg.computeReprojectionError(world_to_vew, normToPixels, observed, X), UtilEjml.TEST_F64);
    observed.x += 2.0;
    assertEquals(4.0, alg.computeReprojectionError(world_to_vew, normToPixels, observed, X), UtilEjml.TEST_F64);
}

Example 3 with Point4D_F64

use of georegression.struct.point.Point4D_F64 in project BoofCV by lessthanoptimal.

the class TestRefineMetricWorkingGraph method assignKnown3DToUnassignedObs.

void assignKnown3DToUnassignedObs(boolean shouldReject) {
    var dbSimilar = new MockLookupSimilarImagesRealistic().pathLine(5, 0.3, 1.5, 2);
    var pairwise = dbSimilar.createPairwise();
    var graph = dbSimilar.createWorkingGraph(pairwise);
    var alg = new RefineMetricWorkingGraph() {

        // Override so that it can return an error for which all should be accepted or rejected
        @Override
        double computeReprojectionError(Se3_F64 world_to_view, Point2Transform2_F64 normToPixels, Point2D_F64 pixelObs, Point4D_F64 world3D) {
            // this is also a sanity check on the used error being squared
            double error = maxReprojectionErrorPixel * maxReprojectionErrorPixel;
            return error + (shouldReject ? 0.001 : -0.001);
        }
    };
    alg.maxReprojectionErrorPixel = 10;
    alg.initializeDataStructures(dbSimilar, graph);
    alg.metricSba.structure.points.resize(20);
    // create an inlier set composed of observations from 3 views
    var inliers = new SceneWorkingGraph.InlierInfo();
    for (int viewIdx : new int[] { 1, 2, 3 }) {
        inliers.views.add(pairwise.nodes.get(viewIdx));
        inliers.observations.grow().setTo(DogArray_I32.array(1, 2, 3, 5, 6));
    }
    // Specific which of the observations in the inlier set are currently unassigned
    var unassignedOrig = DogArray_I32.array(0, 2);
    alg.unassigned.setTo(unassignedOrig);
    // There is only one 3D feature they can be matched with
    alg.featureIdx3D.add(3);
    // this is the inlier set that's going to be inspected
    int inlierFeatIdx = 4;
    alg.initLookUpTablesForInlierSet(graph, inliers.views);
    alg.assignKnown3DToUnassignedObs(graph, inliers, inlierFeatIdx);
    // it keeps tracks of what was actually assigned by removing it
    assertEquals(shouldReject ? unassignedOrig.size : 0, alg.unassigned.size);
    BoofMiscOps.forIdx(graph.listViews, (i, v) -> {
        // See if the observation were added to the inliers and nothing changed for others
        boolean isInlier = inliers.views.contains(v.pview);
        int inlierViewIdx = inliers.views.indexOf(v.pview);
        boolean shouldBeAssigned = !shouldReject && isInlier && unassignedOrig.contains(inlierViewIdx);
        if (shouldBeAssigned) {
            // Make sure the point in the inlier set which is being inspected was assigned a value
            int pointID = inliers.observations.get(inlierViewIdx).get(inlierFeatIdx);
            DogArray_I32 point = alg.metricSba.observations.views.get(i).point;
            assertEquals(3, point.get(pointID));
            // Set it to -1 make the next test easier since everything should now be -1
            point.set(pointID, -1);
        }
        alg.metricSba.observations.views.get(i).point.forIdx((j, value) -> assertEquals(-1, value));
    });
}

Example 4 with Point4D_F64

use of georegression.struct.point.Point4D_F64 in project BoofCV by lessthanoptimal.

the class TestRefineMetricWorkingGraph method triangulateAndSave.

@Test
void triangulateAndSave() {
    var dbSimilar = new MockLookupSimilarImagesRealistic().pathLine(5, 0.3, 1.5, 2);
    var pairwise = dbSimilar.createPairwise();
    var graph = dbSimilar.createWorkingGraph(pairwise);
    var alg = new RefineMetricWorkingGraph();
    alg.initializeDataStructures(dbSimilar, graph);
    // create an inlier set composed of observations from 3 views
    var inliers = new SceneWorkingGraph.InlierInfo();
    for (int viewIdx : new int[] { 1, 2, 3 }) {
        inliers.views.add(pairwise.nodes.get(viewIdx));
        inliers.observations.grow().setTo(DogArray_I32.array(1, 2, 3, 5, 6));
    }
    alg.sceneViewIntIds.setTo(DogArray_I32.array(1, 2, 3));
    alg.unassigned.setTo(DogArray_I32.array(1, 2));
    alg.initLookUpTablesForInlierSet(graph, inliers.views);
    alg.triangulateAndSave(inliers, 4);
    // see if it added the point to the structure correctly
    Point3D_F64 expectedX = dbSimilar.points.get(5).world;
    Point4D_F64 foundX = new Point4D_F64();
    assertEquals(1, alg.metricSba.structure.points.size);
    alg.metricSba.structure.points.get(0).get(foundX);
    foundX.divideIP(foundX.w);
    assertEquals(0.0, expectedX.distance(expectedX.x, expectedX.y, expectedX.z), 1e-6);
    // observations should now point to this new 3D feature
    BoofMiscOps.forIdx(inliers.views.toList(), (i, v) -> {
        int expected = alg.unassigned.contains(i) ? 0 : -1;
        int viewID = alg.sceneViewIntIds.get(i);
        assertEquals(expected, alg.metricSba.observations.getView(viewID).point.get(6));
    });
}

Example 5 with Point4D_F64

use of georegression.struct.point.Point4D_F64 in project BoofCV by lessthanoptimal.

the class TestProjectiveInitializeAllCommon method checkReconstruction.

/**
 * Check reconstruction by seeing if it's consistent with the input observations
 */
private void checkReconstruction(ProjectiveInitializeAllCommon alg, MockLookupSimilarImages db, DogArray_I32 seedConnIdx, int numFeatures, double reprojectionTol) {
    final SceneStructureProjective structure = alg.getStructure();
    // Sanity check the number of each type of structure
    assertEquals(seedConnIdx.size + 1, structure.views.size);
    assertEquals(numFeatures, structure.points.size);
    alg.inlierIndexes.forEach(list -> assertEquals(numFeatures, list.size));
    int dbIndexSeed = db.viewIds.indexOf(alg.getPairwiseGraphViewByStructureIndex(0).id);
    // Check results for consistency. Can't do a direct comparison to ground truth since a different
    // but equivalent projective frame would have been estimated.
    Point4D_F64 X = new Point4D_F64();
    Point2D_F64 found = new Point2D_F64();
    for (int i = 0; i < numFeatures; i++) {
        int seedFeatureIdx = alg.inlierIndexes.get(0).get(i);
        int truthFeatIdx = db.viewToFeat.get(dbIndexSeed)[seedFeatureIdx];
        int structureIdx = alg.seedToStructure.get(seedFeatureIdx);
        // only features that have structure should be in this list
        assertTrue(structureIdx >= 0);
        // Get the estimated point in 3D
        structure.points.get(structureIdx).get(X);
        // Project the point to the camera using found projection matrices
        for (int viewIdx = 0; viewIdx < structure.views.size; viewIdx++) {
            int viewDbIdx = db.viewIds.indexOf(alg.getPairwiseGraphViewByStructureIndex(viewIdx).id);
            // Project this feature to the camera
            DMatrixRMaj P = structure.views.get(viewIdx).worldToView;
            PerspectiveOps.renderPixel(P, X, found);
            // undo the offset
            found.x += db.intrinsic.cx;
            found.y += db.intrinsic.cy;
            // Lookup the expected pixel location
            // The seed feature ID and the ground truth feature ID are the same
            int viewFeatIdx = db.featToView.get(viewDbIdx)[truthFeatIdx];
            Point2D_F64 expected = db.viewObs.get(viewDbIdx).get(viewFeatIdx);
            assertEquals(0.0, expected.distance(found), reprojectionTol);
        }
    }
}