집밖은 위험해

--- 추가 ---

이전에 ORB SLAM을 보면서

단안 카메라 만으로도 시각 궤적 트래킹을 그래도 간단히 할수 있을줄 알았는데,

그렇지 않더라

단안 영상에서는 매칭쌍으로 회전 행렬과 평행이동 백터를 구할수는 있지만

스케일 정보가 없어 정확한 시각 궤적을 구할수가 없었다.

ORB SLAM의 경우에는 키프레임들을 잘 최적화시켜서 가능했었는데

g2o를 커녕

파이썬 기본 프로젝트 개념 조차 제대로 정리못하니 아직도 손댈수가 없겠다.

-------------------

기존에 단안 카메라로 시각 궤적을 구하는 코드를 공개해놓은게 있었다

이 자료는 KITTI dataset에 맞게 되어있어서

zumi에서 사용할수 있도록 조금 수정해야 될거같다.

https://github.com/uoip/monoVO-python

봐야 될 코드는

test.py와 visual_odometry.py 2개로

test.py에는 KITTI dataset 테스트 코드가

visual_odometry.py에는 핀홀 카메라와 시각 궤적에 대한 코드가 정의되어 있다.

조금전에 카메라 캘리브레이션으로 카메라 매트릭스를 구해서

핀홀 카메라 모델 초기화에 필요한 값들은 가지고 있다.

위 결과를 보면

핀홀 카메라 초기화에 사용할 값들은

self.fx = 307.36614763

self.fy = 307.93093352

self.cx = 163.09438444

self.cy = 121.38304299

self.d = distortion coeff

핀홀 카메라는 괜찬아 보이지만

시각 궤적이 문제다.

현재 R, t와 카메라 주점을 떠나서

어노테이션 파일이 없는게 큰데

일단 test.py를 보면

핀홀 카메라를 초기화하고

시각 궤적 초기화에는

핀홀 카메라 모델, 어노테이션 파일이 파라미터로 주어야한다.

나머지는

1. 이미지 읽기

2. vo.update()

3. 궤적 그리기

로 정리할수 있을것같다.

일단 vo.update부터 돌아가서보면

이미지가 첫번째, 두번째, 이외의 경우를 나눠서 처리하고 있다.

첫 이미지 처리 과정을 보면

1. 특징 검출기로 기준 키포인트부터 구한다.

* detector.detect()에서는 검출한 키포인트들을 반환

2. 키포인트의 좌표들을 넘파이 배열에담는다.  

* https://docs.opencv.org/3.4/d2/d29/classcv_1_1KeyPoint.html#ae6b87d798d3e181a472b08fa33883abe

* x.pt는 point2f 특징 좌표

=> 첫번째 이미지의 특징 좌표들을 기준 좌표로 설정하는 과정

두번째 이미지 처리 과정에서는

1. 피처 트래킹 -> featureTracking

2. 에센셜 메트릭스 검출 -> findEssentialMat

3. 자세 복원 -> recoverPose

4. 현재 특징 좌표를 기준 좌표로 -> px_ref = px_cur

우선 피처 트래킹 부터 살펴보면

* 두번째 프레임을 받았다고 가정

last_frame 이전 영상(첫번째 프레임)

new_frame 새 영상(두번째 프레임)

prx_ref 이전 영상(첫번째 프레임)의 특징 좌표

이제 피처 트래킹 함수 로직을 살펴보자

피처 프래킹을 보면

가장 먼저 옵티컬 플로우를 계산한다.

옵티컬 플로우 : 두 연속 이미지 사이에서 풀체의 움직이는 패턴 검출

ref : https://throwexception.tistory.com/683

* opencv-python 에서도 옵티컬 플로우 설명이 있다.

ref : https://opencv-python-tutroals.readthedocs.io/en/latest/py_tutorials/py_video/py_lucas_kanade/py_lucas_kanade.html

* 옵티컬 플로우 번역 설명

ref : https://m.blog.naver.com/samsjang/220662493920

cv2.calcOpticalFlowPyrLK 함수의 리턴 값은

kp2 : 현재 영상에서의 특징 키포인트

st : 검출 성공여부 ( 0이면 실패, 1이면 성공)

이전과 현재 영상의 매치되는 특징들을 찾아낸뒤

에센셜 메트릭스를 구하고 있습니다.

에센셜 메트릭스 : 두 영상에서 찾은 특징쌍, 매칭쌍의 변환 관계(평행이동, 회전).(카메라 내부 파라미터 제거된 좌표계)

* fundamental matrix : 에센셜 메트릭스와 동일하나 카메라 내부 파라미터 행렬을 포함한 좌표계서 변환 관계

ref : https://darkpgmr.tistory.com/83

리커버포즈 : 말그대로 자세 변화를 구해주는 함수

- 입력 : 에센셜 메트릭스, 이미지1 특징 좌표, 이미지2 특징 좌표, 카메라 행렬, 회전 변환 출력 행렬, 평행이동변환 출력행렬

=> 두 이미지 사이의 자세 변화를 구함

다시 세컨드 프레임 프로세스로 돌아오면

현재 R 변화와 T 변화를 구하고

이후에는 기본 프레임 단계로 지정된다.

나머지 내용 잠깐 리커버 포즈부터 해보고 진행한다.

모든 카메라에는

이미지 센서와 렌즈사이는 정확하게 나란하지 않아 약간의 이미지 왜곡이 발생한다.

이 왜곡에 대한 행렬을 camera matrix라 한다.

camera matrix는 카매라 내부 (왜곡 정도에 대한) 행렬과

카메라 이미지 평면 ~ 외부 물체에 대한 회전, 평행이동에 대한 백터로 이루어진다.

카메라 캘리브레이션 카메라 왜곡 행렬 camera matrix를 찾아주는 작업이다.

스테레오 비전 같은 3차원 복원을 한다면 이 카메라 왜곡을 잡아주어야 한다.

자세한 설명은 다음 링크에서

ref : https://darkpgmr.tistory.com/32

카매라 캘리브레이션은 체스판이 있으면 할수 있는데,

테블릿에다 채스보드를 띄워서 검출했다.

ref : https://opencv-python-tutroals.readthedocs.io/en/latest/py_tutorials/py_calib3d/py_calibration/py_calibration.html#calibration

카메라 매트릭스와 왜곡 계수를 구하였으면

이 값으로 왜곡된 카메라 영상을 보정시킬수가 있다.

import numpy as np
import cv2
import glob
import pickle
import time

#https://opencv-python-tutroals.readthedocs.io/en/latest/py_tutorials/py_calib3d/py_calibration/py_calibration.html
#https://stackoverflow.com/questions/6568007/how-do-i-save-and-restore-multiple-variables-in-python

#This should be as close to zero as possible
def reprojection_error(imgpoints, objpoints, mtx, dist, rvecs, tvecs):
    mean_error = 0
    for i in range(len(objpoints)):
        imgpoints2, _ = cv2.projectPoints(objpoints[i], rvecs[i], tvecs[i], mtx, dist)
        error = cv2.norm(imgpoints[i],imgpoints2, cv2.NORM_L2)/len(imgpoints2)
        mean_error = mean_error + error

    print("total error: " + str(mean_error/len(objpoints)))
    return mean_error



# termination criteria
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)

# prepare object points, like (0,0,0), (1,0,0), (2,0,0) ....,(6,5,0)
objp = np.zeros((4*6,3), np.float32)
objp[:,:2] = np.mgrid[0:6,0:4].T.reshape(-1,2)

# Arrays to store object points and image points from all the images.
objpoints = [] # 3d point in real world space
imgpoints = [] # 2d points in image plane.








cap = cv2.VideoCapture(0)
while(len(objpoints) < 20):
    ret, frame = cap.read()
    frame = cv2.flip(frame, -1)
    rsz = cv2.resize(frame, dsize=(320,240))
    gray = cv2.cvtColor(rsz, cv2.COLOR_BGR2GRAY)


    # Find the chess board corners
    ret, corners = cv2.findChessboardCorners(gray, (6,4),None)

    # If found, add object points, image points (after refining them)
    if ret == True:
        objpoints.append(objp)

        corners2 = cv2.cornerSubPix(gray,corners,(11,11),(-1,-1),criteria)
        imgpoints.append(corners2)

        # Draw and display the corners
        gray = cv2.drawChessboardCorners(gray, (6,4), corners2,ret)
        print("chessobard corner detected. curr num objpoints : " + str(len(objpoints)) +  ", curr num imgpoints : " + str(len(imgpoints)))

        time.sleep(0.2)

    cv2.imshow('res',gray)
    if cv2.waitKey(20) & 0xFF == ord('q'):
        break


ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints, gray.shape[::-1],None,None)


reprojection_error(imgpoints, objpoints, mtx, dist, rvecs, tvecs)


cap.release()
cv2.destroyAllWindows()


print("camera matrix")
print(mtx)
print("distortion coeff")
print(dist)


# Saving the objects:
with open('cam_calib.pkl', 'wb') as f:
    pickle.dump([mtx, dist, rvecs, tvecs], f)

지금 사용하는 파이 카메라는 내부 왜곡이 크지 않아서인지 보정 전과 후에도 큰 차이가 없다.

실제 이미지 왜곡은 다음 링크를 참조하는게 좋을것같다.

ref : https://medium.com/analytics-vidhya/camera-calibration-with-opencv-f324679c6eb7

import numpy as np
import cv2
import glob
import pickle

def get_cameramat_dist(filename):

    f = open(filename, 'rb')
    mat, dist, rvecs, tvecs = pickle.load(f)
    f.close()

    print("camera matrix")
    print(mat)
    print("distortion coeff")
    print(dist)
    return mat,dist





def main():

    mat, dist = get_cameramat_dist("cam_calib.pkl")

    cap = cv2.VideoCapture(0)

    ret, frame = cap.read()
    frame = cv2.flip(frame, -1)
    rsz = cv2.resize(frame, dsize=(320,240))
    gray = cv2.cvtColor(rsz, cv2.COLOR_BGR2GRAY)


    h,  w = gray.shape[:2]
    newcameramtx, roi=cv2.getOptimalNewCameraMatrix(mat,dist,(w,h),1,(w,h))



    while(True):
        ret, frame = cap.read()
        frame = cv2.flip(frame,-1)
        rsz = cv2.resize(frame, dsize=(320,240))
        gray = cv2.cvtColor(rsz, cv2.COLOR_BGR2GRAY)

        # undistort
        mapx,mapy = cv2.initUndistortRectifyMap(mat,dist,None,newcameramtx,(w,h),5)
        res = cv2.remap(gray,mapx,mapy,cv2.INTER_LINEAR)

        # crop the image
        x,y,w,h = roi
        res = res[y:y+h, x:x+w]

        cv2.imshow('res',res)
        if cv2.waitKey(20) & 0xFF == ord('q'):
            break

    cap.release()
    cv2.destroyAllWindows()



if __name__ == "__main__":
    main()

ref : https://opencv-python-tutroals.readthedocs.io/en/latest/py_tutorials/py_feature2d/py_feature_homography/py_feature_homography.html

ref : https://throwexception.tistory.com/838?category=873104

import numpy as np
import cv2
from matplotlib import pyplot as plt



# Initiate
MIN_MATCH_COUNT = 10
cap = cv2.VideoCapture(0)


#FLANN Feature Matcher & Param
index_params = dict(algorithm=6,
                    table_number=6,
                    key_size=12,
                    multi_probe_level=2)
search_params = {}
flann = cv2.FlannBasedMatcher(index_params, search_params)



# Initiate STAR detector
orb = cv2.ORB_create()

img1 = cv2.imread('../../../res/manual.png',0)
# find the keypoints with ORB
kp1 = orb.detect(img1,None)
kp1, des1 = orb.compute(img1, kp1)





while(True):
    ret, frame = cap.read()
    frame = cv2.flip(frame, -1)
    rsz = cv2.resize(frame, dsize=(320,240))
    res = cv2.cvtColor(rsz, cv2.COLOR_BGR2GRAY)
    kp2 = orb.detect(res,None)
    kp2, des2 = orb.compute(res, kp2)




    try:
        matches = flann.knnMatch(des1, des2, k=2)
        good = []
        for i,(m,n) in enumerate(matches):
            if m.distance < 0.85*n.distance:
                good.append(m)


        if len(good)>MIN_MATCH_COUNT:
            src_pts = np.float32([ kp1[m.queryIdx].pt for m in good ]).reshape(-1,1,2)
            dst_pts = np.float32([ kp2[m.trainIdx].pt for m in good ]).reshape(-1,1,2)

            M, mask = cv2.findHomography(src_pts, dst_pts, cv2.RANSAC,5.0)
            matchesMask = mask.ravel().tolist()

            h,w = img1.shape
            pts = np.float32([ [0,0],[0,h-1],[w-1,h-1],[w-1,0] ]).reshape(-1,1,2)
            dst = cv2.perspectiveTransform(pts,M)

            res = cv2.polylines(res,[np.int32(dst)],True,255,3, cv2.LINE_AA)

        else:
            #print("Not enough matches are found - %d/%d" % (len(good),MIN_MATCH_COUNT))
            matchesMask = None

        draw_params = dict(matchColor = (0,255,0), # draw matches in green color
                        singlePointColor = None,
                        matchesMask = matchesMask, # draw only inliers
                        flags = 2)
        res = cv2.drawMatches(img1,kp1,res,kp2,good,None,**draw_params)

    except Exception as e:
        print(e)

    cv2.imshow('res',res)
    if cv2.waitKey(20) & 0xFF == ord('q'):
        break

cap.release()
cv2.destroyAllWindows()

원래 주미에는 opencv 3.4.3(?) 버전이 설치되어있는데

이상하게 피처 매치 부분에서 계속 버그가 나더라

아무리 삽질해봐도 도저히 고칠수 없어서 다른 버전을 사용하려고 했다.

그래서 opencv를 업그레이드 하겠다고 한참 삽질을 하다가

whl 파일 설치해서 마무리하고

다시 ORB 특징을 이용한 MPU6050 피처 매치

좌측의 센서 사진은 그냥 폰으로 찍어서 사용했다.

import numpy as np
import cv2

cap = cv2.VideoCapture(0)


#queryImg
#qImg = cv2.imread('./res/mpu6050_2.png',0)
qImg = cv2.imread('../../../res/mpu6050_2.png',0)

# Initiate STAR detector
orb = cv2.ORB_create()
# find the keypoints with ORB
kp1 = orb.detect(qImg,None)
# compute the descriptors with ORB
kp1, desc1 = orb.compute(qImg, kp1)

# create BFMatcher object
bf = cv2.BFMatcher()
print(len(desc1))

while(True):
    ret, frame = cap.read()
    frame = cv2.flip(frame, 0)
    rsz = cv2.resize(frame, dsize=(320,240))
    gray = cv2.cvtColor(rsz, cv2.COLOR_BGR2GRAY)

    # find the keypoints with ORB
    kp2 = orb.detect(gray,None)
    # compute the descriptors with ORB
    kp2, desc2 = orb.compute(gray, kp2)

    # Match descriptors.
    matches = bf.knnMatch(desc1,desc2, k=2)
    # Apply ratio test
    good = []
    for m,n in matches:
        if m.distance < 0.75*n.distance:
            good.append([m])


    gray = cv2.drawMatchesKnn(qImg,kp1,gray,kp2,good, None, flags=2)


    cv2.imshow('res',gray)
    if cv2.waitKey(20) & 0xFF == ord('q'):
        break

cap.release()
cv2.destroyAllWindows()

라즈베리파이에 영상처리를 한다면

opencv 소스를 직접 다운 받고, 설정, 빌드, 인스톨까지 해야되지만

빌드하는데 시간이 너무 오래걸린다..

그런데 이번에 whl 파일이 빌드 하지 않고 사용할수 있게 만든 실행 파일이고,

PyPi가 whl, 소스를 제공하는 저장소인걸 알게 되고

arm 아키텍처에서 pip install opencv-python 할때마다 build wheel을 하니

PyPI에는 암 아키텍처용 whl 파일이 없구나 싶어

"파이썬 패키지 배포하기" 글을 보고

https://rampart81.github.io/post/python_package_publish/

pizero에서 opencv를 빌드해서 PyPi에다가 배포해보려고 했었다.

주미에게 무거운 opencv를 빌드 시킨 동안 

opencv-python 다큐먼트를 보고있었는데

ref: https://pypi.org/project/opencv-python/

ARM 아키텍처용 opencv python 파일은

PyPI이 아니라 piwheels.org에서 제공해준다고 하더라...

마침 주미에서 사용하는 python35- arm6l whl 파일이 제공되니 이걸 바로 다운받으면 된다.

https://www.piwheels.org/project/opencv-python/

piwheel 저장소 사용하는 방법은 홈페이지에 들어가면 바로 나온다.

/etc/pip.conf에 piwheels.org 주소만 등록해주면 되더라

ref : https://www.piwheels.org/

주소를 등록해주고 나니 이제서야 piwheels 저장소에서 검색하더라

그런데 시스템 에러가 발생한다. 

설치할 버전을 지정해주니 잘 설치된다.

파이썬 3.5, arm, opencv 4.1.1 설치 버전 확인해주고

opencv-contrib-python도 설치하자.

이제 번거로운 빌드는 그만해도 되겠지..?

mpu6050은 관성 센서인데

가속도계도 들어있으니

가속도로 속도를 구하고, 속도로 위치를 구할수 있을거라 생각했다.

그래서 우선 x축 가속도 값을 출력해보기도 하고

from zumi.zumi import Zumi
import time
import datetime as pydatetime

def get_now():
    return pydatetime.datetime.now().timestamp()

#initialize
zumi = Zumi()


bf_time = 0
curr_time = 0

def get_offset(zumi, sampling):
    offset = 0
    for i in range(1, sampling):
        acc = zumi.get_acc()
        accx = round(acc[0], 3)
        offset = offset + accx
    
    offset = offset/sampling
    print("offset : " + str(offset))
    return offset

def print_acc(zumi, offset):
    global curr_time, bf_time
    acc = zumi.get_acc()
    acc_x = round(acc[0] - offset, 2)
    curr_time = get_now()
    dt = round(curr_time - bf_time, 3)
    msg = "acc x : "+str(acc_x)  + ", dt : " + str(dt)
    bf_time = curr_time
    print(msg)

def do_something(zumi, offset):
    print_acc(zumi, offset)
    time.sleep(0.1)



try:
    offset = get_offset(zumi, 100)
    while True:
        do_something(zumi, offset)
except KeyboardInterrupt:
    pass

센서 데이터 특정 시간을 타임 스탬프로 출력해서

시간 구간으로 적분해보기도 했다.

더해서 LPF도 써보기는 했으나

from zumi.zumi import Zumi
import time
import datetime as pydatetime

def get_now():
    return pydatetime.datetime.now().timestamp()

#initialize
zumi = Zumi()
vel = 0
alpha = 0.7
bf_time = 0
curr_time = 0
prev_acc = 0
def get_offset(zumi, sampling):
    offset = 0
    for i in range(1, sampling):
        acc = zumi.get_acc()
        accx = round(acc[0], 3)
        offset = offset + accx
    
    offset = offset/sampling
    print("offset : " + str(offset))
    return offset


def LPF(alpha, prev, curr):
    val = alpha * prev + (1-alpha) * curr
    return val

def print_acc(zumi, offset):
    global curr_time, bf_time, vel, prev_acc
    acc = zumi.get_acc()
    acc_x = LPF(alpha, prev_acc,acc[0])
    acc_x = round(acc[0] - offset, 2)

    curr_time = get_now()
    dt = round(curr_time - bf_time, 2)
    vel = vel + acc_x * dt
    vel = round(vel, 2)
    msg = "acc x : "+str(acc_x)  + ", vel : " +str(vel)+", dt : " + str(dt)
    bf_time = curr_time
    prev_acc = acc_x
    print(msg)

def do_something(zumi, offset):
    print_acc(zumi, offset)
    time.sleep(0.1)



try:
    offset = get_offset(zumi, 100)
    bf_time = get_now()
    while True:
        do_something(zumi, offset)
except KeyboardInterrupt:
    pass

가속도계 자체 오차가 너무 심해서 

처음 초기화 당시에는 안정적이더라도

조금만 움직이고 나서는 오차가 점점 누적되어 발산하게 되버리더라

내 딴에는 나름대로는 칼만필터도 써보긴했는데 

과정은 따로 녹화는 안해서 올리진 못한게 아쉬워도 여기까지 하고

그래서 mpu6050에서 가속도계는

자이로 센서로 각 변화율을 구하는데 보조해주기만 하나보더라

ORB 

- FAST + Rotated BRIEF

- 고속 특징 검출이 가능하며 회전 변화에 강인

import numpy as np
import cv2

cap = cv2.VideoCapture(0)
ret, frame = cap.read()
rsz = cv2.resize(frame, dsize=(320,240))
gray = cv2.cvtColor(rsz, cv2.COLOR_BGR2GRAY)
# Initiate ORB detector
orb = cv2.ORB_create()

# find the keypoints with ORB
kp = orb.detect(gray,None)
# compute the descriptors with ORB
kp, desc = orb.compute(gray, kp)
#print("orb kp.shape : " + str(len(kp)) + ", orb desc.shape : " + str(desc.shape))


while(True):
    ret, frame = cap.read()
    frame = cv2.flip(frame, 0)
    rsz = cv2.resize(frame,dsize=(320,240))
    gray = cv2.cvtColor(rsz, cv2.COLOR_BGR2GRAY)

    # find the keypoints with ORB
    kp = orb.detect(gray,None)
    # compute the descriptors with ORB
    kp, desc = orb.compute(gray, kp)

    # draw only keypoints location,not size and orientation
    res = cv2.drawKeypoints(gray,kp,None)

    cv2.imshow('res',res)
    if cv2.waitKey(20) & 0xFF == ord('q'):
        break

cap.release()
cv2.destroyAllWindows()

영상 특징 FAST

ref : https://opencv-python-tutroals.readthedocs.io/en/latest/py_tutorials/py_feature2d/py_fast/py_fast.html#fast

ref : https://throwexception.tistory.com/831?category=873104

import numpy as np
import cv2

cap = cv2.VideoCapture(0)
while(True):
    ret, frame = cap.read()
    frame = cv2.flip(frame, 0)
    rsz = cv2.resize(frame, dsize=(320,240))
    gray = cv2.cvtColor(rsz, cv2.COLOR_BGR2GRAY)

    # Initiate FAST object with default values
    fast = cv2.FastFeatureDetector_create()

    # find and draw the keypoints
    kp = fast.detect(gray,None)
    gray = cv2.drawKeypoints(gray, kp, gray)

    cv2.imshow('res',gray)
    if cv2.waitKey(20) & 0xFF == ord('q'):
        break

cap.release()
cv2.destroyAllWindows()

간단한 1차 저주파 통과 필터

ref : https://gaussian37.github.io/autodrive-ose-low-pass-filter/

저주파 통과 필터 : 잡음 제거 + 변화 민감

from zumi.zumi import Zumi
import numpy as np
import matplotlib.pyplot as plt
from matplotlib.animation import FuncAnimation
import time

data_len = 100
sampling_len = 10
alpha = 0.5


def LPF(acc_xf_lst, sampling_len, alpha, acc_x):
    prevX = acc_xf_lst[len(acc_xf_lst)-sampling_len:len(acc_xf_lst)-1].sum()/sampling_len
    acc_xf = alpha * prevX + (1-alpha)*acc_x
    acc_xf = round(acc_xf, 3)
    return acc_xf


def data_insert(acc_x_lst, acc_x):
    acc_x_lst = np.append(acc_x_lst, acc_x)
    acc_x_lst = np.delete(acc_x_lst, 0)
    return acc_x_lst

def filter_data_insert(acc_xf_lst,  alpha, acc_x):
    acc_xf_lst = np.delete(acc_xf_lst, 0)
    acc_xf = LPF(acc_xf_lst, sampling_len, alpha, acc_x)
    acc_xf_lst = np.append(acc_xf_lst, acc_xf)
    return acc_xf_lst





def init():
    idx = 0
    acc_x_lst = np.zeros(data_len)
    acc_xf_lst = np.zeros(data_len)

    t = np.linspace(0, 1, data_len)

    while idx < data_len:
        acc = zumi.get_acc()
        acc_x = round(acc[0], 3)
        acc_x_lst = data_insert(acc_x_lst, acc_x)
        acc_xf_lst = filter_data_insert(acc_xf_lst, alpha, acc_x)
    
        idx = idx + 1
    return t, acc_x_lst, acc_xf_lst

def update(i):
    global acc_x_lst, acc_xf_lst
    acc = zumi.get_acc()
    acc_x = round(acc[0], 3)

    acc_x_lst = data_insert(acc_x_lst, acc_x)
    acc_xf_lst = filter_data_insert(acc_xf_lst, alpha, acc_x)

    ln0.set_data(t, acc_x_lst)
    ln1.set_data(t, acc_xf_lst)
    return ln0, ln1



zumi = Zumi()
t, acc_x_lst, acc_xf_lst = init()
fig, ax = plt.subplots(2,1)


ln0, = ax[0].plot(t,acc_x_lst, 'r')
ax[0].grid(True)
ax[0].set_title("acc x")
ax[0].set_ylim((-0.2,0.2))
ln1, = ax[1].plot(t,acc_xf_lst, 'g')
ax[1].grid(True)
ax[1].set_title("acc x LPF")
ax[1].set_ylim((-0.2,0.2))



ani = FuncAnimation(fig, update, frames=t, blit=True)
plt.show()

x축 가속도 데이터에 평균 이동 필터를 적용하겠습니다.

ref : https://gaussian37.github.io/autodrive-ose-average-filter/

* 필터 처리에 관하여 "칼만 필터는 어렵지 않아" 추천합니다.

평균 내는 길이를 5로 한 코드입니다.

from zumi.zumi import Zumi
import numpy as np
import matplotlib.pyplot as plt
from matplotlib.animation import FuncAnimation
import time

data_len = 100
mean_len = 5

def init():
    idx = 0
    acc_x_lst = np.zeros(data_len)
    acc_xf_lst = np.zeros(data_len)

    t = np.linspace(0, 1, data_len)

    while idx < data_len:
        acc = zumi.get_acc()
        acc_x = round(acc[0], 3)
        acc_x_lst = np.append(acc_x_lst, acc_x)
        acc_x_lst = np.delete(acc_x_lst, 0)


        acc_xf_lst = np.delete(acc_xf_lst, 0)
        mean_bf = ((mean_len - 1) / mean_len) * (acc_xf_lst[len(acc_xf_lst)-mean_len:len(acc_xf_lst)-1].sum()/(mean_len-1))
        acc_xf = round(mean_bf + 1/mean_len * acc_x, 3)
        #print("acc_x : " + str(acc_x) + ",  acc_xf : " + str(acc_xf))
        acc_xf_lst = np.append(acc_xf_lst, acc_xf)
        idx = idx + 1
    return t, acc_x_lst, acc_xf_lst

def update(i):
    global acc_x_lst, acc_xf_lst
    acc = zumi.get_acc()
    acc_x = round(acc[0], 3)
    acc_x_lst = np.append(acc_x_lst, acc_x)
    acc_x_lst = np.delete(acc_x_lst, 0)


    acc_xf_lst = np.delete(acc_xf_lst, 0)
    mean_bf = ((mean_len - 1) / mean_len) * (acc_xf_lst[len(acc_xf_lst)-mean_len:len(acc_xf_lst)-1].sum()/(mean_len-1))
    acc_xf = round(mean_bf + 1/mean_len * acc_x, 3)
    acc_xf_lst = np.append(acc_xf_lst, acc_xf)
    #print("acc_x : " + str(acc_x) + ",  acc_xf : " + str(acc_xf))
    ln0.set_data(t, acc_x_lst)
    ln1.set_data(t, acc_xf_lst)
    return ln0, ln1



zumi = Zumi()
t, acc_x_lst, acc_xf_lst = init()
fig, ax = plt.subplots(2,1)


ln0, = ax[0].plot(t,acc_x_lst, 'r')
ax[0].grid(True)
ax[0].set_title("acc x")
ax[0].set_ylim((-0.5,0.5))
ln1, = ax[1].plot(t,acc_xf_lst, 'g')
ax[1].grid(True)
ax[1].set_title("acc x average filtered")
ax[1].set_ylim((-0.5,0.5))



ani = FuncAnimation(fig, update, frames=t, blit=True)
plt.show()

평균 필터 적용시

필터 처리 후 변화에 둔감