camera_realworldxyz.py

import numpy as np
import cv2
import image_recognition_singlecam

class camera_realtimeXYZ:

    #camera variables
    cam_mtx=None
    dist=None
    newcam_mtx=None
    roi=None
    rvec1=None
    tvec1=None
    R_mtx=None
    Rt=None
    P_mtx=None

    #images
    img=None


    def __init__(self):

        imgdir="/home/pi/Desktop/Captures/"
        savedir="camera_data/"
        self.imageRec=image_recognition_singlecam.image_recognition(False,False,imgdir,imgdir,False,True,False)

        #self.imageRec=image_recognition_singlecam.image_recognition(True,False,imgdir,imgdir,True,True)

        self.cam_mtx=np.load(savedir+'cam_mtx.npy')
        self.dist=np.load(savedir+'dist.npy')
        self.newcam_mtx=np.load(savedir+'newcam_mtx.npy')
        self.roi=np.load(savedir+'roi.npy')
        self.rvec1=np.load(savedir+'rvec1.npy')
        self.tvec1=np.load(savedir+'tvec1.npy')
        self.R_mtx=np.load(savedir+'R_mtx.npy')
        self.Rt=np.load(savedir+'Rt.npy')
        self.P_mtx=np.load(savedir+'P_mtx.npy')

        s_arr=np.load(savedir+'s_arr.npy')
        self.scalingfactor=s_arr[0]

        self.inverse_newcam_mtx = np.linalg.inv(self.newcam_mtx)
        self.inverse_R_mtx = np.linalg.inv(self.R_mtx)
    
    def previewImage(self, text, img):
        #show full screen
        cv2.namedWindow(text, cv2.WND_PROP_FULLSCREEN)
        cv2.setWindowProperty(text,cv2.WND_PROP_FULLSCREEN,cv2.WINDOW_FULLSCREEN)

        cv2.imshow(text,img)
        cv2.waitKey(2000)
        cv2.destroyAllWindows()

    def undistort_image(self,image):
        image_undst = cv2.undistort(image, self.cam_mtx, self.dist, None, self.newcam_mtx)

        return image_undst

    def load_background(self,background):
        self.bg_undst=self.undistort_image(background)
        self.bg=background

    def detect_xyz(self,image,calcXYZ=True,calcarea=False):

        image_src=image.copy()
        
        #if calcXYZ==True:
        #    img= self.undistort_image(image_src)
        #    bg = self.bg_undst
        #else:
        img=image_src
        bg=self.bg
                    
        XYZ=[]
        #self.previewImage("capture image",img_undst)
        #self.previewImage("bg image",self.bg_undst)
        obj_count, detected_points, img_output=self.imageRec.run_detection(img,self.bg)

        if (obj_count>0):

            for i in range(0,obj_count):
                x=detected_points[i][0]
                y=detected_points[i][1]
                w=detected_points[i][2]
                h=detected_points[i][3]
                cx=detected_points[i][4]
                cy=detected_points[i][5]

                cv2.rectangle(img,(x,y),(x+w,y+h),(0,255,0),2)
                
                #draw center
                cv2.circle(img,(cx,cy),3,(0,255,0),2)

                
                cv2.putText(img,"cx,cy: "+str(self.truncate(cx,2))+","+str(self.truncate(cy,2)),(x,y+h+28),cv2.FONT_HERSHEY_SIMPLEX,0.5,(0,255,0),2)
                if calcXYZ==True:
                    XYZ.append(self.calculate_XYZ(cx,cy))
                    cv2.putText(img,"X,Y: "+str(self.truncate(XYZ[i][0],2))+","+str(self.truncate(XYZ[i][1],2)),(x,y+h+14),cv2.FONT_HERSHEY_SIMPLEX,0.5,(0,255,0),2)
                if calcarea==True:
                    cv2.putText(img,"area: "+str(self.truncate(w*h,2)),(x,y-12),cv2.FONT_HERSHEY_SIMPLEX,0.5,(0,255,0),2)

        return img, XYZ

    def calculate_XYZ(self,u,v):
                                      
        #Solve: From Image Pixels, find World Points

        uv_1=np.array([[u,v,1]], dtype=np.float32)
        uv_1=uv_1.T
        suv_1=self.scalingfactor*uv_1
        xyz_c=self.inverse_newcam_mtx.dot(suv_1)
        xyz_c=xyz_c-self.tvec1
        XYZ=self.inverse_R_mtx.dot(xyz_c)

        return XYZ


    def truncate(self, n, decimals=0):
        n=float(n)
        multiplier = 10 ** decimals
        return int(n * multiplier) / multiplier