/** * Removes observations which have been marked with NaN */ private void removeMarkedObservations() { Point2D_F64 observation = new Point2D_F64(); for (int viewIndex = 0; viewIndex < observations.views.length; viewIndex++) { // System.out.println("ViewIndex="+viewIndex); SceneObservations.View v = observations.views[viewIndex]; for(int pointIndex = v.point.size-1; pointIndex >= 0; pointIndex-- ) { int pointID = v.getPointId(pointIndex); SceneStructureMetric.Point f = structure.points[pointID]; // System.out.println(" pointIndex="+pointIndex+" pointID="+pointID+" hash="+f.hashCode()); v.get(pointIndex, observation); if( !Double.isNaN(observation.x)) continue; if( !f.views.contains(viewIndex)) throw new RuntimeException("BUG!"); // Tell the feature it is no longer visible in this view f.removeView(viewIndex); // Remove the observation of this feature from the view v.remove(pointIndex); } } }
/** * Check to see if a point is behind the camera which is viewing it. If it is remove that observation * since it can't possibly be observed. */ public void pruneObservationsBehindCamera() { Point3D_F64 X = new Point3D_F64(); for (int viewIndex = 0; viewIndex < observations.views.length; viewIndex++) { SceneObservations.View v = observations.views[viewIndex]; SceneStructureMetric.View view = structure.views[viewIndex]; for (int pointIndex = 0; pointIndex < v.point.size; pointIndex++) { SceneStructureMetric.Point f = structure.points[v.getPointId(pointIndex)]; // Get feature location in world f.get(X); if( !f.views.contains(viewIndex)) throw new RuntimeException("BUG!"); // World to View view.worldToView.transform(X, X); // Is the feature behind this view and can't be seen? if( X.z <= 0 ) { v.set(pointIndex, Float.NaN, Float.NaN); } } } removeMarkedObservations(); }