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 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