Vision based pose and velocity estimation

Description 

• Abstract

Pose estimation:

we made the quadrotor fly through the certain trajectory. We store the a struct array of data, which contains time stamp ID of every apriltags. Center and four corners of the apriltags in the image. 
We have a true map of the apriltags on the floor. In this case, we are able to have the information of all the corners in world frame. We also have those in the image frame. We then convert them to the camera frame using K. Then we linearize the transformation function by putting together to one matrix A. then we use svd to solve for A. the transformation H is the last column of the svd. Then reshape the H to 3*3 matrix. Since the Z=0 on the ground plane, we can recover R,T.

 

velocity estimation:

It is complete with feature detection and tracking as well as outliers rejection using ransac. First is extracting features in the form of corners from provided camera image. We use toolbox for this. 
Next , we compute the sparse optical flow of these features as they propagate through consecutive image. We use KLT algorithm for this. Assume all corners are on the floor, Z can be computed based on the estimated height and rotation of the quadrotor using apriltags. Computing 3D Velocity from 2D
We encounter outliers in the optical flow, where we use ransac to reject outliers. A three-point ransac can be used for outlier rejection. We compute again using the formula with all the inliers.

 

​

​

​