boofcv.alg.tracker.combined.CombinedTrackerScalePoint Maven / Gradle / Ivy

/*
 * Copyright (c) 2011-2016, Peter Abeles. All Rights Reserved.
 *
 * This file is part of BoofCV (http://boofcv.org).
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *   http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

package boofcv.alg.tracker.combined;

import boofcv.abst.feature.associate.AssociateDescription;
import boofcv.abst.feature.detdesc.DetectDescribePoint;
import boofcv.struct.feature.AssociatedIndex;
import boofcv.struct.feature.TupleDesc;
import boofcv.struct.image.ImageGray;
import boofcv.struct.pyramid.PyramidDiscrete;
import georegression.struct.point.Point2D_F64;
import org.ddogleg.struct.FastQueue;

import java.util.ArrayList;
import java.util.List;
import java.util.Stack;

/**
 * 
 * Combines a KLT tracker with Detect-Describe-Associate type trackers.  Features are nominally tracked
 * using KLT, but after KLT drops a track it is deactivated and made dormant.  Upon request, it will
 * attempt to reactivate a dormant track by associating it with newly detected features.  After a
 * track has been reactivated it will be tracked normally.
 * 
 *
 * @author Peter Abeles
 */
// TODO Two versions.  One for InterestPointDetector and one for corners
public class CombinedTrackerScalePoint
		 {

	// current image in sequence
	private I input;

	// The KLT tracker used to perform the nominal track update
	protected PyramidKltForCombined trackerKlt;

	// feature detector and describer
	protected DetectDescribePoint detector;
	// Used to associate features using their DDA description
	protected AssociateDescription associate;

	// all active tracks that have been tracked purely by KLT
	protected List> tracksPureKlt = new ArrayList<>();
	// tracks that had been dropped by KLT but have been reactivated
	protected List> tracksReactivated = new ArrayList<>();
	// tracks that are not actively being tracked
	protected List> tracksDormant = new ArrayList<>();
	// recently spawned tracks
	protected List> tracksSpawned = new ArrayList<>();
	// track points whose data is to be reused
	protected Stack> tracksUnused = new Stack<>();

	// local storage used by association
	protected FastQueue detectedDesc;
	protected FastQueue knownDesc;

	// number of tracks it has created
	protected long totalTracks = 0;

	// Marks a known track as being associated
	private boolean associated[] = new boolean[1];

	/**
	 * Configures tracker
	 *
	 * @param trackerKlt KLT tracker used nominally
	 * @param detector Feature detector
	 * @param associate Association algorithm
	 */
	public CombinedTrackerScalePoint(PyramidKltForCombined trackerKlt,
									 DetectDescribePoint detector,
									 AssociateDescription associate ) {
		this.trackerKlt = trackerKlt;
		this.detector = detector;

		detectedDesc = new FastQueue<>(10, detector.getDescriptionType(), false);
		knownDesc = new FastQueue<>(10, detector.getDescriptionType(), false);

		this.associate = associate;
	}

	/**
	 * Used for unit tests
	 */
	protected CombinedTrackerScalePoint() {
	}

	/**
	 * Sets the tracker into its initial state.  Previously declared track data structures are saved
	 * for re-use.
	 */
	public void reset() {
		dropAllTracks();
		totalTracks = 0;
	}

	/**
	 * Updates the location and description of tracks using KLT.  Saves a reference
	 * to the input image for future processing.
	 *
	 * @param input Input image.
	 * @param pyramid Image pyramid of input.
	 * @param derivX Derivative pyramid of input x-axis
	 * @param derivY Derivative pyramid of input y-axis
	 */
	public void updateTracks( I input ,
							  PyramidDiscrete pyramid ,
							  D[] derivX,
							  D[] derivY ) {
		// forget recently dropped or spawned tracks
		tracksSpawned.clear();

		// save references
		this.input = input;

		trackerKlt.setInputs(pyramid, derivX, derivY);

		trackUsingKlt(tracksPureKlt);
		trackUsingKlt(tracksReactivated);
	}

	/**
	 * Tracks features in the list using KLT and update their state
	 */
	private void trackUsingKlt(List> tracks) {
		for( int i = 0; i < tracks.size();  ) {
			CombinedTrack track = tracks.get(i);

			if( !trackerKlt.performTracking(track.track) ) {
				// handle the dropped track
				tracks.remove(i);
				tracksDormant.add(track);
			} else {
				track.set(track.track.x,track.track.y);
				i++;
			}
		}
	}

	/**
	 * From the found interest points create new tracks.  Tracks are only created at points
	 * where there are no existing tracks.
	 *
	 * Note: Must be called after {@link #associateAllToDetected}.
	 */
	public void spawnTracksFromDetected() {
		// mark detected features with no matches as available
		FastQueue matches = associate.getMatches();

		int N = detector.getNumberOfFeatures();
		for( int i = 0; i < N; i++ )
			associated[i] = false;

		for( AssociatedIndex i : matches.toList() ) {
			associated[i.dst] = true;
		}

		// spawn new tracks for unassociated detected features
		for( int i = 0; i < N; i++ ) {
			if( associated[i])
				continue;

			Point2D_F64 p = detector.getLocation(i);

			TD d = detectedDesc.get(i);

			CombinedTrack track;

			if( tracksUnused.size() > 0 ) {
				track = tracksUnused.pop();
			} else {
				track = new CombinedTrack<>();
				track.desc = detector.createDescription();
				track.track = trackerKlt.createNewTrack();
			}

			// create the descriptor for tracking
			trackerKlt.setDescription((float)p.x,(float)p.y,track.track);
			// set track ID and location
			track.featureId = totalTracks++;
			track.desc.setTo(d);
			track.set(p);

			// update list of active tracks
			tracksPureKlt.add(track);
			tracksSpawned.add(track);
		}
	}


	/**
	 * Associates pre-existing tracks to newly detected features
	 *
	 * @param known List of known tracks
	 */
	private void associateToDetected( List> known ) {
		// initialize data structures
		detectedDesc.reset();
		knownDesc.reset();

		// create a list of detected feature descriptions
		int N = detector.getNumberOfFeatures();
		for( int i = 0; i < N; i++ ) {
			detectedDesc.add(detector.getDescription(i));
		}

		// create a list of previously created track descriptions
		for( CombinedTrack t : known ) {
			knownDesc.add(t.desc);
		}

		// associate features
		associate.setSource(knownDesc);
		associate.setDestination(detectedDesc);
		associate.associate();

		N = Math.max(known.size(),detector.getNumberOfFeatures());
		if( associated.length < N )
			associated = new boolean[N];
	}

	/**
	 * Associate all tracks in any state to the latest observations.  If a dormant track is associated it
	 * will be reactivated.  If a reactivated track is associated it's state will be updated.  PureKLT
	 * tracks are left unmodified.
	 */
	public void associateAllToDetected() {
		// initialize data structures
		List> all = new ArrayList<>();
		all.addAll(tracksReactivated);
		all.addAll(tracksDormant);
		all.addAll(tracksPureKlt);

		int numTainted = tracksReactivated.size() + tracksDormant.size();

		tracksReactivated.clear();
		tracksDormant.clear();

		// detect features
		detector.detect(input);
		// associate features
		associateToDetected(all);

		FastQueue matches = associate.getMatches();

		// See which features got respawned and which ones are made dormant
		for( int i = 0; i < numTainted; i++ ) {
			associated[i] = false;
		}

		for( AssociatedIndex a : matches.toList() ) {
			// don't mess with pure-KLT tracks
			if( a.src >= numTainted )
				continue;

			CombinedTrack t = all.get(a.src);

			t.set(detector.getLocation(a.dst));
			trackerKlt.setDescription((float) t.x, (float) t.y, t.track);
			tracksReactivated.add(t);
			associated[a.src] = true;
		}

		for( int i = 0; i < numTainted; i++ ) {
			if( !associated[i] ) {
				tracksDormant.add(all.get(i));
			}
		}
	}

	/**
	 * Stops tracking the specified track and recycles its data.
	 *
	 * @param track The track being dropped
	 * @return true if the track was being tracked and data was recycled false if not.
	 */
	public boolean dropTrack( CombinedTrack track ) {
		if( !tracksPureKlt.remove(track) )
			if( !tracksReactivated.remove(track) )
				if( !tracksDormant.remove(track) )
					return false;

		tracksUnused.add(track);
		return true;
	}

	public List> getSpawned() {
		return tracksSpawned;
	}

	public List> getPureKlt() {
		return tracksPureKlt;
	}

	public List> getReactivated() {
		return tracksReactivated;
	}

	public List> getDormant() {
		return tracksDormant;
	}

	public PyramidKltForCombined getTrackerKlt() {
		return trackerKlt;
	}

	public DetectDescribePoint getDetector() {
		return detector;
	}

	/**
	 * Drops all tracks and recycles the data
	 */
	public void dropAllTracks() {
		tracksUnused.addAll(tracksDormant);
		tracksUnused.addAll(tracksPureKlt);
		tracksUnused.addAll(tracksReactivated);

		tracksSpawned.clear();
		tracksPureKlt.clear();
		tracksReactivated.clear();
		tracksSpawned.clear();
		tracksDormant.clear();
	}
}