Skip to content

Instantly share code, notes, and snippets.

@ahojnnes

ahojnnes/camera.py

Forked from bdholt1/camera.py
Created August 14, 2012 10:20
Show Gist options
  • Star 0 You must be signed in to star a gist
  • Fork 0 You must be signed in to fork a gist
  • Save ahojnnes/3348005 to your computer and use it in GitHub Desktop.
Save ahojnnes/3348005 to your computer and use it in GitHub Desktop.
Download ZIP
Camera class
Raw
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)
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment