klt.py

#*********************************************************************
#* klt.h
#*
#* Kanade-Lucas-Tomasi tracker
#*********************************************************************/

from __future__ import print_function

#typedef float KLT_locType;
#typedef unsigned char KLT_PixelType;

#define KLT_BOOL int

#ifndef TRUE
#define TRUE 1
#define FALSE 0
#endif

#ifndef NULL
#define NULL 0
#endif

class kltState:
	KLT_TRACKED = 0
	KLT_NOT_FOUND = -1
	KLT_SMALL_DET = -2
	KLT_MAX_ITERATIONS = -3
	KLT_OOB = -4
	KLT_LARGE_RESIDUE = -5

#include "klt_util.h" /* for affine mapping */

import math
from error import *
from convolve import *
from klt_util import *

#*******************
#* Structures
#*

class KLT_TrackingContext:
	def __init__(self):
		#Set values to default values
		self.mindist = 10
		self.window_width = 7
		self.window_height = 7
		self.sequentialMode = False
		self.retainTrackers = False
		self.smoothBeforeSelecting = True
		self.writeInternalImages = False
		self.lighting_insensitive = False
		self.min_eigenvalue = 1
		self.min_determinant = 0.01
		self.max_iterations = 10
		self.min_displacement = 0.1
		self.max_residue = None #10.0
		self.grad_sigma = 1.0
		self.smooth_sigma_fact = 0.1
		self.pyramid_sigma_fact = 0.9
		self.step_factor = 1.0
		self.nSkippedPixels = 0
		self.pyramid_last = None
		self.pyramid_last_gradx = None
		self.pyramid_last_grady = None
		# for affine mapping
		self.affineConsistencyCheck = -1
		self.affine_window_width = 15
		self.affine_window_height = 15
		self.affine_max_iterations = 10
		self.affine_max_residue = 10.
		self.affine_min_displacement = 0.02
		self.affine_max_displacement_differ = 1.5

		# Change nPyramidLevels and subsampling
		search_range=15
		self.KLTChangeTCPyramid(search_range)
	
		# Update border, which is dependent upon
		# smooth_sigma_fact, pyramid_sigma_fact, window_size, and subsampling
		self.KLTUpdateTCBorder()


	def KLTChangeTCPyramid(self,search_range):

		# Check window size (and correct if necessary) 
		if self.window_width % 2 != 1:
			self.window_width = self.window_width+1
			KLTWarning("(KLTChangeTCPyramid) Window width must be odd. Changing to {0}\n".format(self.window_width))

		if self.window_height % 2 != 1:
			self.window_height = self.window_height+1
			KLTWarning("(KLTChangeTCPyramid) Window height must be odd. Changing to {0}.\n".format(self.window_height))
		
		if self.window_width < 3:
			self.window_width = 3;
			KLTWarning("(KLTChangeTCPyramid) Window width must be at least three. \nChanging to {0}.\n".format(self.window_width))

		if self.window_height < 3:
			self.window_height = 3
			KLTWarning("(KLTChangeTCPyramid) Window height must be at least three. \nChanging to %d.\n".format(self.window_height))
		
		window_halfwidth = min(self.window_width,self.window_height)/2.0

		subsampling = float (search_range) / window_halfwidth

		if subsampling < 1.0:		# 1.0 = 0+1
			self.nPyramidLevels = 1
		elif subsampling <= 3.0:	# 3.0 = 2+1
			self.nPyramidLevels = 2
			self.subsampling = 2
		elif subsampling <= 5.0: 	# 5.0 = 4+1
			self.nPyramidLevels = 2
			self.subsampling = 4
		elif subsampling <= 9.0: 	# 9.0 = 8+1
			self.nPyramidLevels = 2
			self.subsampling = 8
		else:
			# The following lines are derived from the formula:
			# search_range = 
			# window_halfwidth * \sum_{i=0}^{nPyramidLevels-1} 8^i,
			# which is the same as:
			# search_range = 
			# window_halfwidth * (8^nPyramidLevels - 1)/(8 - 1).
			# Then, the value is rounded up to the nearest integer. 
			val = float (math.log(7.0*subsampling+1.0)/math.log(8.0))
			self.nPyramidLevels = int(val + 0.99)
			self.subsampling = 8
		


	#*********************************************************************
	#* Updates border, which is dependent upon 
	#* smooth_sigma_fact, pyramid_sigma_fact, window_size, and subsampling
	#*

	def KLTUpdateTCBorder(self):
	
		num_levels = self.nPyramidLevels;
		ss = self.subsampling;

		# Check window size (and correct if necessary)
		if self.window_width % 2 != 1:
			self.window_width = self.window_width+1
			KLTWarning("(KLTUpdateTCBorder) Window width must be odd. Changing to {0}.\n".format(self.window_width))
		
		if self.window_height % 2 != 1:
			self.window_height = self.window_height+1
			KLTWarning("(KLTUpdateTCBorder) Window height must be odd. Changing to {0}.\n".format(self.window_height))
		
		if self.window_width < 3:
			self.window_width = 3
			KLTWarning("(KLTUpdateTCBorder) Window width must be at least three. \nChanging to {0}.\n".format(self.window_width))
		
		if self.window_height < 3:
			self.window_height = 3
			KLTWarning("(KLTUpdateTCBorder) Window height must be at least three. \nChanging to {0}.\n".format(self.window_height))
		
		window_hw = max(self.window_width, self.window_height)/2

		# Find widths of convolution windows
		gauss_width, gaussderiv_width = KLTGetKernelWidths(KLTComputeSmoothSigma(self))

		smooth_gauss_hw = gauss_width/2
		gauss_width, gaussderiv_width = KLTGetKernelWidths(_pyramidSigma(self))
		pyramid_gauss_hw = gauss_width/2

		# Compute the # of invalid pixels at each level of the pyramid.
		# n_invalid_pixels is computed with respect to the ith level 
		# of the pyramid. So, e.g., if n_invalid_pixels = 5 after 
		# the first iteration, then there are 5 invalid pixels in 
		# level 1, which translated means 5*subsampling invalid pixels 
		# in the original level 0. 
		n_invalid_pixels = smooth_gauss_hw
		for i in range(1,num_levels):
			val = (float(n_invalid_pixels) + pyramid_gauss_hw) / ss
			n_invalid_pixels = int(val + 0.99) # Round up
	
		# ss_power = ss^(num_levels-1) 
		ss_power = 1
		for i in range(1,num_levels):
			ss_power *= ss

		# Compute border by translating invalid pixels back into
		# original image
		border = (n_invalid_pixels + window_hw) * ss_power

		self.borderx = border
		self.bordery = border
	

#*********************************************************************
#* NOTE: Manually must ensure consistency with _KLTComputePyramid()
#*
 
def _pyramidSigma(tc):
	return (tc.pyramid_sigma_fact * tc.subsampling)


 #Available to user
 #int mindist		# min distance b/w features
 #int window_width, window_height
 #KLT_BOOL sequentialMode;	/* whether to save most recent image to save time */
 #/* can set to TRUE manually, but don't set to */
 #/* FALSE manually */
 #KLT_BOOL smoothBeforeSelecting;	/* whether to smooth image before */
 #/* selecting features */
 #KLT_BOOL writeInternalImages;	/* whether to write internal images */
 #/* tracking features */
 #KLT_BOOL lighting_insensitive; /* whether to normalize for gain and bias (not in original algorithm) */
 
 #/* Available, but hopefully can ignore */
 #int min_eigenvalue;		/* smallest eigenvalue allowed for selecting */
 #float min_determinant;	/* th for determining lost */
 #float min_displacement;	/* th for stopping tracking when pixel changes little */
 #int max_iterations;		/* th for stopping tracking when too many iterations */
 #float max_residue;		/* th for stopping tracking when residue is large */
 #float grad_sigma;
 #float smooth_sigma_fact;
 #float pyramid_sigma_fact;
 #float step_factor; /* size of Newton steps; 2.0 comes from equations, 1.0 seems to avoid overshooting */
 #int nSkippedPixels;		/* # of pixels skipped when finding features */
 #int borderx;			/* border in which features will not be found */
 #int bordery;
 #int nPyramidLevels;		/* computed from search_ranges */
 #int subsampling;		/* 		" */

 
 # for affine mapping 
 #int affine_window_width, affine_window_height;
 #int affineConsistencyCheck; /* whether to evaluates the consistency of features with affine mapping 
 # -1 = don't evaluates the consistency
 # 0 = evaluates the consistency of features with translation mapping
 # 1 = evaluates the consistency of features with similarity mapping
 # 2 = evaluates the consistency of features with affine mapping
 
 #int affine_max_iterations; 
 #float affine_max_residue;
 #float affine_min_displacement; 
 #float affine_max_displacement_differ; /* th for the difference between the displacement calculated 
 #by the affine tracker and the frame to frame tracker in pel*/

 # User must not touch these
 #void *pyramid_last;
 #void *pyramid_last_gradx;
 #void *pyramid_last_grady;
#KLT_TrackingContextRec, *KLT_TrackingContext;

class KLT_Feature:
	def __init__(self):
		x = None
		y = None
		val = None
		# for affine mapping
		aff_img = None
		aff_img_gradx = None
		aff_img_grady = None
		aff_x = None
		aff_y = None
		aff_Axx = None
		aff_Ayx = None
		aff_Axy = None
		aff_Ayy = None
# KLT_FeatureRec, *KLT_Feature;

#class KLT_FeatureList:
#	def __init__(self, nFeatures):
#		#self.nFeatures = nFeatures
#		self.feature = [KLT_Feature() for i in range(nFeatures)]
#KLT_FeatureListRec, *KLT_FeatureList;

class KLT_FeatureHistory:
 pass
 #int nFrames;
 #KLT_Feature *feature;
#KLT_FeatureHistoryRec, *KLT_FeatureHistory;

class KLT_FeatureTable:
 pass
 #int nFrames;
 #int nFeatures;
 #KLT_Feature **feature;
#KLT_FeatureTableRec, *KLT_FeatureTable;

def KLTPrintTrackingContext(tc):

	print(tc)
	print("\n\nTracking context:\n")
	print("\tmindist = {0}".format(tc.mindist))
	print("\twindow_width = {0}".format(tc.window_width))
	print("\twindow_height = {0}".format(tc.window_height))
	print("\tsequentialMode = {0}".format(tc.sequentialMode))
	print("\tsmoothBeforeSelecting = {0}".format(tc.smoothBeforeSelecting))
	print("\twriteInternalImages = {0}".format(tc.writeInternalImages))

	print("\tmin_eigenvalue = {0}".format(tc.min_eigenvalue))
	print("\tmin_determinant = {0}".format(tc.min_determinant))
	print("\tmin_displacement = {0}".format(tc.min_displacement))
	print("\tmax_iterations = {0}".format(tc.max_iterations))
	print("\tmax_residue = {0}".format(tc.max_residue))
	print("\tgrad_sigma = {0}".format(tc.grad_sigma))
	print("\tsmooth_sigma_fact = {0}".format(tc.smooth_sigma_fact))
	print("\tpyramid_sigma_fact = {0}".format(tc.pyramid_sigma_fact))
	print("\tnSkippedPixels = {0}".format(tc.nSkippedPixels))
	print("\tborderx = {0}".format(tc.borderx))
	print("\tbordery = {0}".format(tc.bordery))
	print("\tnPyramidLevels = {0}".format(tc.nPyramidLevels))
	print("\tsubsampling = {0}".format(tc.subsampling))

	print("\n\tpyramid_last = {0}".format(tc.pyramid_last))
	print("\tpyramid_last_gradx = {0}".format(tc.pyramid_last_gradx))
	print("\tpyramid_last_grady = {0}".format(tc.pyramid_last_grady))
	print("\n")

#*********************************************************************
#* KLTCountRemainingFeatures
#*

def KLTCountRemainingFeatures(fl):

	count = 0
	for feat in fl:
		if feat.val >= 0:
			count = count + 1
	return count