98 lines
2.4 KiB
Plaintext
98 lines
2.4 KiB
Plaintext
|
|
||
|
|
|
||
|
|
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
|