def transform(self, R, t, scale = 1):
""" Apply rigid 3D transformation to model.
This function is useful to align a MOPED model (arbitrary axes) so that
it corresponds to a known scale and axis. The fields 'pts3D' and
'cam_poses' are updated.
Usage: model.transform (R, t, scale=1)
Input:
R - 3x3 Rotation matrix or 3x1 Rodrigues rotation vector that rotates
from the model coord frame to the desired coord frame.
Alternatively, if only 2 parameters are given, 3-by-4 or 4-by-4
transformation matrices are also accepted.
t - 3x1 Translation vector (model to world translation)
scale - Scaling parameter. Default: 1.
Output:
-NONE- but model is updated in place (fields 'pts3D' and 'cam_poses')
Alternatively, if only 2 parameters are given, 3-by-4 or 4-by-4
transformation matrices are also valid
"""
# Build 4-by-4 projection matrix from args ----------------------------
# This is what we are doing internally:
# Proj = np.r_[ scale * np.c_[R, t], [[0, 0, 0, 1]] ]
# InvProj = np.r_[ scale * np.c_[R.T, -np.dot(R.T, t)], [[0,0,0,scale]] ]
Proj = tf_format.tf_format('4x4', R, t)
Proj[:-1,:] *= scale
InvProj = tf_format.tf_format('i4x4', R, t) * scale
# Apply transformation to pts3D ---------------------------------------
if self.pts3D is not None and self.pts3D.shape[1] > 0:
# Use homogeneous coords
pts3D = np.r_[self.pts3D, np.ones((1, self.pts3D.shape[1]))]
pts3D = np.dot(Proj, pts3D)
self.pts3D = pts3D[:3, :]
# Apply transformation to cameras -------------------------------------
# Camera poses are stored using camera-to-world transformations, we
# need to invert the projection matrix for this to work -->
# we use InvProj
cposes = self.cam_poses
for i in range(cposes.shape[1]):
# Extract camera projection matrix
p_cam = tf_format.tf_format('4x4', cposes[:, i])
# Transform camera projection matrix
new_p_cam = np.dot(p_cam, InvProj)
# Make sure it's a true rotation!
[u, s, vT] = np.linalg.svd(new_p_cam[:3,:3])
cposes[:3, i] = tf_format.rodrigues( np.dot(u,vT) ).ravel()
cposes[3:, i] = new_p_cam[:3, 3]
self.cam_poses = cposes
def transform(self, R, t, scale=1):
""" Apply rigid 3D transformation to model.
This function is useful to align a MOPED model (arbitrary axes) so that
it corresponds to a known scale and axis. The fields 'pts3D' and
'cam_poses' are updated.
Usage: model.transform (R, t, scale=1)
Input:
R - 3x3 Rotation matrix or 3x1 Rodrigues rotation vector that rotates
from the model coord frame to the desired coord frame.
Alternatively, if only 2 parameters are given, 3-by-4 or 4-by-4
transformation matrices are also accepted.
t - 3x1 Translation vector (model to world translation)
scale - Scaling parameter. Default: 1.
Output:
-NONE- but model is updated in place (fields 'pts3D' and 'cam_poses')
Alternatively, if only 2 parameters are given, 3-by-4 or 4-by-4
transformation matrices are also valid
"""
# Build 4-by-4 projection matrix from args ----------------------------
# This is what we are doing internally:
# Proj = np.r_[ scale * np.c_[R, t], [[0, 0, 0, 1]] ]
# InvProj = np.r_[ scale * np.c_[R.T, -np.dot(R.T, t)], [[0,0,0,scale]] ]
Proj = tf_format.tf_format('4x4', R, t)
Proj[:-1, :] *= scale
InvProj = tf_format.tf_format('i4x4', R, t) * scale
# Apply transformation to pts3D ---------------------------------------
if self.pts3D is not None and self.pts3D.shape[1] > 0:
# Use homogeneous coords
pts3D = np.r_[self.pts3D, np.ones((1, self.pts3D.shape[1]))]
pts3D = np.dot(Proj, pts3D)
self.pts3D = pts3D[:3, :]
# Apply transformation to cameras -------------------------------------
# Camera poses are stored using camera-to-world transformations, we
# need to invert the projection matrix for this to work -->
# we use InvProj
cposes = self.cam_poses
for i in range(cposes.shape[1]):
# Extract camera projection matrix
p_cam = tf_format.tf_format('4x4', cposes[:, i])
# Transform camera projection matrix
new_p_cam = np.dot(p_cam, InvProj)
# Make sure it's a true rotation!
[u, s, vT] = np.linalg.svd(new_p_cam[:3, :3])
cposes[:3, i] = tf_format.rodrigues(np.dot(u, vT)).ravel()
cposes[3:, i] = new_p_cam[:3, 3]
self.cam_poses = cposes
def print_Camera(f, cpose, idx_cam):
# print_Camera - Camera entry
f.write( ' <Camera ')
f.write( 'id="{0}" '.format(idx_cam))
f.write( 'rot_type="quat" ')
q_t = tf_format.tf_format('quat', cpose)
f.write( 'rot="')
for val in q_t[:4].ravel():
f.write( '{0:6f} '.format(val))
f.write( '" ')
f.write( 'tx="')
for val in q_t[4:].ravel():
f.write( '{0:6f} '.format(val))
f.write( '"/>\n')
def print_Camera(f, cpose, idx_cam):
# print_Camera - Camera entry
f.write(' <Camera ')
f.write('id="{0}" '.format(idx_cam))
f.write('rot_type="quat" ')
q_t = tf_format.tf_format('quat', cpose)
f.write('rot="')
for val in q_t[:4].ravel():
f.write('{0:6f} '.format(val))
f.write('" ')
f.write('tx="')
for val in q_t[4:].ravel():
f.write('{0:6f} '.format(val))
f.write('"/>\n')
def ProjectPts(pts3D, cam_pose, KK, filter_negZ=True):
""" ProjectPts - Project 3D points to camera
Usage: pts2D = ProjectPts(pts3D, cam_pose, KK, [filter_negZ])
Input:
pts3D - 3-by-N numpy array of points
cam_pose - 6-by-1 or 4-by-4 numpy array of camera extrinsic
parameters (world-to-image)
KK - Camera Intrinsic parameters [fx fy cx cy]
filter_negZ{True} - If true, filters all points behind the camera
(Z < 0) and sets them to [nan nan].
Output:
pts2D - 2-by-N numpy array of points [x y] in pixel coords as
projected in the image plane of CalibImage.
"""
import numpy as np
from tf_format import tf_format
# If there are no points in, no points out
if not pts3D.shape[1]:
return np.array([])
pts2D = np.zeros([2, pts3D.shape[1]])
# Get rotation and translation
R, T = tf_format('RT', cam_pose)
# Transform points in world coords to camera coords
tx_pts3D = np.dot(R, pts3D) + T[:, None]
# Perspective projection (I use abs(Z) because I don't want points
# behind the camera
pts2D[0, :] = KK[0] * tx_pts3D[0, :] / tx_pts3D[2, :] + KK[2]
pts2D[1, :] = KK[1] * tx_pts3D[1, :] / tx_pts3D[2, :] + KK[3]
if filter_negZ:
negZ = tx_pts3D[2, :] < 0
pts2D[:, negZ] = np.nan
return pts2D
def ProjectPts(pts3D, cam_pose, KK, filter_negZ=True):
""" ProjectPts - Project 3D points to camera
Usage: pts2D = ProjectPts(pts3D, cam_pose, KK, [filter_negZ])
Input:
pts3D - 3-by-N numpy array of points
cam_pose - 6-by-1 or 4-by-4 numpy array of camera extrinsic
parameters (world-to-image)
KK - Camera Intrinsic parameters [fx fy cx cy]
filter_negZ{True} - If true, filters all points behind the camera
(Z < 0) and sets them to [nan nan].
Output:
pts2D - 2-by-N numpy array of points [x y] in pixel coords as
projected in the image plane of CalibImage.
"""
import numpy as np
from tf_format import tf_format
# If there are no points in, no points out
if not pts3D.shape[1]:
return np.array([])
pts2D = np.zeros([2, pts3D.shape[1]])
# Get rotation and translation
R, T = tf_format('RT', cam_pose)
# Transform points in world coords to camera coords
tx_pts3D = np.dot(R, pts3D) + T[:,None]
# Perspective projection (I use abs(Z) because I don't want points
# behind the camera
pts2D[0, :] = KK[0] * tx_pts3D[0,:] / tx_pts3D[2,:] + KK[2]
pts2D[1, :] = KK[1] * tx_pts3D[1,:] / tx_pts3D[2,:] + KK[3]
if filter_negZ:
negZ = tx_pts3D[2,:] < 0
pts2D[:, negZ] = np.nan
return pts2D
def distRay3D(pts3D, rays3D, cam_pose):
"""distRay3D - Compute distances from 3D point(s) to 3D ray(s).
For a ray3D = [v, c], the distance to a point X = RP+t is computed as:
dist = || (I-vv')(X-c) ||;
Usage: dist = distRay3D(cam_pose, pts3D, rays3D, output);
Input:
pts3D - 3-by-N array of 3D points to compute distances from.
rays3D - 6-by-N array of rays [v(1:3,:); c(4:6,:)] to project pts3D to.
v is the ray direction (a vector) and c is a point in the
ray (e.g. the camera center).
cam_pose - 6-by-1 camera pose [r1 r2 r3 t1 t2 t3]' (world-to-camera),
where [r1 r2 r3] is a rodrigues rotation vector.
Output:
dist - N-by-1 distance vector. Dist(i) = dist(pts3D(:,i), rays3D(:,i)).
NOTE: This function computes 1 point to 1 ray distances, NOT all rays
to all points!
"""
from tf_format import tf_format
import numpy as np
if rays3D.ndim < 2:
rays3D = np.atleast_2d(rays3D).T
R, T = tf_format('RT', cam_pose)
# Transform points according to camera position
tx_pts3D = np.dot(R, pts3D) + T[:,None]
# Pcentered --> X-c
Pcentered = tx_pts3D - rays3D[3:, :]
# dotp --> v'(X-c)
dotp = np.sum(rays3D[0:3, :] * Pcentered, axis=0)
# This is it: dist = (I-vv')(X-c) = (X-c) - vv'(X-c)
dist = Pcentered - (rays3D[0:3, :] * dotp)
return np.sqrt(np.sum(dist**2, axis=0))
def distRay3D(pts3D, rays3D, cam_pose):
"""distRay3D - Compute distances from 3D point(s) to 3D ray(s).
For a ray3D = [v, c], the distance to a point X = RP+t is computed as:
dist = || (I-vv')(X-c) ||;
Usage: dist = distRay3D(cam_pose, pts3D, rays3D, output);
Input:
pts3D - 3-by-N array of 3D points to compute distances from.
rays3D - 6-by-N array of rays [v(1:3,:); c(4:6,:)] to project pts3D to.
v is the ray direction (a vector) and c is a point in the
ray (e.g. the camera center).
cam_pose - 6-by-1 camera pose [r1 r2 r3 t1 t2 t3]' (world-to-camera),
where [r1 r2 r3] is a rodrigues rotation vector.
Output:
dist - N-by-1 distance vector. Dist(i) = dist(pts3D(:,i), rays3D(:,i)).
NOTE: This function computes 1 point to 1 ray distances, NOT all rays
to all points!
"""
from tf_format import tf_format
import numpy as np
if rays3D.ndim < 2:
rays3D = np.atleast_2d(rays3D).T
R, T = tf_format('RT', cam_pose)
# Transform points according to camera position
tx_pts3D = np.dot(R, pts3D) + T[:, None]
# Pcentered --> X-c
Pcentered = tx_pts3D - rays3D[3:, :]
# dotp --> v'(X-c)
dotp = np.sum(rays3D[0:3, :] * Pcentered, axis=0)
# This is it: dist = (I-vv')(X-c) = (X-c) - vv'(X-c)
dist = Pcentered - (rays3D[0:3, :] * dotp)
return np.sqrt(np.sum(dist**2, axis=0))
def ProjectPtsOut(pts2D, KK, cam_pose):
"""projectPtsOut - Use inverse projection to map pts in 2D to 3D rays.
Usage: vec3D, cam_center = projectPtsOut(pts2D, KK, cam_pose);
Input:
pts2D - 2-by-N array of 2D points to be backprojected
KK - internal camera parameters: [fx fy px py].
cam_pose - 6-by-1 camera pose [r1 r2 r3 t1 t2 t3]' (world-to-camera),
where [r1 r2 r3] is a rodrigues rotation vector.
Output:
vec3D - 3-by-N array of 3D vectors backprojected from the camera plane
cam_center - 3-by-1 point that works as camera center. All rays
backprojected from the camera pass through cam_center and vec3D,
i.e.: line(lambda) = cam_center + lambda*vec3D;
"""
from tf_format import tf_format
import numpy as np
Kinv = np.array([1/KK[0], 1/KK[1], -KK[2]/KK[0], -KK[3]/KK[1]])
# We need cam-to-world R,T, so we use inverse-RT 'iRT'
R, T = tf_format('iRT', cam_pose)
cam_center = T
# Vectors in camera coords
vec3D = np.ones((3, pts2D.shape[1]))
vec3D[0, :] = Kinv[0] * pts2D[0,:] + Kinv[2]
vec3D[1, :] = Kinv[1] * pts2D[1,:] + Kinv[3]
# Normalize vectors
vec3D = vec3D / np.sqrt(np.sum(vec3D**2, axis=0))
# Put vectors in world coords (just rotate them)
vec3D = np.dot(R, vec3D)
return vec3D, cam_center
def ProjectPtsOut(pts2D, KK, cam_pose):
"""projectPtsOut - Use inverse projection to map pts in 2D to 3D rays.
Usage: vec3D, cam_center = projectPtsOut(pts2D, KK, cam_pose);
Input:
pts2D - 2-by-N array of 2D points to be backprojected
KK - internal camera parameters: [fx fy px py].
cam_pose - 6-by-1 camera pose [r1 r2 r3 t1 t2 t3]' (world-to-camera),
where [r1 r2 r3] is a rodrigues rotation vector.
Output:
vec3D - 3-by-N array of 3D vectors backprojected from the camera plane
cam_center - 3-by-1 point that works as camera center. All rays
backprojected from the camera pass through cam_center and vec3D,
i.e.: line(lambda) = cam_center + lambda*vec3D;
"""
from tf_format import tf_format
import numpy as np
Kinv = np.array([1 / KK[0], 1 / KK[1], -KK[2] / KK[0], -KK[3] / KK[1]])
# We need cam-to-world R,T, so we use inverse-RT 'iRT'
R, T = tf_format('iRT', cam_pose)
cam_center = T
# Vectors in camera coords
vec3D = np.ones((3, pts2D.shape[1]))
vec3D[0, :] = Kinv[0] * pts2D[0, :] + Kinv[2]
vec3D[1, :] = Kinv[1] * pts2D[1, :] + Kinv[3]
# Normalize vectors
vec3D = vec3D / np.sqrt(np.sum(vec3D**2, axis=0))
# Put vectors in world coords (just rotate them)
vec3D = np.dot(R, vec3D)
return vec3D, cam_center
def show_model(model, type = 'all', IdxCameras = None):
""" Visualize data from a bundler model.
Usage: show_model(model, type, IdxCameras)
Input:
model - Moped model to visualize
type{'all'} - Choose what you want to see: 'pts', 'cam' or 'all'.
IdxCameras{None} - If type = {'all', 'cam'}, options is a list of camIDs that
specify which camera(s) to display. If not given, the default if to
display all cameras used in the training process.
Output:
-NONE-
"""
if IdxCameras is None:
IdxCameras = range(model.nViews)
# Our figure
fig = mlab.figure()
# Display only the points
if type.lower() == 'pts':
# Get a proper scale for the cameras: the std after removing mean
scale = np.mean( np.std(model.pts3D - \
np.mean(model.pts3D, axis=1)[:, None]) )
mlab.points3d(model.pts3D[0,:], \
model.pts3D[1,:], \
model.pts3D[2,:], \
scale_mode='none', scale_factor=0.02, \
color = (0,0,1), figure = fig)
elif type.lower() == 'cam':
scale = 0.1
for i in IdxCameras:
# To use draw_camera we need the camera to world transf.
R, t = tf_format('iRT', model.cam_poses[:, i])
draw_camera(R, t, scale, figure = fig); # Draw cameras
# Draw 3D points and cameras
elif type.lower() == 'all':
# Get a proper scale for the cameras: the std after removing mean
scale = np.mean( np.std(model.pts3D - \
np.mean(model.pts3D, axis=1)[:, None]) )
for i in IdxCameras:
# To use draw_camera we need the camera to world transf.
R, t = tf_format('iRT', model.cam_poses[:, i])
draw_camera(R, t, scale, figure = fig); # Draw cameras
mlab.points3d(model.pts3D[0,:], \
model.pts3D[1,:], \
model.pts3D[2,:], \
scale_mode='none', scale_factor=0.02, \
color = (0,0,1), figure = fig)
# Oops
else:
print('Unknown option')
mlab.view(0,0, distance = scale*50, focalpoint = np.r_[0, 0, 0])
mlab.show()
def read_data(file, imsize = None, read_pts = True, read_images = True):
""" Import camera and scene data from bundle.out.
Usage: data = ReadCamsBundler(file, read_pts=False, imsize)
Input:
file - Full path and filename of the bundler output file (usually,
bundle.out). If only a path is given, we assume the file is
bundle.out. Also, we look for the file 'list.txt' in that same
folder to extract the image names.
imsize{(0, 0)} - (optional) If read_pts is set to true, imsize is used to
re-align pts2D from image center to the lowest left corner.
If not given, no realignment is done.
read_pts{True} - (optional) if True, read also reconstructed 3D points
read_images{True} - (optional) if True, read also image names involved
in bundle adjustment.
Output:
cam_poses - 6-by-K array of 6DOF camera poses [r1 r2 r3 t1 t2 t3]'
where [r1 r2 r3] is a rodrigues rotation vector. This
is a WORLD TO CAMERA transformation: x = K[R t]X.
scene - Structure that contains all output data after running the sfm
algorithm. See sfm_model.m for details.
image_names - List of image names used in this bundle adjustment. It is
an ordered list, so image_names[i] corresponds to cam_poses[:,i].
"""
imsize = (0,0) if imsize is None else imsize
if not os.path.exists(file):
raise "File does not exist!"
path, name = os.path.split(file)
images_file = os.path.join(path, 'list.txt')
# Open file
with open(file, 'rt') as fp:
# Read first line (uninteresting)
fp.readline()
# Read #images and #points
line = fp.readline().split()
nImgs = int(line[0])
nPoints = int(line[1])
# Create matrix of camera poses
cam_poses = np.zeros((6, nImgs))
# Mirror matrix
Q = np.c_[[1, 0, 0], [0, -1, 0], [0, 0, -1]];
# Start extracting the camera positions
for i in range(nImgs):
R = np.zeros((3,3))
# First three values of cam_data are estimation of some intrinsics (don't care)
cam_data = np.fromstring(fp.readline(), sep = ' ')
# Read R line by line
R[0,:] = np.fromstring(fp.readline(), sep = ' ')
R[1,:] = np.fromstring(fp.readline(), sep = ' ')
R[2,:] = np.fromstring(fp.readline(), sep = ' ')
T = np.fromstring(fp.readline(), sep = ' ')
# Bundler uses -Z as the positive depth, we don't (need to mirror it!)
R = np.dot(Q, R)
T = np.dot(Q, T)
cam_poses[:,i] = tf_format.tf_format('rod', R, T);
# Extract 3D points too
scene = dict()
if read_pts:
pts3D = np.zeros((3, nPoints))
color3D = np.zeros((3, nPoints), dtype=np.uint8)
pts2D = np.zeros((2, nPoints, nImgs))
keys = np.zeros((nPoints, nImgs))
nViews = np.zeros((nPoints,))
for i in range(nPoints):
pts3D[:, i] = np.fromstring(fp.readline(), sep = ' ')
color3D[:,i] = np.fromstring(fp.readline(), sep = ' ')
buffer = np.fromstring(fp.readline(), sep=' ')
num_views = int(buffer[0])
nViews[i] = num_views
for j in range(num_views):
# Values: view, key, x, y for each point
view_data = buffer[1+j*4:5+j*4]
# view and key are both zero-based, and (x=0,y=0) is the
# image center
pts2D[0, i, view_data[0]] = view_data[2] + imsize[0]/2
pts2D[1, i, view_data[0]] = imsize[1]/2 - view_data[3]
keys[i, view_data[0]] = view_data[1]
scene['pts3D'] = pts3D
scene['color3D'] = color3D
scene['pts2D'] = pts2D
scene['keys'] = keys
scene['num_views'] = nViews
# Now read image names
image_names = list()
if read_images:
with open(images_file, 'rt') as fp:
for image_line in fp:
# There are three fields, we only want the first
image_names.append(image_line.split()[0])
# If we don't want scene data, 'scene' is empty
# If we don't want image names, 'image_names' is empty
return cam_poses, scene, image_names
def read_data(file, imsize = None, read_pts = True, read_images = True):
""" Import camera and scene data from bundle.out.
Usage: data = ReadCamsBundler(file, read_pts=False, imsize)
Input:
file - Full path and filename of the bundler output file (usually,
bundle.out). If only a path is given, we assume the file is
bundle.out. Also, we look for the file 'list.txt' in that same
folder to extract the image names.
imsize{(0, 0)} - (optional) If read_pts is set to true, imsize is used to
re-align pts2D from image center to the lowest left corner.
If not given, no realignment is done.
read_pts{True} - (optional) if True, read also reconstructed 3D points
read_images{True} - (optional) if True, read also image names involved
in bundle adjustment.
Output:
cam_poses - 6-by-K array of 6DOF camera poses [r1 r2 r3 t1 t2 t3]'
where [r1 r2 r3] is a rodrigues rotation vector. This
is a WORLD TO CAMERA transformation: x = K[R t]X.
scene - Structure that contains all output data after running the sfm
algorithm. See sfm_model.m for details.
image_names - List of image names used in this bundle adjustment. It is
an ordered list, so image_names[i] corresponds to cam_poses[:,i].
"""
imsize = (0,0) if imsize is None else imsize
if not os.path.exists(file):
raise "File does not exist!"
path, name = os.path.split(file)
images_file = os.path.join(path, 'list.txt')
# Open file
with open(file, 'rt') as fp:
# Read first line (uninteresting)
fp.readline()
# Read #images and #points
line = fp.readline().split()
nImgs = int(line[0])
nPoints = int(line[1])
# Create matrix of camera poses
cam_poses = np.zeros((6, nImgs))
# Mirror matrix
Q = np.c_[[1, 0, 0], [0, -1, 0], [0, 0, -1]];
# Start extracting the camera positions
for i in range(nImgs):
R = np.zeros((3,3))
# First three values of cam_data are estimation of some intrinsics (don't care)
cam_data = np.fromstring(fp.readline(), sep = ' ')
# Read R line by line
R[0,:] = np.fromstring(fp.readline(), sep = ' ')
R[1,:] = np.fromstring(fp.readline(), sep = ' ')
R[2,:] = np.fromstring(fp.readline(), sep = ' ')
T = np.fromstring(fp.readline(), sep = ' ')
# Bundler uses -Z as the positive depth, we don't (need to mirror it!)
R = np.dot(Q, R)
T = np.dot(Q, T)
cam_poses[:,i] = tf_format.tf_format('rod', R, T);
# Extract 3D points too
scene = dict()
if read_pts:
pts3D = np.zeros((3, nPoints))
color3D = np.zeros((3, nPoints), dtype=np.uint8)
pts2D = np.zeros((2, nPoints, nImgs))
keys = np.zeros((nPoints, nImgs))
nViews = np.zeros((nPoints,))
for i in range(nPoints):
pts3D[:, i] = np.fromstring(fp.readline(), sep = ' ')
color3D[:,i] = np.fromstring(fp.readline(), sep = ' ')
buffer = np.fromstring(fp.readline(), sep=' ')
num_views = int(buffer[0])
nViews[i] = num_views
for j in range(num_views):
# Values: view, key, x, y for each point
view_data = buffer[1+j*4:5+j*4]
# view and key are both zero-based, and (x=0,y=0) is the
# image center
pts2D[0, i, view_data[0]] = view_data[2] + imsize[0]/2
pts2D[1, i, view_data[0]] = imsize[1]/2 - view_data[3]
keys[i, view_data[0]] = view_data[1]
scene['pts3D'] = pts3D
scene['color3D'] = color3D
scene['pts2D'] = pts2D
scene['keys'] = keys
scene['num_views'] = nViews
# Now read image names
image_names = list()
if read_images:
with open(images_file, 'rt') as fp:
for image_line in fp:
# There are three fields, we only want the first
image_names.append(image_line.split()[0])
# If we don't want scene data, 'scene' is empty
# If we don't want image names, 'image_names' is empty
return cam_poses, scene, image_names
def ReprojectionErr(cam_pose, pts2D, pts3D, KK):
RT = tf_format.tf_format('3x4', cam_pose, 'rpy')
pts2D_proj = utils.C_ProjectPts(pts3D, RT, KK, False)
val = np.sqrt(np.sum( (pts2D - pts2D_proj)**2 )) / pts2D.shape[1]
print (val)
return val
def update(self, **kwargs):
""" Update value(s) in class. Please use K or KK, pose or M, dist, \
and/or rect as input args.
Usage: update(**kwargs)
Examples:
update(KK=np.array([1000., 1000., 320., 240.]))
update(pose=np.array([0., 1.5, 0., 0.1, 0.2, 0,5]))
"""
# Assign attributes
for arg in kwargs:
if arg == 'yaml_file':
self.load_yaml(kwargs[arg])
elif arg == 'name':
self.name = kwargs[arg]
elif arg == 'KK':
self.KK = kwargs[arg].copy()
self.K = np.eye(3)
self.K[0,0] = self.KK[0]
self.K[1,1] = self.KK[1]
self.K[0,2] = self.KK[2]
self.K[1,2] = self.KK[3]
self.KKinv = np.array([1/self.KK[0], 1/self.KK[1], \
-self.KK[2]/self.KK[0], -self.KK[3]/self.KK[1]]);
elif arg == 'K':
self.K = kwargs[arg].copy()
self.KK = np.zeros([4,1])
self.KK[0] = self.K[0,0]
self.KK[1] = self.K[1,1]
self.KK[2] = self.K[0,2]
self.KK[3] = self.K[1,2]
self.KKinv = np.array([1/self.KK[0], 1/self.KK[1], \
-self.KK[2]/self.KK[0], -self.KK[3]/self.KK[1]]);
elif arg == 'pose' or arg == 'M':
pose = kwargs[arg].copy()
self.pose = tf_format('rod', pose)
self.M = tf_format('4x4', pose)
self.Minv = tf_format('i4x4', pose)
self.R, self.T = tf_format('RT', pose)
self.Rinv, self.Tinv = tf_format('iRT', pose)
elif arg == 'dist':
# Ensure dimensions are (5,1)
dist = kwargs[arg].copy()
self.dist = np.zeros((5,1))
self.dist.flat[:dist.size] = dist
elif arg == 'rect':
self.rect = kwargs[arg].copy()
elif arg == 'width':
self.width = kwargs[arg]
elif arg == 'height':
self.height = kwargs[arg]
else:
raise KeyError, 'The argument %s cannot be updated. Please \
use K, KK, pose, M, dist or rect as initial \
args.' % arg
