<commit_before>#!/usr/bin/python3

# write out the data in a form useful to pass to the sba (demo) program

# it appears camera poses are basically given as [ R | t ] where R is
# the same R we use throughout and t is the 'tvec'

# todo, run sba and automatically parse output ...

import sys
#sys.path.insert(0, "/usr/local/opencv3/lib/python2.7/site-packages/")

import argparse
import pickle
import cv2
import math
import numpy as np
import os
import random

sys.path.append('../lib')
import Groups
import Optimizer
import ProjectMgr
import transformations

d2r = math.pi / 180.0       # a helpful constant

parser = argparse.ArgumentParser(description='Keypoint projection.')
parser.add_argument('--project', required=True, help='project directory')

args = parser.parse_args()

# return a 3d affine tranformation between current camera locations
# and original camera locations.
def get_recenter_affine(src_list, dst_list):
    print('get_recenter_affine():')
    src = [[], [], [], []]      # current camera locations
    dst = [[], [], [], []]      # original camera locations
    for i in range(len(src_list)):
        src_ned = src_list[i]
        src[0].append(src_ned[0])
        src[1].append(src_ned[1])
        src[2].append(src_ned[2])
        src[3].append(1.0)
        dst_ned = dst_list[i]
        dst[0].append(dst_ned[0])
        dst[1].append(dst_ned[1])
        dst[2].append(dst_ned[2])
        dst[3].append(1.0)
        print("{} <-- {}".format(dst_ned, src_ned))
    A = transformations.superimposition_matrix(src, dst, scale=True)
    print("A:\n{}".formata(A))
    return A

# transform a point list given an affine transform matrix
def transform_points( A, pts_list ):
    src = [[], [], [], []]
    for p in pts_list:
        src[0].append(p[0])
        src[1].append(p[1])
        src[2].append(p[2])
        src[3].append(1.0)
    dst = A.dot( np.array(src) )
    result = []
    for i in range(len(pts_list)):
        result.append( [ float(dst[0][i]),
                         float(dst[1][i]),
                         float(dst[2][i]) ] )
    return result

proj = ProjectMgr.ProjectMgr(args.project)
proj.load_image_info()
proj.load_features()
#proj.undistort_keypoints()
# proj.load_match_pairs()

grouped_file = os.path.join(args.project, 'matches_grouped' )
direct_file = os.path.join(args.project, 'matches_direct' )
if os.path.isfile( grouped_file ):
    print('Match file:', grouped_file)
    matches = pickle.load( open(grouped_file, "rb") )
elif os.path.isfile( direct_file ):
    print('Match file:', direct_file)
    matches = pickle.load( open(direct_file, "rb") )
else:
    print("Cannot find a matches file to load... aborting")
    quit()
    
print('Match features:', len(matches))

# load the group connections within the image set
groups = Groups.load(args.project)
print('Main group size:', len(groups[0]))

image_width = proj.image_list[0].width
camw, camh = proj.cam.get_image_params()
scale = float(image_width) / float(camw)
print('scale: {}'.format(scale))
K = proj.cam.get_K(scale)
distCoeffs = np.array(proj.cam.get_dist_coeffs())

opt = Optimizer.Optimizer(args.project)
cameras, features, cam_index_map, feat_index_map, fx_opt, fy_opt, cu_opt, cv_opt, distCoeffs_opt = opt.run( proj.image_list, groups[0], matches, K, distCoeffs, use_sba=False )

# wipe the sba pose for all images
for image in proj.image_list:
    image.camera_pose_sba = None
    image.placed = False
proj.save_images_meta()

for i, cam in enumerate(cameras):
    image_index = cam_index_map[i]
    image = proj.image_list[image_index]
    orig = image.camera_pose
    print('sba cam: {}'.format(cam))
    if len(cam) == 6:
        Rned2cam, jac = cv2.Rodrigues(cam[0:3])
        tvec = cam[3:6]
    elif len(cam) == 7:
        newq = np.array( [ cam[0], cam[1], cam[2], cam[3] ] )
        Rned2cam = transformations.quaternion_matrix(newq)[:3,:3]
        tvec = np.array( [ cam[4], cam[5], cam[6] ] )
    elif len(cam) == 12:
        newq = np.array( cam[5:9] )
        Rned2cam = transformations.quaternion_matrix(newq)[:3,:3]
        tvec = np.array( cam[9:12] )
        k = np.array( [ [cam[0], 0, cam[1]],
                        [0, cam[3]*cam[0], cam[2]] ] )
        print('est K:\n{}'.format(k))
    elif len(cam) == 17:
        newq = np.array( cam[10:14] )
        tvec = np.array( cam[14:17] )
        Rned2cam = transformations.quaternion_matrix(newq)[:3,:3]
    cam2body = image.get_cam2body()
    Rned2body = cam2body.dot(Rned2cam)
    Rbody2ned = np.matrix(Rned2body).T
    (yaw, pitch, roll) = transformations.euler_from_matrix(Rbody2ned, 'rzyx')
    #print "orig ypr =", image.camera_pose['ypr']
    #print "new ypr =", [yaw/d2r, pitch/d2r, roll/d2r]
    pos = -np.matrix(Rned2cam).T * np.matrix(tvec).T
    newned = pos.T[0].tolist()[0]
    #print "orig ned =", image.camera_pose['ned']
    #print "new ned =", newned
    image.set_camera_pose_sba( ned=newned, ypr=[yaw/d2r, pitch/d2r, roll/d2r] )
    image.placed = True<commit_msg>clean up some older cruft.<commit_after>#!/usr/bin/python3

# write out the data in a form useful to pass to the sba (demo) program

# it appears camera poses are basically given as [ R | t ] where R is
# the same R we use throughout and t is the 'tvec'

# todo, run sba and automatically parse output ...

import sys
#sys.path.insert(0, "/usr/local/opencv3/lib/python2.7/site-packages/")

import argparse
import pickle
import cv2
import math
import numpy as np
import os
import random

sys.path.append('../lib')
import Groups
import Optimizer
import ProjectMgr
import transformations

d2r = math.pi / 180.0       # a helpful constant

parser = argparse.ArgumentParser(description='Keypoint projection.')
parser.add_argument('--project', required=True, help='project directory')

args = parser.parse_args()

# return a 3d affine tranformation between current camera locations
# and original camera locations.
def get_recenter_affine(src_list, dst_list):
    print('get_recenter_affine():')
    src = [[], [], [], []]      # current camera locations
    dst = [[], [], [], []]      # original camera locations
    for i in range(len(src_list)):
        src_ned = src_list[i]
        src[0].append(src_ned[0])
        src[1].append(src_ned[1])
        src[2].append(src_ned[2])
        src[3].append(1.0)
        dst_ned = dst_list[i]
        dst[0].append(dst_ned[0])
        dst[1].append(dst_ned[1])
        dst[2].append(dst_ned[2])
        dst[3].append(1.0)
        print("{} <-- {}".format(dst_ned, src_ned))
    A = transformations.superimposition_matrix(src, dst, scale=True)
    print("A:\n{}".formata(A))
    return A

# transform a point list given an affine transform matrix
def transform_points( A, pts_list ):
    src = [[], [], [], []]
    for p in pts_list:
        src[0].append(p[0])
        src[1].append(p[1])
        src[2].append(p[2])
        src[3].append(1.0)
    dst = A.dot( np.array(src) )
    result = []
    for i in range(len(pts_list)):
        result.append( [ float(dst[0][i]),
                         float(dst[1][i]),
                         float(dst[2][i]) ] )
    return result

proj = ProjectMgr.ProjectMgr(args.project)
proj.load_image_info()
proj.load_features()
#proj.undistort_keypoints()
# proj.load_match_pairs()

grouped_file = os.path.join(args.project, 'matches_grouped' )
direct_file = os.path.join(args.project, 'matches_direct' )
if os.path.isfile( grouped_file ):
    print('Match file:', grouped_file)
    matches = pickle.load( open(grouped_file, "rb") )
elif os.path.isfile( direct_file ):
    print('Match file:', direct_file)
    matches = pickle.load( open(direct_file, "rb") )
else:
    print("Cannot find a matches file to load... aborting")
    quit()
    
print('Match features:', len(matches))

# load the group connections within the image set
groups = Groups.load(args.project)
print('Main group size:', len(groups[0]))

image_width = proj.image_list[0].width
camw, camh = proj.cam.get_image_params()
scale = float(image_width) / float(camw)
print('scale: {}'.format(scale))
K = proj.cam.get_K(scale)
distCoeffs = np.array(proj.cam.get_dist_coeffs())

opt = Optimizer.Optimizer(args.project)
cameras, features, cam_index_map, feat_index_map, fx_opt, fy_opt, cu_opt, cv_opt, distCoeffs_opt = opt.run( proj.image_list, groups[0], matches, K, distCoeffs, use_sba=False )

# wipe the sba pose for all images
for image in proj.image_list:
    image.camera_pose_sba = None
    image.placed = False
proj.save_images_meta()

for i, cam in enumerate(cameras):
    image_index = cam_index_map[i]
    image = proj.image_list[image_index]
    orig = image.camera_pose
    print('sba cam: {}'.format(cam))
    rvec = cam[0:3]
    tvec = cam[3:6]
    Rned2cam, jac = cv2.Rodrigues(rvec)
    cam2body = image.get_cam2body()
    Rned2body = cam2body.dot(Rned2cam)
    Rbody2ned = np.matrix(Rned2body).T
    (yaw, pitch, roll) = transformations.euler_from_matrix(Rbody2ned, 'rzyx')
    #print "orig ypr =", image.camera_pose['ypr']
    #print "new ypr =", [yaw/d2r, pitch/d2r, roll/d2r]
    pos = -np.matrix(Rned2cam).T * np.matrix(tvec).T
    newned = pos.T[0].tolist()[0]
    print(image.name, image.camera_pose['ned'], '->', newned, 'dist:', np.linalg.norm(np.array(image.camera_pose['ned']) - np.array(newned)))
    image.set_camera_pose_sba( ned=newned, ypr=[yaw/d2r, pitch/d2r, roll/d2r] )
    image.placed = True
{'fraction_non_alphanumeric': 0.15160910937667058, 'fraction_numerical': 0.019031326847000964, 'mean_word_length': 3.4521656354117085, 'pattern_counts': {'":': 0, '<': 5, '<?xml version=': 0, '>': 4, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '1422478', 'n_tokens_mistral': 3464, 'n_tokens_neox': 3383, 'n_words': 744}