Navigation Menu

Skip to content

Instantly share code, notes, and snippets.

@adityakamath
Last active June 3, 2023 09:28
Show Gist options
  • Star 0 You must be signed in to star a gist
  • Fork 0 You must be signed in to fork a gist
  • Save adityakamath/b88d36209b5ebf08d484bc9898362812 to your computer and use it in GitHub Desktop.
Save adityakamath/b88d36209b5ebf08d484bc9898362812 to your computer and use it in GitHub Desktop.
PMW3901/PAA5100 optical flow pixel to distances
# h = height/altitude of the sensor from ground
# fov = field of view of the sensor in radians
# res = resolution of the sensor in pixels
# scaler = scaling factor
# dt = time period of this loop
conversion_factor = h*2*np.tan(fov/2)/(res*scaler)
if 'paa5100':
# Convert data from sensor frame to ROS frame for PAA5100
# ROS frame: front/back = +x/-x, left/right = +y/-y
# Sensor frame: front/back = -y/+y, left/right = +x/-x
dist_x = -1*conversion_factor*delta_y
dist_y = conversion_factor*delta_x
elif 'pmw3901':
# ROS and Sensor frames are assumed to align for PMW3901
# based on https://docs.px4.io/main/en/sensor/pmw3901.html#mounting-orientation
dist_x = conversion_factor*delta_x
dist_y = conversion_factor*delta_y
abs_dist_x += dist_x
abs_dist_y += dist_y
vel_x = dist_x/dt
vel_y = dist_y/dt
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment