params['position_error']=pos.cost
cam_rays.append({'start': pos.x, 'dir':c})
print('\tangles: zx={}, zy={}, xy={}'.format(angle_between(fwd,right)*rad_to_deg, angle_between(fwd,up)*rad_to_deg, angle_between(right, up)*rad_to_deg))
# for camera transform setup. look along -z so invert, also invert fwd as it is the z+ axis of the camera
cam_json[str(i)] = params
flip_z = np.asarray([1,1,-1])
cam_params.append([list(-1*(c*flip_z)), list(up*flip_z), list(right*flip_z), list(pos.x*flip_z)])
#print('{}, {}, {}, {}'.format(list(-1*(c*flip_z)), list(up*flip_z), list(right*flip_z), list(pos.x*flip_z)))
# if len(cam)>=5:
# fov = (angle_between(cam[1], cam[2]),angle_between(cam[3], cam[4]))
# print('\tfov: x', fov[0]*rad_to_deg,'y',fov[1]*rad_to_deg)
# up = np.cross(cam[1], cam[2])
# up /= np.linalg.norm(up)
# print('\tup:', up, angle_between(up, cam[0])*rad_to_deg)
logger.flush()
focus = least_squares(f, [0,0,0], kwargs={'rays':cam_rays})
print('focus: {}, error: {}:'.format(focus.x, focus.cost))
cam_json['fov_horizontal_average'] = np.mean([cam_json[str(i)]['fov_horizontal'] for i in cam_order if cam_json[str(i)]['fov_horizontal'] is not None])
cam_json['fov_vertical_average'] = np.mean([cam_json[str(i)]['fov_vertical'] for i in cam_order if cam_json[str(i)]['fov_vertical'] is not None])
cam_json['focus']=list(focus.x)
cam_json['focus_error']=focus.cost
# fixed parameters from scalarFlow (/source/util/ray.cpp)
cam_json['scale_y'] = 1.77
cam_json['marker_width'] = 0.4909
cam_json['volume_width'] = cam_json['marker_width']
cam_json['volume_offset'] = [cam_json['marker_width']/6., -cam_json['marker_width']/11., -cam_json['marker_width']/100.] #world-space position of the (0,0,0) corner of the volume
#cam_json['volume_size'] = [cam_json['volume_width'], math.ceil(cam_json['scale_y']*cam_json['volume_width']), cam_json['volume_width']]
cam_json['volume_size'] = [cam_json['volume_width'], cam_json['scale_y']*cam_json['volume_width'], cam_json['volume_width']]
