def run():
motor_controller = MotorController('/dev/cu.usbmodem14211')
sp = StereoPair(DEVICES_ID)
block_matcher = StereoBM()
camera_pair = CalibratedPair(
None, StereoCalibration(input_folder='calibration/'), block_matcher)
current_value = None
last_size = None
i = 0
while True:
frames_single_img = sp.get_frames()
points = camera_pair.get_point_cloud(frames_single_img)
points = points.filter_infinity()
number = []
for level1 in points.coordinates:
if not level1[2] == 512:
number.append([0.0, 0.0, level1[2]])
number_size = len(number)
if last_size:
current_value = (number_size + last_size) / 2
# print current_value
last_size = number_size
i += 1
motor_state = False
if not current_value >= 3000:
print current_value
motor_state = True
motor_controller.send(motor_state)
def main():
"""Produce PLY point clouds from stereo image pair."""
parser = argparse.ArgumentParser(description="Read images taken with "
"stereo pair and use them to produce 3D "
"point clouds that can be viewed with "
"MeshLab.",
parents=[STEREO_BM_FLAG])
parser.add_argument("calibration", help="Path to calibration folder.")
parser.add_argument("left", help="Path to left image")
parser.add_argument("right", help="Path to right image")
parser.add_argument("output", help="Path to output file.")
parser.add_argument("--bm_settings",
help="Path to block matcher's settings.")
args = parser.parse_args()
image_pair = [cv2.imread(image) for image in [args.left, args.right]]
calib_folder = args.calibration
if args.use_stereobm:
block_matcher = StereoBM()
else:
block_matcher = StereoSGBM()
if args.bm_settings:
block_matcher.load_settings(args.bm_settings)
camera_pair = CalibratedPair(None,
StereoCalibration(input_folder=calib_folder),
block_matcher)
rectified_pair = camera_pair.calibration.rectify(image_pair)
points = camera_pair.get_point_cloud(rectified_pair)
points = points.filter_infinity()
points.write_ply(args.output)
def __init__(self,
calib_foler='calib_files_test',
SGBM_setting='settings/SGBM'):
self.calibration = StereoCalibration(input_folder=calib_foler)
self.block_matcher = StereoSGBM()
self.block_matcher.load_settings(settings=SGBM_setting)
self.CalibratedPair = CalibratedPair(None, self.calibration,
self.block_matcher)
self.imageSize = (1280, 720)
self.left_img = None
self.right_img = None
def __init__(self, calib_foler = 'calib_files_test', SGBM_setting = 'settings/SGBM'):
self.calibration = StereoCalibration(input_folder=calib_foler)
self.block_matcher = StereoSGBM()
self.block_matcher.load_settings(settings=SGBM_setting)
self.CalibratedPair = CalibratedPair(None, self.calibration, self.block_matcher)
self.imageSize = (1280, 720)
self.left_img = None
self.right_img = None
def setup_full_operation(self):
print(
'WARNING: This step should be run after calibration is successful.'
)
self._stereo_rig = CalibratedPair(
None, StereoCalibration(input_folder='calib'), StereoBM())
class NavAid(object):
def __init__(self):
# Setup face detector.
self._detector = FaceDetector()
self._speaker = Speaker()
print('=' * 80 + '\nSetting up camera feed...')
self._cameras = StereoPair([1, 2])
print('-' * 80 + '\nNavAid property initialized. Ready to move!')
self.stereoMatcher = cv.StereoSGBM_create()
# self.stereoMatcher = cv2.StereoBM_create()
self.stereoMatcher.setMinDisparity(0)
self.stereoMatcher.setNumDisparities(256)
self.stereoMatcher.setBlockSize(8)
# self.stereoMatcher.setROI1(leftROI)
# self.stereoMatcher.setROI2(rightROI)
self.stereoMatcher.setSpeckleRange(3)
self.stereoMatcher.setSpeckleWindowSize(8)
def run(self, full_operation_mode=False):
if full_operation_mode:
self.setup_full_operation()
plt.ion()
while True:
left_image, right_image = self._cameras.get_frames()
if left_image is not None and right_image is not None:
left_imgage, right_image = self.anonymize(
left_image, right_image)
cv.imshow('Left', left_image)
cv.imshow('Right', right_image)
if full_operation_mode:
distances = self.process_depth_view(
left_image, right_image)
self.send_instructions(distances)
self._speaker._engine.runAndWait()
ch = cv.waitKey(1)
if ch == 27:
break
cv.destroyAllWindows()
def setup_full_operation(self):
print(
'WARNING: This step should be run after calibration is successful.'
)
self._stereo_rig = CalibratedPair(
None, StereoCalibration(input_folder='calib'), StereoBM())
def process_depth_view2(self, left, right):
# Rectify images.
rectified_pair = self._stereo_rig.calibration.rectify([left, right])
cv.imshow('rect_left', rectified_pair[0])
cv.imshow('rect_right', rectified_pair[1])
points, disparity = self._stereo_rig.get_point_cloud(rectified_pair)
points = points.filter_infinity()
# points.write_ply('output/pcd.ply')
# DEPTH_VISUALIZATION_SCALE = 2048
# cv.imshow('depth', disparity / DEPTH_VISUALIZATION_SCALE)
cv.imshow('depth', disparity)
plt.imshow(disparity)
# plt.show()
def process_depth_view(self, left, right):
grayLeft = cv.cvtColor(left, cv.COLOR_BGR2GRAY)
grayRight = cv.cvtColor(right, cv.COLOR_BGR2GRAY)
depth = self.stereoMatcher.compute(grayLeft, grayRight)
selem = disk(4)
result = dilation(depth, selem)
result = erosion(result, selem)
plt.imshow(result, cmap='viridis')
distances = np.ones_like(result) * 10000.0
non_zero = result > 0.0
distances[non_zero] = (F * T) / result[non_zero]
# for i in range(result.shape[0]):
# for j in range(result.shape[1]):
# if result[i, j] > 0.0:
# distances[i, j] = (F * T) / result[i, j]
# else:
# result[i, j] = 10000.0
return distances
def send_instructions(self, depth_view):
# Convert depth to instruction
print('Distances', depth_view)
if (depth_view < 2.0).count():
instruction = WARNING + PAUSE + ' obstacle ' + AHEAD
self._speaker.say_direction(instruction)
else:
self._speaker.say_direction('All clear ahead.')
return True
def anonymize(self, left, right):
left, left_faces = self._detector.find_faces_haar(left)
left = self._detector.pixelate_faces(left, left_faces)
right, right_faces = self._detector.find_faces_haar(right)
right = self._detector.pixelate_faces(right, right_faces)
return left, right
def prepare_to_calibrate(self, num_pictures, output_folder='output'):
progress = ProgressBar(maxval=num_pictures,
widgets=[Bar("=", "[", "]"), " ",
Percentage()])
if not os.path.exists(output_folder):
os.makedirs(output_folder)
progress.start()
with ChessboardFinder((1, 2)) as pair:
for i in range(num_pictures):
frames = pair.get_chessboard(9, 7, True)
for side, frame in zip(("left", "right"), frames):
number_string = str(i + 1).zfill(len(str(num_pictures)))
filename = "{}_{}.png".format(side, number_string)
output_path = os.path.join(output_folder, filename)
cv.imwrite(output_path, frame)
progress.update(progress.maxval - (num_pictures - i))
for i in range(10):
pair.show_frames(1)
progress.finish()
def calibrate(self, output_folder='output', calibration_folder='calib'):
# Make the calibration step.
print('>' * 5 + ' Performing calibration....')
parser = argparse.ArgumentParser()
args = parser.parse_args()
args.rows = 7
args.columns = 9
args.square_size = 2.2
args.input_files = find_files(output_folder)
args.output_folder = output_folder
args.calibrate_folder = calibrate_folder
args.show_chessboards = True
calibrate_folder(args)
class Point_cloud:
def __init__(self,
calib_foler='calib_files_test',
SGBM_setting='settings/SGBM'):
self.calibration = StereoCalibration(input_folder=calib_foler)
self.block_matcher = StereoSGBM()
self.block_matcher.load_settings(settings=SGBM_setting)
self.CalibratedPair = CalibratedPair(None, self.calibration,
self.block_matcher)
self.imageSize = (1280, 720)
self.left_img = None
self.right_img = None
def load_image(self, leftPath, rightPath):
self.left_img = cv2.imread(leftPath)
self.right_img = cv2.imread(rightPath)
def rectify_image(self):
return self.calibration.rectify([self.left_img, self.right_img])
def get_disparity(self):
imagePair = self.rectify_image()
return self.block_matcher.get_disparity(imagePair)
def get_point_cloud(self, filename):
rectify_pair = self.CalibratedPair.calibration.rectify(
[self.left_img, self.right_img])
points = self.CalibratedPair.get_point_cloud(rectify_pair)
points = points.filter_infinity()
points.write_ply(filename)
def highlight_point_cloud(self, rectangle):
rectify_pair = self.CalibratedPair.calibration.rectify(
[self.left_img, self.right_img])
disparity = self.get_disparity()
points = self.block_matcher.get_3d(disparity,
self.calibration.disp_to_depth_mat)
colors = rectify_pair[0]
#Conver to homogeneous coord
pta = rectangle[0] + (1, )
ptb = rectangle[1] + (1, )
ptc = rectangle[2] + (1, )
ptd = rectangle[3] + (1, )
for row in range(0, self.imageSize[1]):
for col in range(0, self.imageSize[0]):
p = (col, row, 1)
if pointInQuadrilateral(p, pta, ptb, ptc, ptd):
colors[row, col, 0] = 255
colors[row, col, 1] = 0
colors[row, col, 2] = 0
point_cloud = PointCloud(points, colors)
point_cloud.write_ply('test.ply')
def find_pos(self, quadra):
rectify_pair = self.CalibratedPair.calibration.rectify(
[self.left_img, self.right_img])
disparity = self.block_matcher.get_disparity(rectify_pair)
points = self.block_matcher.get_3d(disparity,
self.calibration.disp_to_depth_mat)
#Find the larger rectangle that contains quadrilateral
rmin = min(map(lambda x: x[0], quadra))
rmax = max(map(lambda x: x[0], quadra))
cmin = min(map(lambda x: x[1], quadra))
cmax = max(map(lambda x: x[1], quadra))
point_lst = []
pta = quadra[0] + (1, )
ptb = quadra[1] + (1, )
ptc = quadra[2] + (1, )
ptd = quadra[3] + (1, )
for row in range(rmin, rmax + 1):
for col in range(cmin, cmax + 1):
p = (col, row, 1)
# if pointInQuadrilateral(p, pta, ptb, ptc, ptd):
point_lst.append(points[col, row])
x_list = map(lambda x: x[0], point_lst)
x_list.sort()
y_list = map(lambda x: x[1], point_lst)
y_list.sort()
z_list = map(lambda x: x[2], point_lst)
z_list.sort()
print(len(x_list))
median = (x_list[len(x_list) / 2], y_list[len(y_list) / 2],
z_list[len(z_list) / 3])
return median
from stereovision.stereo_cameras import StereoPair
from stereovision.stereo_cameras import CalibratedPair
from stereovision.calibration import StereoCalibration
from stereovision.blockmatchers import StereoBM
import numpy as np
# NOTE: INCLUDED CALIB FILES ARE FOR THE (LEFT, RIGHT) STEREOPAIR.
# DO NOT USE (LEFT, CENTER)
# camera numbers
devices = (1, 2)
st_pair = StereoPair(devices)
st_bm = StereoBM(stereo_bm_preset=.0,
search_range=80,
window_size=21,
settings=None)
load_calib = StereoCalibration(st_pair, input_folder='calib_leftright')
cal_pair = CalibratedPair(devices, load_calib, st_bm)
cal_pair.live_point_cloud(devices)
np.save("cloudtest.npy", np.ndarray(cal_pair.live_point_cloud(devices)))
import cv2
from stereovision.blockmatchers import StereoBM, StereoSGBM
from stereovision.calibration import StereoCalibration
from stereovision.stereo_cameras import CalibratedPair
from stereovision.ui_utils import STEREO_BM_FLAG
if __name__ == '__main__':
image_pair = [
cv2.imread(image)
for image in ['./test/left_test.jpg', './test/right_test.jpg']
]
calib_folder = 'calibration_result'
use_stereobm = False
bm_settings = 'bm_setting'
if use_stereobm:
block_matcher = StereoBM()
else:
block_matcher = StereoSGBM()
if bm_settings:
block_matcher.load_settings(bm_settings)
camera_pair = CalibratedPair(None,
StereoCalibration(input_folder=calib_folder),
block_matcher)
rectified_pair = camera_pair.calibration.rectify(image_pair)
points = camera_pair.get_point_cloud(rectified_pair)
points = points.filter_infinity()
points.write_ply('./test/test_poc')
class Point_cloud:
def __init__(self, calib_foler = 'calib_files_test', SGBM_setting = 'settings/SGBM'):
self.calibration = StereoCalibration(input_folder=calib_foler)
self.block_matcher = StereoSGBM()
self.block_matcher.load_settings(settings=SGBM_setting)
self.CalibratedPair = CalibratedPair(None, self.calibration, self.block_matcher)
self.imageSize = (1280, 720)
self.left_img = None
self.right_img = None
def load_image(self, leftPath, rightPath):
self.left_img = cv2.imread(leftPath)
self.right_img = cv2.imread(rightPath)
def rectify_image(self):
return self.calibration.rectify([self.left_img, self.right_img])
def get_disparity(self):
imagePair = self.rectify_image()
return self.block_matcher.get_disparity(imagePair)
def get_point_cloud(self, filename):
rectify_pair = self.CalibratedPair.calibration.rectify([self.left_img, self.right_img])
points = self.CalibratedPair.get_point_cloud(rectify_pair)
points = points.filter_infinity()
points.write_ply(filename)
def highlight_point_cloud(self, rectangle):
rectify_pair = self.CalibratedPair.calibration.rectify([self.left_img, self.right_img])
disparity = self.get_disparity()
points = self.block_matcher.get_3d(disparity, self.calibration.disp_to_depth_mat)
colors = rectify_pair[0]
#Conver to homogeneous coord
pta = rectangle[0] + (1,)
ptb = rectangle[1] + (1,)
ptc = rectangle[2] + (1,)
ptd = rectangle[3] + (1,)
for row in range(0, self.imageSize[1]):
for col in range(0, self.imageSize[0]):
p = (col, row, 1)
if pointInQuadrilateral(p, pta, ptb, ptc, ptd):
colors[row, col, 0] = 255
colors[row, col, 1] = 0
colors[row, col, 2] = 0
point_cloud = PointCloud(points, colors)
point_cloud.write_ply('test.ply')
def find_pos(self, quadra):
rectify_pair = self.CalibratedPair.calibration.rectify([self.left_img, self.right_img])
disparity = self.block_matcher.get_disparity(rectify_pair)
points = self.block_matcher.get_3d(disparity,
self.calibration.disp_to_depth_mat)
#Find the larger rectangle that contains quadrilateral
rmin = min(map(lambda x: x[0], quadra))
rmax = max(map(lambda x: x[0], quadra))
cmin = min(map(lambda x: x[1], quadra))
cmax = max(map(lambda x: x[1], quadra))
point_lst = []
pta = quadra[0] + (1,)
ptb = quadra[1] + (1,)
ptc = quadra[2] + (1,)
ptd = quadra[3] + (1,)
for row in range(rmin, rmax + 1):
for col in range(cmin, cmax + 1):
p = (col, row, 1)
# if pointInQuadrilateral(p, pta, ptb, ptc, ptd):
point_lst.append(points[col, row])
x_list = map(lambda x: x[0], point_lst)
x_list.sort()
y_list = map(lambda x: x[1], point_lst)
y_list.sort()
z_list = map(lambda x: x[2], point_lst)
z_list.sort()
print(len(x_list))
median = (x_list[len(x_list) / 2], y_list[len(y_list) / 2], z_list[len(z_list) / 3])
return median
from stereovision.stereo_cameras import StereoPair
from stereovision.stereo_cameras import CalibratedPair
from stereovision.calibration import StereoCalibration as StereoCalibration
from stereovision.blockmatchers import BlockMatcher as Blockmatcher
file_path = 'calib'
calibration = StereoCalibration(input_folder="../../calib/")
devices = (1, 2)
st_pair = StereoPair(devices)
bl_match = Blockmatcher(calibration)
cal_pair = CalibratedPair(st_pair, calibration, bl_match)
if calibration:
calibration.export("calibrated_cameras")
