code

[SAgiKPJH]

    start =time.time()
	
    while True: ## 반복문 돌입
        for item in {"A","B"}:
            select_channel(item) ## GPIO 출력 설정
            time.sleep(0.02) ## 0.02초 휴식
            try:
                buf = picam2.capture_array("main",wait=True) ## 이미지 가져오기 (캡쳐)
                buf = picam2.capture_array("main",wait=True)
                buf = cv2.cvtColor(buf, cv2.COLOR_BGR2RGB)
                if item == 'A':
                    cv2.imshow('A_image', buf)
                elif item == 'B':
                    cv2.imshow('B_image', buf)
                    endtime = time.time()-start
                    print("t : ",endtime)
                    print("FPS = ",1/endtime)
                    start = time.time()
                key = cv2.waitKey(1) & 0xff
                if key == 27 :
                  break
            except Exception as e:
                print("capture_buffer: "+ str(e))

[SAgiKPJH]

import numpy as np
from picamera2 import Picamera2
import RPi.GPIO as gp
import time
import os
import cv2

adapter_info = {  
  "A" : {   
      "i2c_cmd":"i2cset -y 10 0x70 0x00 0x01",
      "gpio_sta":[0,0],
  }, "B" : {
      "i2c_cmd":"i2cset -y 10 0x70 0x00 0x02",
      "gpio_sta":[1,0],
  }
}

width = 320
height = 240
angle = -10
M = cv2.getRotationMatrix2D((width / 2, height / 2), angle, 1)

## GPIO 7, 11을 OUTPUT 모드로 설정한다.
gp.setwarnings(False)
gp.setmode(gp.BOARD)
gp.setup(7, gp.OUT)
gp.setup(11, gp.OUT)

picam2 = Picamera2()

## A번 카메라, B번 카메라에 따라 출력을 달리 한다. (00, 01) 
def select_channel(index):
    channel_info = adapter_info.get(index)
    gpio_sta = channel_info["gpio_sta"] # gpio write
    gp.output(7, gpio_sta[0])
    gp.output(11, gpio_sta[1])

## os.system을 통해 직접 cmd에 "i2cset -y 10 0x70 0x00 0x01" or "0x02"를 입력한다.
def init_i2c(index):
    channel_info = adapter_info.get(index)
    os.system(channel_info["i2c_cmd"]) # i2c write

## Thread 실행시
def run():
    global picam2
    flag = False
    for item in {"A","B"}:
        try:
            select_channel(item) ## GPIO 출력 설정
            init_i2c(item) ## i2cset 명령어 실행
            time.sleep(0.5)  ## 0.5초의 휴식기간
            picam2.close()   ## 카메라 종료
            print("init1 "+ item) 
            picam2 = Picamera2() ## 새로운 카메라 객체 선언
            picam2.configure(picam2.create_still_configuration(main={"size": (320, 240), "format": "BGR888"},buffer_count=2))  ## 카메라 속성 설정
            picam2.start() ## 카메라 시작
            picam2.set_controls({"AeEnable":False,"ExposureTime":30000,"AnalogueGain":6}) ## 
            time.sleep(2) ## 2ms 대기
            picam2.capture_array("main",wait=True) ## 캡쳐 테스트
            time.sleep(0.1) ## 0.1ms 대기
        except Exception as e:
            print("except: "+str(e))
            
    start =time.time()
    select_channel("A") ## GPIO 출력
    imageA = picam2.capture_array("main",wait=True)
    while True: ## 반복문 돌입
        for item in {"A","B"}:
            select_channel(item) ## GPIO 출력 설정
            time.sleep(0.002) ## 0.02초 휴식
            try:
                buf = picam2.capture_array("main",wait=True) ## 이미지 가져오기 (캡쳐)
                buf = picam2.capture_array("main",wait=True)
                buf = cv2.cvtColor(buf, cv2.COLOR_BGR2RGB)
                if item == 'A':                
                    buf = cv2.warpAffine(buf, M, (width, height))
                    cv2.imshow('A_image', buf)
                    imageA = buf.copy()
                elif item == 'B':
                    buf = cv2.warpAffine(buf, M, (width, height))
                    cv2.imshow('B_image', buf)
                    cv2.imshow('stereo',StereoBM(imageA, buf))
                    #cv2.imshow('stereo',StereoSGBM(imageA, buf, 16*4, 17))
                    #cv2.imshow('stereo',Census_Transform(imageA, buf, 17, 16*4))
                    endtime = time.time()-start
                    print("t : ",endtime)
                    print("FPS = ",1/endtime)
                    start = time.time()
                key = cv2.waitKey(1) & 0xff
                if key == 27 :
                  break
            except Exception as e:
                print("capture_buffer: "+ str(e))


def transform(image, window_size=3):
    half_window_size = window_size // 2

    image = cv2.copyMakeBorder(image, top=half_window_size, left=half_window_size, right=half_window_size, bottom=half_window_size, borderType=cv2.BORDER_CONSTANT, value=0)
    rows, cols = image.shape
    census = np.zeros((rows - half_window_size * 2, cols - half_window_size * 2), dtype=np.uint8)
    center_pixels = image[half_window_size:rows - half_window_size, half_window_size:cols - half_window_size]

    offsets = [(row, col) for row in range(half_window_size) for col in range(half_window_size) if not row == half_window_size + 1 == col]
    for (row, col) in offsets:
        census = (census << 1) | (image[row:row + rows - half_window_size * 2, col:col + cols - half_window_size * 2] >= center_pixels)
    return census

def column_cost(left_col, right_col):
    return np.sum(np.unpackbits(np.bitwise_xor(left_col, right_col), axis=1), axis=1).reshape(left_col.shape[0], left_col.shape[1])

def cost(left, right, window_size=3, disparity=0):
    ct_left = transform(left, window_size=window_size)
    ct_right = transform(right, window_size=window_size)
    rows, cols = ct_left.shape
    C = np.full(shape=(rows, cols), fill_value=0)
    for col in range(disparity, cols):
        C[:, col] = column_cost(
            ct_left[:, col:col + 1],
            ct_right[:, col - disparity:col - disparity + 1]
        ).reshape(ct_left.shape[0])
    return C

def norm(image):
    return cv2.normalize(image, dst=None, alpha=0, beta=255, norm_type=cv2.NORM_MINMAX).astype(np.uint8)

def StereoBM(imgL,imgR, nD = 16, bS = 17) : # 16, 15
    imgL = cv2.cvtColor(imgL,cv2.COLOR_RGB2GRAY)
    imgR = cv2.cvtColor(imgR,cv2.COLOR_RGB2GRAY)
    stereo = cv2.StereoBM_create(nD, bS)
    stereo.setDisp12MaxDiff(25)
    stereo.setUniquenessRatio(1)
    start = time.time()
    disparity = stereo.compute(imgL,imgR)
    end = time.time()
    print(f"cal {end - start:.5f} sec")
    print(f"fps {1/(end - start):.5f} sec")
    disparity = cv2.normalize(disparity, None, 0, 255, cv2.NORM_MINMAX)
    disparity = np.uint8(disparity)
    return disparity
    # disparity = cv2.cvtColor(disparity, cv2.COLOR_GRAY2RGB)
    #cv2.imshow("stereovisions",disparity)
    #cv2.waitKey(1)
  
def StereoSGBM(imgL,imgR, nD = 16, bS = 15, win_size=3) :
    imgL = cv2.cvtColor(imgL,cv2.COLOR_RGB2GRAY)
    imgR = cv2.cvtColor(imgR,cv2.COLOR_RGB2GRAY)
    stereo = cv2.StereoSGBM_create(
        numDisparities=nD,
        blockSize=bS,
        uniquenessRatio=5,
        speckleWindowSize=5,
        speckleRange=5,
        disp12MaxDiff=25,
        P1=8 * 3 * win_size ** 2,
        P2=32 * 3 * win_size ** 2,
    )
    start = time.time()
    disparity = stereo.compute(imgL,imgR)
    end = time.time()
    print(f"cal {end - start:.5f} sec")
    print(f"fps {1/(end - start):.5f} sec")
    disparity = cv2.normalize(disparity, None, 0, 255, cv2.NORM_MINMAX)
    disparity = np.uint8(disparity)
    return disparity
  
def Census_Transform (left, right, window_size = 5, ndisp = 7):
    left = cv2.cvtColor(left,cv2.COLOR_RGB2GRAY)
    right = cv2.cvtColor(right,cv2.COLOR_RGB2GRAY)
    ct_left = norm(transform(left, window_size)) ## Census Transform
    ct_right = norm(transform(right, window_size))
    ct_costs = [] ## Result
    start = time.time()
    for exponent in range(0, 6):
        import math
        disparity = int(ndisp / math.pow(2, exponent))
        #print(math.pow(2, exponent), disparity)
        ct_costs.append(norm(cost(left, right, window_size, disparity)))
    disparity = ct_costs[0].copy()
    end = time.time()
    print(f"cal {end - start:.5f} sec")
    print(f"fps {1/(end - start):.5f} sec")
    disparity = cv2.normalize(disparity, None, 0, 255, cv2.NORM_MINMAX)
    disparity = np.uint8(disparity)
    return disparity

run()