Examples with AssociatedTupleDN boofcv.struct.geo.AssociatedTupleDN used on opensource projects

Example 1 with AssociatedTupleDN

use of boofcv.struct.geo.AssociatedTupleDN in project BoofCV by lessthanoptimal.

the class TestProjectiveInitializeAllCommon method lookupInfoForMetricElevation.

/**
 * Looks up features then tests using triangulation and checks for perfect results.
 */
@Test
void lookupInfoForMetricElevation() {
    int numViews = 4;
    var dbSimilar = new MockLookupSimilarImages(numViews, 0xDEADBEEF);
    var dbCams = new MockLookUpCameraInfo(800, 800);
    var alg = new ProjectiveInitializeAllCommon();
    alg.utils.dbSimilar = dbSimilar;
    alg.utils.dbCams = dbCams;
    // sanity check that all features are visible in all views. Requirement of metric escalation
    for (int i = 0; i < dbSimilar.viewIds.size(); i++) {
        assertEquals(dbSimilar.numFeatures, dbSimilar.viewObs.get(i).size());
    }
    // dividing number of features by two because only even observations are inliers
    alg.utils.structure.initialize(numViews, dbSimilar.numFeatures / 2);
    alg.utils.observations.initialize(numViews);
    // Transform that makes view[0] identity
    DMatrixRMaj H = new DMatrixRMaj(4, 4);
    MultiViewOps.projectiveToIdentityH(dbSimilar.listCameraMatrices.get(0), H);
    // construct projective SBA scene from metric ground truth
    for (int viewIdx = 0; viewIdx < dbSimilar.viewIds.size(); viewIdx++) {
        DMatrixRMaj P = new DMatrixRMaj(3, 4);
        CommonOps_DDRM.mult(dbSimilar.listCameraMatrices.get(viewIdx), H, P);
        alg.viewsByStructureIndex.add(dbSimilar.graph.nodes.get(viewIdx));
        alg.utils.structure.views.get(viewIdx).worldToView.setTo(P);
        alg.utils.structure.views.get(viewIdx).width = viewIdx;
        // only features with an even ID will be inliers
        DogArray_I32 inliers = alg.inlierIndexes.grow();
        int[] featureIDs = dbSimilar.viewToFeat.get(viewIdx);
        SceneObservations.View oview = alg.utils.observations.views.get(viewIdx);
        for (int obsIdx = 0; obsIdx < featureIDs.length; obsIdx++) {
            int featureID = featureIDs[obsIdx];
            if (featureID % 2 == 1)
                continue;
            inliers.add(oview.size());
            Point2D_F64 pixel = dbSimilar.viewObs.get(viewIdx).get(obsIdx);
            oview.add(featureID / 2, (float) pixel.x, (float) pixel.y);
        }
    }
    // Call the function we are testing
    List<String> viewIds = new ArrayList<>();
    DogArray<ElevateViewInfo> views = new DogArray<>(ElevateViewInfo::new);
    DogArray<DMatrixRMaj> cameraMatrices = new DogArray<>(() -> new DMatrixRMaj(3, 4));
    DogArray<AssociatedTupleDN> observations = new DogArray<>(AssociatedTupleDN::new);
    alg.lookupInfoForMetricElevation(viewIds, views, cameraMatrices, observations);
    // check what can be checked trivially by comparing to the db
    assertEquals(4, viewIds.size());
    assertEquals(4, views.size());
    assertEquals(3, cameraMatrices.size());
    assertEquals(dbSimilar.numFeatures / 2, observations.size());
    for (int viewIdx = 0; viewIdx < 4; viewIdx++) {
        assertEquals(dbSimilar.viewIds.get(viewIdx), viewIds.get(viewIdx));
        assertEquals(viewIdx, views.get(viewIdx).shape.width);
        // only one camera
        assertEquals(0, views.get(viewIdx).cameraID);
    }
    // See if it unscrambled the observations
    for (int obsIdx = 0; obsIdx < dbSimilar.numFeatures / 2; obsIdx++) {
        for (int viewIdx = 0; viewIdx < 4; viewIdx++) {
            Point2D_F64 expected = dbSimilar.viewObs.get(viewIdx).get(dbSimilar.featToView.get(viewIdx)[obsIdx * 2]);
            Point2D_F64 found = observations.get(obsIdx).get(viewIdx);
            assertEquals(0.0, expected.distance(found), UtilEjml.TEST_F32);
        }
    }
}