Camera class

camera.py

import numpy as np
from scipy import linalg

def RawDepthToMeters(depthValue):
    """
    Convert a kinect depth value to meters
    """

    if (depthValue < 2047):
        return float(1.0 / (float(depthValue) * -0.0030711016 + 3.3309495161))
    return 0.0

class PointCloudException:
    pass

class Camera:
    
    def __init__(self, f_x, f_y, c_x, c_y, baseline):
        """ 
        Intrinsic parameters are:
        Centre X = from 0 to 1, equal to (principle point x pos in pixels / image width)
        Centre Y = from 0 to 1, equal to (principle point y pos in pixels / image height)
        Focal X = focal distance in pixel widths * 1000. 
        Focal Y = focal distance in pixel heights * 1000. 
        Baseline = Distance between cameras in meters
        N.B. - Focal lengths should probably be 0.5-2, if they are several hundred they likely need to be divided by image_height / image_width        
        """
        self.f_x = float(f_x)
        self.f_y = float(f_y)
        self.c_x = float(c_x)
        self.c_y = float(c_y)
        # throw away baseline, we don't need it
        
        self.extrinsics = False
        self.rot_mat = None
        self.inverse_rot_mat = None
        self.trans_mat = None
        
    def set_extrinsics(self, rot_mat, trans_mat):
        self.rot_mat = rot_mat
        if rot_mat is not None:
            self.inverse_rot_mat = linalg.pinv(rot_mat)
        self.trans_mat = trans_mat
        self.extrinsics = True
    
    def set_resolution(self, cols, rows):
        self.cols = float(cols)
        self.rows = float(rows)
    
    def worldToImagePlane(self, points3D):
        """
        Convert a list of world coordinates to (x,y,depth)
        and populate an image 
        """          
        
        image = np.zeros((self.rows, self.cols), dtype=np.float32)

        for point in points3D:
            #print "worldToImagePlane ", point
            try:
                imageplane = \
                    self.worldToImagePlane_single(point[0], point[1], point[2])
                #print "worldToImagePlane setting ", imageplane[0], imageplane[1]
                #print "    to ", imageplane[2]
                image[imageplane[0], imageplane[1]] = 1
            except PointCloudException:
                # don't use a point that isn't valid 
                pass
        
        print "worldToImagePlane", image
        
        return image
    
    def worldToImagePlane_single(self, world_x, world_y, world_z):
        """
        Convert real world coordinates to (x,y,depth) 
        """        
        
        world3DPoint =  np.array([world_x, world_y, world_z], dtype=np.float32)
        #print "worldToImagePlane_single ", world_x, world_y, world_z
        if self.extrinsics:
            # the image points are in a different coordinate frame
            # use the extrinsic to convert to this coordinate frame
            world3DPoint = self.__apply_extrinsic(world3DPoint)
        
        #print "worldToImagePlane_single after extrinsic", world3DPoint[0], world3DPoint[1], world3DPoint[2]
            
        (imageplane_x, imageplane_y, imageplane_depth) = \
            self.__project(world3DPoint[0], world3DPoint[1], world3DPoint[2])

        #print "worldToImagePlane_single after project", imageplane_x, imageplane_y, imageplane_depth        
        
        return (imageplane_x, imageplane_y, imageplane_depth)

    def imagePlaneToWorld(self, image):
        """
        Convert an image (x,y,depth) to list of world coordinates
        """          
        
        worldPoints = []
        (rows, cols) = image.shape
        print (rows, cols)
        if (rows != self.rows or cols != self.cols ):
            raise ValueError("image shape must accord with camera resolution!")

        for row in range(rows):
            for col in range(cols):
                #print "row = ", row, " col = ", col, "img = ", image[row, col]
                try:
                    point3D = \
                        self.imagePlaneToWorld_single(col, row, image[row, col])
                    worldPoints.append(point3D)
                except PointCloudException:
                    # don't append a point to 3d that isn't valid 
                    pass
        
        print "imagePlaneToWorld returning ", len(worldPoints), " points"
        return np.array(worldPoints)

    def imagePlaneToWorld_single(self, imageplane_x, imageplane_y, imageplane_depth):
        """
        Convert a kinect (x,y,depth) triple to real world coordinates
        """

        #print "imagePlaneToWorld_single ", imageplane_x, imageplane_y, imageplane_depth
        (world_x, world_y, world_z) = \
            self.__unproject(float(imageplane_x), 
                             float(imageplane_y), 
                             float(imageplane_depth))
        
        #print "imagePlaneToWorld_single after unproject", world_x, world_y, world_z

        world3DPoint =  np.array([world_x, world_y, world_z], dtype=np.float32)
        if self.extrinsics:
            # the image points are in our coordinate frame
            # use the inverse extrinsic to convert to the 
            # other coordinate frame
            world3DPoint = self.__apply_inverse_extrinsic(world3DPoint)
        
        #print "imagePlaneToWorld_single after inverse extrinsic", world3DPoint[0], world3DPoint[1], world3DPoint[2]
        
        return (world3DPoint[0], world3DPoint[1], world3DPoint[2])
    
    def normaliseOpenNIAnnotation(self, openni3DPoint):

        print "normaliseOpenNIAnnotation before ", openni3DPoint
        if self.extrinsics:
            openni3DPoint = self.__apply_extrinsic(openni3DPoint)
        openni3DPoint[0] /= self.cols
        openni3DPoint[1] /= self.rows
        if self.extrinsics:
            openni3DPoint = self.__apply_inverse_extrinsic(openni3DPoint)
        
        print "normaliseOpenNIAnnotation after", openni3DPoint
        
        return openni3DPoint

    def __apply_extrinsic(self, world3DPoint):
        
        #print "__apply_extrinsic to ", world3DPoint
        return np.dot(self.rot_mat, world3DPoint) + self.trans_mat
    
    def __apply_inverse_extrinsic(self, world3DPoint):

        #print "__apply_inverse_extrinsic to ", world3DPoint        
        return  np.dot( self.inverse_rot_mat, (world3DPoint - self.trans_mat))
    
    def __project(self, world_x, world_y, world_z):

        #print "__project ", world_x, world_y, world_z
        #print "c_x = ", self.c_x
        #print "f_x = ", self.f_x
        imageplane_x = self.cols * (world_x * self.f_x / world_z + self.c_x)
        imageplane_y = self.rows * (world_y * self.f_y / world_z + self.c_y)
        imageplane_depth = world_z
        
        if imageplane_x < 0 or imageplane_x >= self.cols or \
            imageplane_y < 0 or imageplane_y >= self.rows or \
            imageplane_depth <= 0:
            print "__project invalid result ", (imageplane_x, imageplane_y, imageplane_depth)
            raise PointCloudException()                 
        
        return (imageplane_x, imageplane_y, imageplane_depth)
    
    def __unproject(self, imageplane_x, imageplane_y, imageplane_depth):

        if imageplane_x < 0 or imageplane_x >= self.cols or \
            imageplane_y < 0 or imageplane_y >= self.rows or \
            imageplane_depth <= 0:
            #print "__unproject invalid arguments"            
            raise PointCloudException()  
        
        world_x = (imageplane_x / self.cols - self.c_x) * float(imageplane_depth) / self.f_x
        world_y = (imageplane_y / self.rows - self.c_y) * float(imageplane_depth) / self.f_y
        world_z = float(imageplane_depth)

        if world_z <= 0: 
            #print "__unproject invalid results"            
            raise PointCloudException()  

        return (world_x, world_y, world_z)

if __name__ == "__main__":

    import Image

    app_cam = Camera(0.52921508098293293 * 1000/640, # f_x 
                     -0.52556393630057437 * 1000/480, # f_y 
                     0.513973, #c_x 
                     0.55725, #c_y 
                     1.0)
    app_cam.set_resolution(640, 480)

    # projection matrix from Nicolas Burrus
    dep_cam = Camera(0.59421434211923247 * 1000, #f_x in meters
                     -0.59104053696870778 * 1000, #f_y in meters (OpenNI uses inverted y-axis)
                     0.339307800 , #c_x is a fraction of the image
                     0.242739136, #c_y is a fraction of the image
                     1.0)    
    dep_cam.set_resolution(640, 480)        
    
    # Rotation matrix taken from Nicolas Burrus page at
    # http://nicolas.burrus.name/index.php/Research/KinectCalibration
    rot_mat = np.array([9.9984628826577793e-01, 1.2635359098409581e-03,
                        -1.7487233004436643e-02, -1.4779096108364480e-03,
                        9.9992385683542895e-01, -1.2251380107679535e-02,
                        1.7470421412464927e-02, 1.2275341476520762e-02,
                        9.9977202419716948e-01 ]).reshape(3,3)
                                                    
    # Translation matrix taken from Nicolas Burrus page at
    # http://nicolas.burrus.name/index.php/Research/KinectCalibration                                
    # the y value is increased to translate the real world coords up
    # prior to projection.
    trans_mat = np.array([0.019985242312, 
                          -0.050744237387616, 
                          -0.01091673633]).reshape(3,) * 1000
                              
    dep_cam.set_extrinsics(rot_mat, trans_mat)