This component of Heatmapper provides a professional stereo imaging with extensive features, including 3D mapping entire rooms at once and finding markers.
Some parts of this component are written in Cython to improve performance. Please run python cython_setup.py build_ext --inplace to generate the C version and symbolic objects.
To generate said C version on Windows, a version of C++ 14.0 or higher is required. C++ can be installed here: https://visualstudio.microsoft.com/visual-cpp-build-tools/
The following script is meant to run on a raspberry that has two 5MP cameras attached to it. The while loop has the ColorMarkerTracker continuously capture frames from the cameras and calculate the coordinates of the marker.
# file: rpi_track_purple_marker.py
from stereo_mapping.hardware_specific.DIYZone_RaspberryPi4_5MP_130deg_Arducam_Multi_Camera_Adapter_Module_V2_2 import DIYZone_RaspberryPi4_5MP_130deg_Arducam_Multi_Camera_Adapter_Module_V2_2
from stereo_mapping.single_point.color_marker_detection import MarkerValueFunction
from stereo_mapping.single_point.color_marker_tracker import ColorMarkerTracker
# camera and marker selection
stereo_camera = DIYZone_RaspberryPi4_5MP_130deg_Arducam_Multi_Camera_Adapter_Module_V2_2()
marker_value_fn = MarkerValueFunction.purple
cmt = ColorMarkerTracker(stereo_camera, marker_value_fn)
while True:
print(cmt.get_coordinates())Let's have a quick look at when the ColorMarkerTracker class is doing:
# file: single_point/color_marker_tracker.py
from stereo_mapping.hardware_specific.stereo_camera import StereoCamera
from stereo_mapping.single_point.color_marker_detection import MarkerValueFunction, find_marker
from stereo_mapping.single_point.trig import calculate_angles_to, calculate_coordinates, Camera
from recording.lib.dataset import Coordinates
class ColorMarkerTracker:
def __init__(
self,
stereo_camera: StereoCamera,
marker_value_fn: MarkerValueFunction
):
# save the selected camera and value function
self.__stereo_camera = stereo_camera
self.__marker_value_fn = marker_value_fn
# compute static values that are required to compute coordinates
frame_width, frame_height = stereo_camera.get_frame_dimensions()
self.__stereo_camera_specs = (frame_width,
frame_height,
stereo_camera.vertical_angle_of_view_rad,
stereo_camera.horizontal_angle_of_view_rad)
def get_coordinates(self) -> Coordinates:
# capture left and right frames using the selected stereo camera
left_frame, right_frame = self.__stereo_camera.capture_stereo_frames()
# calculate the vertical and horizontal angles from the left camera to the marker
left_angles = calculate_angles_to(
# find the marker in the left frame
*find_marker(left_frame, self.__marker_value_fn),
*self.__stereo_camera_specs,
Camera.LEFT
)
# calculate the vertical and horizontal angles from the right camera to the marker
right_angles = calculate_angles_to(
# find the marker in the right frame
*find_marker(right_frame, self.__marker_value_fn),
*self.__stereo_camera_specs,
Camera.RIGHT
)
# compute the actual 3d coordinates from the angles
return calculate_coordinates(left_angles, right_angles, self.__stereo_camera.parallax_m)The Hardware Specific component deals with obtaining image data from different cameras and removing lens distortion ("undistorting") the images so they are ready to be processed by other scripts.
The Single Point component provides scripts related to performing simple stereo imaging procedures to calculate the location of a marker.
Warning: This component is still highly experimental.
The Multi Point component provides scripts related to creating 3D maps from stereo imaging data.
- OpenCV guide to Stereo Block Matching https://docs.opencv.org/3.4/d9/db7/tutorial_py_table_of_contents_calib3d.html
- OpenCV edge detection https://learnopencv.com/edge-detection-using-opencv/