private void addNewTracks ( ) { tracker . spawnTracks ( ) ; List < PointTrack > spawned = tracker . getNewTracks ( null ) ; // estimate 3D coordinate using stereo vision for ( PointTrack t : spawned ) { // System.out.println("track spawn "+t.x+" "+t.y); VoTrack p = t . getCookie ( ) ; if ( p == null ) { t . cookie = p = new VoTrack ( ) ; } // compute normalized image coordinate pixelToNorm . compute ( t . x , t . y , n ) ; // System.out.println(" pointing "+pointing.x+" "+pointing.y+" "+pointing.z); // See if the point ever intersects the ground plane or not if ( planeProjection . normalToPlane ( n . x , n . y , p . ground ) ) { // the line above computes the 2D plane position of the point p . onPlane = true ; } else { // Handle points at infinity which are off plane. // rotate observation pointing vector into plane reference frame pointing . set ( n . x , n . y , 1 ) ; GeometryMath_F64 . mult ( cameraToPlane . getR ( ) , pointing , pointing ) ; pointing . normalize ( ) ; // save value of y-axis in pointing vector double normXZ = Math . sqrt ( pointing . x * pointing . x + pointing . z * pointing . z ) ; p . pointingY = pointing . y / normXZ ; // save the angle as a vector p . ground . x = pointing . z ; p . ground . y = - pointing . x ; // normalize to make later calculations easier p . ground . x /= normXZ ; p . ground . y /= normXZ ; p . onPlane = false ; } p . lastInlier = tick ; } }
Requests that new tracks are spawned determines if they are on the plane or not and computes other required data structures .