private void addNewTracks ( ) { // System.out.println("----------- Adding new tracks ---------------"); tracker . spawnTracks ( ) ; List < PointTrack > spawned = tracker . getNewTracks ( null ) ; // estimate 3D coordinate using stereo vision for ( PointTrack t : spawned ) { Point2D3DTrack p = t . getCookie ( ) ; if ( p == null ) { t . cookie = p = new Point2D3DTrack ( ) ; } // discard point if it can't localized if ( ! pixelTo3D . process ( t . x , t . y ) || pixelTo3D . getW ( ) == 0 ) { tracker . dropTrack ( t ) ; } else { Point3D_F64 X = p . getLocation ( ) ; double w = pixelTo3D . getW ( ) ; X . set ( pixelTo3D . getX ( ) / w , pixelTo3D . getY ( ) / w , pixelTo3D . getZ ( ) / w ) ; // translate the point into the key frame // SePointOps_F64.transform(currToKey,X,X); // not needed since the current frame was just set to be the key frame p . lastInlier = tick ; pixelToNorm . compute ( t . x , t . y , p . observation ) ; } } }
Detects new features and computes their 3D coordinates