

stereo callback()

	create image pyramids
		_Constructs the image pyramid which can be passed to calcOpticalFlowPyrLK._
.
	if first Frame:
		*initialize first Frame ()

	else:
		*track Features ()

		*addnewFeatures ()

		*pruneGridFeatures()
			_removes worst features from any overflowing grid_

	publish features (u1, v1, u2, v2)
		_undistorts them beforehand_

addnewFeatures()
	*mask existing features
	*detect new fast features
	*collect in a grid, keep only best n per grid
	*stereomatch()
	*save inliers into a new feature with u,v on cam0 and cam1


track Features()
	*integrateIMUData ()
		_uses existing IMU data between two frames to calc. rotation between the two frames_

	*predictFeatureTracking()
		_compensates the rotation between consecutive frames - rotates previous camera frame features to current camera rotation_

	*calcOpticalFlowPyrLK()
		_measures the change between the features in the previous frame and in the current frame (using the predicted features)_

	*remove points outside of image region
		_how does this even happen?_

	*stereo match()
		_find tracked features from optical flow in the camera 1 image_
		_remove all features that could not be matched_

	*twoPointRansac(cam0)
	*twoPointRansac(cam1)
		_remove any features outside best found ransac model_




twoPointRansac()
	*mark all points as inliers
	*compensate rotation between frames
	*normalize points
	*calculate difference bewteen previous and current points
	*mark large distances (over 50 pixels currently)
	*calculate mean points distance
	*return if inliers (non marked) < 3
	*return if motion smaller than norm pixel unit
	*ransac
		*optimize with found inlier after random sample

	*set inlier markers

initialize first Frame()

	features = FastFeatureDetector detect ()

	*stereo match ()

	group features into grid
		- according to position in the image
		- sorting them by response
		- save the top responses
		- save the top responses


stereo match ()

	*undistort cam0 Points
	*project cam0 Points to cam1 to initialize points in cam1

	*calculate lk optical flow
		_used because camera calibrations not perfect enough_
		_also, calculation more efficient, because LK calculated anyway_

	*compute relative trans/rot between cam0 and cam1*

	*remove outliers based on essential matrix
		_essential matrix relates points in stereo image (pinhole model)_
		for every point:
			- calculate epipolar line of point in cam0
			- calculate error of cam1 to epipolar line 
			- remove if to big
