def from_xml(element):
"""
Build a StereoRig object out of the given xml node
@type element: lxml.etree.SubElement
@param element: the element to construct an StereoRig object from
@return a new StereoRig object constructed from XML node with matrices in
OpenCV format
"""
use_old_format = element.find("CameraExtrinsics") is not None
if use_old_format:
extrinsics0 = Camera.Extrinsics()
extrinsics1 = Camera.Extrinsics.from_xml(element.find("CameraExtrinsics"))
cameras_elem = element.find(Rig.CAMERAS_ELEMENT_TAG)
cameras = (Camera.from_xml(cameras_elem[0]), Camera.from_xml(cameras_elem[1]))
cameras[0].extrinsics = extrinsics0
cameras[1].extrinsics = extrinsics1
extrinsics = None
else:
cameras_element = element.find(Rig.CAMERAS_ELEMENT_TAG)
cameras = tuple([Camera.from_xml(camera_element) for camera_element in cameras_element])
extrinsics_element = element.find("Extrinsics")
if extrinsics_element is not None:
extrinsics = Camera.Extrinsics.from_xml(extrinsics_element)
else:
extrinsics = None
return Rig(cameras, extrinsics)
def from_xml(element):
"""
Build a StereoRig object out of the given xml node
@type element: lxml.etree.SubElement
@param element: the element to construct an StereoRig object from
@return a new StereoRig object constructed from XML node with matrices in
OpenCV format
"""
use_old_format = element.find("CameraExtrinsics") is not None
if use_old_format:
extrinsics0 = Camera.Extrinsics()
extrinsics1 = Camera.Extrinsics.from_xml(
element.find("CameraExtrinsics"))
cameras_elem = element.find(Rig.CAMERAS_ELEMENT_TAG)
cameras = (Camera.from_xml(cameras_elem[0]),
Camera.from_xml(cameras_elem[1]))
cameras[0].extrinsics = extrinsics0
cameras[1].extrinsics = extrinsics1
extrinsics = None
else:
cameras_element = element.find(Rig.CAMERAS_ELEMENT_TAG)
cameras = tuple([
Camera.from_xml(camera_element)
for camera_element in cameras_element
])
extrinsics_element = element.find("Extrinsics")
if extrinsics_element is not None:
extrinsics = Camera.Extrinsics.from_xml(extrinsics_element)
else:
extrinsics = None
return Rig(cameras, extrinsics)
def from_infinitam_format(file_handle_or_name):
if type(file_handle_or_name) == str:
file_handle = open(file_handle_or_name, 'r')
else:
file_handle = file_handle_or_name
color_resolution_line = file_handle.readline()
color_intrinsics_lines = [
file_handle.readline(),
file_handle.readline()
]
file_handle.readline()
depth_resolution_line = file_handle.readline()
depth_intrinsics_lines = [
file_handle.readline(),
file_handle.readline()
]
file_handle.readline()
extrinsics_lines = [
file_handle.readline(),
file_handle.readline(),
file_handle.readline()
]
file_handle.readline()
depth_ratio_and_offset_line = file_handle.readline()
color_camera = Camera.from_infinitam_format(color_resolution_line,
color_intrinsics_lines)
depth_camera = DepthCamera.from_infinitam_format(
depth_resolution_line, depth_intrinsics_lines, extrinsics_lines,
depth_ratio_and_offset_line)
return DepthCameraRig(cameras=(color_camera, depth_camera))
def __init__(self,
calibration_file_path,
first_frame_path,
second_frame_path,
image_pixel_row,
field_size,
offset,
voxel_size=0.004,
focus_coordinates=(-1, -1, -1)):
super().__init__(focus_coordinates)
self.calibration_file_path = calibration_file_path
self.first_frame_path = first_frame_path
self.second_frame_path = second_frame_path
self.image_pixel_row = image_pixel_row
self.field_size = field_size
self.offset = offset
self.voxel_size = voxel_size
if os.path.exists(self.calibration_file_path) and os.path.isdir(
self.calibration_file_path):
self.rig = DepthCameraRig.from_infinitam_format(
self.calibration_file_path)
else:
camera_intrinsic_matrix = np.array(
[[700., 0., 320.], [0., 700., 240.], [0., 0., 1.]],
dtype=np.float32)
camera = DepthCamera(intrinsics=Camera.Intrinsics(
(480, 640), intrinsic_matrix=camera_intrinsic_matrix),
depth_unit_ratio=0.001)
self.rig = DepthCameraRig(cameras=(camera, ))
parameters_2d = cpp_module.tsdf.Parameters2d()
parameters_2d.interpolation_method = cpp_module.tsdf.FilteringMethod.NONE
parameters_2d.projection_matrix = self.rig.depth_camera.intrinsics.intrinsic_matrix.astype(
np.float32)
parameters_2d.array_offset = cpp_module.Vector2i(
int(self.offset[0]), int(self.offset[2]))
parameters_2d.field_shape = cpp_module.Vector2i(
self.field_size, self.field_size)
self.parameters_2d = parameters_2d
parameters_3d = cpp_module.tsdf.Parameters3d()
parameters_3d.interpolation_method = cpp_module.tsdf.FilteringMethod.NONE
parameters_3d.projection_matrix = self.rig.depth_camera.intrinsics.intrinsic_matrix.astype(
np.float32)
parameters_3d.array_offset = cpp_module.Vector3i(
int(self.offset[0]), int(self.offset[1]), int(self.offset[2]))
parameters_3d.field_shape = cpp_module.Vector3i(
self.field_size, self.field_size, self.field_size)
self.parameters_3d = parameters_3d
def find_offset_range(video_filenames, folder, offset, cut_off_end=2.0):
"""
Find frame & time ranges where to clip provided video files based on specified offset & cut-off.
"""
source_videos = []
frame_durations = []
frame_offsets = []
ranges = []
frame_rates = []
ix_vid = 0
for file_name in video_filenames:
camera = Camera(os.path.join(folder, file_name))
frame_durations.append(camera.frame_count)
source_videos.append(camera)
frame_offset = int(camera.fps * offset[ix_vid])
frame_offsets.append(frame_offset)
trim_frames = int(camera.fps * cut_off_end)
frame_rates.append(camera.fps)
ranges.append((frame_offset, camera.frame_count - trim_frames))
del camera # explicitly release video file
ix_vid += 1
return __sync_ranges(frame_durations, frame_rates, ranges, frame_offsets)
def find_calibration_conversion_range(video_filenames, folder, offset, board_dims,
seek_time_interval=1.0, cut_off_end=20.0, verbose=True):
"""
Find ranges where to clip the videos for calibration, i.e. the first frame where the calibration
board appears and the last, as constrained by provided offset between the videos, the
specified cut-off at the end (in seconds), and the seek interval (in seconds).
@type video_filenames: list[str]
@param video_filenames: names of the video files
@type folder: str
@param folder: path of the folder where the video files reside
@type board_dims: tuple[int]
@param board_dims: dimensions of the calibration board
@type offset: list[int]
@param offset: time offset, in frames, from the first video
@type seek_time_interval: float
@param seek_time_interval: time interval in seconds
@type cut_off_end: float
@param cut_off_end: how much (minimum) to cut off the end of both videos (seconds)
@type verbose: bool
@param verbose: whether to print progress & results to standard output
@rtype: list[tuple[int]]
@return proper ranges to clip the videos for calibration.
"""
source_videos = []
for file_name in video_filenames:
camera = Camera(os.path.join(folder, file_name))
source_videos.append(camera)
ix_vid = 0
ranges = []
frame_offsets = []
frame_durations =[]
frame_rates = []
for camera in source_videos:
frame_durations.append(camera.frame_count)
frame_offset = int(camera.fps * offset[camera.index])
frame_offsets.append(frame_offset)
frame_rates.append(camera.fps)
i_frame = frame_offset
camera.scroll_to_frame(i_frame)
camera.read_next_frame()
skip_interval = int(seek_time_interval * camera.fps)
# find the first frame with corners past the offset mark
cont_cap = True
if verbose:
print("Seeking first frame of {0:s} usable for calibration...".format(camera.name))
while cont_cap:
found_corners = camera.try_approximate_corners(board_dims)
i_frame += skip_interval
camera.scroll_to_frame(i_frame)
if i_frame > (camera.frame_count-1):
camera.more_frames_remain = False
break
camera.read_next_frame()
cont_cap = camera.more_frames_remain and not found_corners
if verbose:
print('.',end="",flush=True)
if not camera.more_frames_remain:
raise ValueError("Could not find a frame in the video containing the checkerboard!")
start_frame = i_frame
if verbose:
print("\nFound at frame {0:d} ({1:.3f} s).".format(start_frame, start_frame / camera.fps))
# find the last frame with corners
i_frame = camera.frame_count-1 - int(camera.fps * cut_off_end)
camera.scroll_to_frame(i_frame)
camera.read_next_frame()
found_corners = False
if verbose:
print("Seeking last usable frame of {0:s}...".format(camera.name))
while not found_corners:
found_corners = camera.try_approximate_corners(board_dims)
i_frame -= skip_interval
camera.scroll_to_frame(i_frame)
camera.read_next_frame()
if verbose:
print('.', end="", flush=True)
# add one for exclusive right bound
end_frame = i_frame + 1
if verbose:
print("\nFound at frame {0:d} ({1:.3f} s).".format(end_frame, end_frame / camera.fps))
del camera # explicitly release video file
ranges.append((start_frame, end_frame))
ix_vid += 1
return __sync_ranges(frame_durations, frame_rates, ranges, frame_offsets)
def __init__(self, stereo_rigs=None, rig=None):
"""
Construct a multi-stereo rig out of either a collection of stereo rigs or a single rig
@type stereo_rigs: tuple[calib.rig.Rig]
@param stereo_rigs: rigs with two cameras each and extrinsics with reference to the global coordinate frame,
i.e. the first camera of the first rig
@type rig: calib.rig.Rig
@param rig: a regular rig to convert to a multistereo rig. Assumes stereo camera pairs are in order in the input
rig.
"""
if stereo_rigs is not None and rig is None:
ix_rig = 0
for rig in stereo_rigs:
if len(rig.cameras) != 2:
raise ValueError(
"Each of the stereo rigs should only have two cameras")
if rig.extrinsics is None and ix_rig != 0:
raise ValueError(
"Expecting valid extrinsics for rig at position {:d} relative to the first rig."
.format(ix_rig))
ix_rig += 1
self.rigs = stereo_rigs
elif rig is not None and stereo_rigs is None:
# convert an arbitrary rig to a multistereo rig.
if len(rig.cameras) % 2 != 0 and len(rig.cameras) != 0:
raise ValueError(
"Expecting a non-zero even number of cameras in the input rig."
)
rigs = [
Rig(cameras=(rig.cameras[0], rig.cameras[1],
Camera.Extrinsics()))
]
# find local extrinsics
for ix_cam in range(2, len(rig.cameras), 2):
left_cam = rig.cameras[ix_cam]
right_cam = rig.cameras[ix_cam + 1]
left_extrinsics = left_cam.extrinsics
right_extrinsics = right_cam.extrinsics
left_pose = Pose(
rotation=left_extrinsics.rotation,
translation_vector=left_extrinsics.translation)
right_pose = Pose(
rotation=right_extrinsics.rotation,
translation_vector=right_extrinsics.translation)
local_pose = Pose(
transform=np.linalg.inv(left_pose.T).dot(right_pose.T))
local_right_extrinsics = Camera.Extrinsics(
rotation=local_pose.rmat, translation=local_pose.rvec)
stereo_rig_extrinsics = left_cam.extrinsics
left_cam.extrinsics = Camera.Extrinsics()
right_cam.extrinsics = local_right_extrinsics
rigs.append(
Rig(cameras=(left_cam, right_cam),
extrinsics=stereo_rig_extrinsics))
self.rigs = tuple(rigs)
else:
raise ValueError(
"Either the ['stereo_rigs' and 'stereo_rig_extrinsics'] OR the 'rig' argument should be "
"provided and not None.")
def __init__(self, args):
"""
Base constructor
"""
self.args = args
self.aux_data_file = {}
# load or initialize auxiliary data
if args.aux_data_file is not None:
self.aux_data_path = os.path.join(self.args.folder,
self.args.aux_data_file)
if osp.isfile(self.aux_data_path):
npz_archive = np.load(osp.join(args.folder,
args.aux_data_file))
# convert to dict
for key in npz_archive:
self.aux_data_file[key] = npz_archive[key]
# set up board (3D object points of checkerboard used for calibration)
self.object_points = []
self.board_dims = (args.board_width, args.board_height)
self.board_object_corner_set = np.zeros(
(args.board_height * args.board_width, 1, 3), np.float32)
self.board_object_corner_set[:, :, :2] = np.indices(
self.board_dims).T.reshape(-1, 1, 2)
self.board_object_corner_set *= args.board_square_size
self.pixel_difference_factor = 1.0 / (self.board_dims[0] *
self.board_dims[1] * 3 * 256.0)
# some vars set to default
self.criteria_subpix = (cv2.TERM_CRITERIA_EPS +
cv2.TERM_CRITERIA_MAX_ITER, 40, 0.001)
if args.output is None:
args.output = "intrinsics{0:s}.xml".format(
re.sub(r"-|:", "",
str(datetime.datetime.now())[:-7]).replace(" ", "-"))
self.videos = [
Video(os.path.join(args.folder, video_filename))
for video_filename in args.videos
]
if args.input_calibration is not None:
intrinsic_arr = []
# load calibration files
initial_calibration = []
for calib_file in args.input_calibration:
initial_calibration.append(
load_opencv_calibration(
os.path.join(args.folder, calib_file)))
for calibration_info in initial_calibration:
if type(calibration_info) == MultiStereoRig:
for rig in calibration_info.rigs:
for camera in rig.cameras:
intrinsic_arr.append(camera.intrinsics)
elif type(calibration_info) == Rig:
for camera in calibration_info.cameras:
intrinsic_arr.append(camera.intrinsics)
elif type(calibration_info) == Camera:
intrinsic_arr.append(calibration_info.intrinsics)
elif type(calibration_info) == Camera.Intrinsics:
intrinsic_arr.append(calibration_info)
else:
raise RuntimeError("Unsupported calibration file format.")
if len(intrinsic_arr) != len(args.videos):
raise ValueError(
"The total number of intrinsics in all the provided input calibration files "
+
"combined ({:d}) does not equal the total number provided of video file paths ({:d})."
+ "These numbers must match.".format(
len(intrinsic_arr), len(args.videos)))
self.cameras = [
Camera(intrinsics=intrinsics) for intrinsics in intrinsic_arr
]
if len(initial_calibration) == 1 and (
type(initial_calibration[0]) == Rig
or type(initial_calibration[0]) == MultiStereoRig):
self.rig = initial_calibration[0]
else:
self.rig = Rig(tuple(self.cameras))
else:
self.cameras = [
Camera(resolution=video.frame_dims) for video in self.videos
]
self.rig = Rig(tuple(self.cameras))
def find_calibration_conversion_range(video_filenames,
folder,
offset,
board_dims,
seek_time_interval=1.0,
cut_off_end=20.0,
verbose=True):
"""
Find ranges where to clip the videos for calibration, i.e. the first frame where the calibration
board appears and the last, as constrained by provided offset between the videos, the
specified cut-off at the end (in seconds), and the seek interval (in seconds).
@type video_filenames: list[str]
@param video_filenames: names of the video files
@type folder: str
@param folder: path of the folder where the video files reside
@type board_dims: tuple[int]
@param board_dims: dimensions of the calibration board
@type offset: list[int]
@param offset: time offset, in frames, from the first video
@type seek_time_interval: float
@param seek_time_interval: time interval in seconds
@type cut_off_end: float
@param cut_off_end: how much (minimum) to cut off the end of both videos (seconds)
@type verbose: bool
@param verbose: whether to print progress & results to standard output
@rtype: list[tuple[int]]
@return proper ranges to clip the videos for calibration.
"""
source_videos = []
for file_name in video_filenames:
camera = Camera(os.path.join(folder, file_name))
source_videos.append(camera)
ix_vid = 0
ranges = []
frame_offsets = []
frame_durations = []
frame_rates = []
for camera in source_videos:
frame_durations.append(camera.frame_count)
frame_offset = int(camera.fps * offset[camera.index])
frame_offsets.append(frame_offset)
frame_rates.append(camera.fps)
i_frame = frame_offset
camera.scroll_to_frame(i_frame)
camera.read_next_frame()
skip_interval = int(seek_time_interval * camera.fps)
# find the first frame with corners past the offset mark
cont_cap = True
if verbose:
print("Seeking first frame of {0:s} usable for calibration...".
format(camera.name))
while cont_cap:
found_corners = camera.try_approximate_corners(board_dims)
i_frame += skip_interval
camera.scroll_to_frame(i_frame)
if i_frame > (camera.frame_count - 1):
camera.more_frames_remain = False
break
camera.read_next_frame()
cont_cap = camera.more_frames_remain and not found_corners
if verbose:
print('.', end="", flush=True)
if not camera.more_frames_remain:
raise ValueError(
"Could not find a frame in the video containing the checkerboard!"
)
start_frame = i_frame
if verbose:
print("\nFound at frame {0:d} ({1:.3f} s).".format(
start_frame, start_frame / camera.fps))
# find the last frame with corners
i_frame = camera.frame_count - 1 - int(camera.fps * cut_off_end)
camera.scroll_to_frame(i_frame)
camera.read_next_frame()
found_corners = False
if verbose:
print("Seeking last usable frame of {0:s}...".format(camera.name))
while not found_corners:
found_corners = camera.try_approximate_corners(board_dims)
i_frame -= skip_interval
camera.scroll_to_frame(i_frame)
camera.read_next_frame()
if verbose:
print('.', end="", flush=True)
# add one for exclusive right bound
end_frame = i_frame + 1
if verbose:
print("\nFound at frame {0:d} ({1:.3f} s).".format(
end_frame, end_frame / camera.fps))
del camera # explicitly release video file
ranges.append((start_frame, end_frame))
ix_vid += 1
return __sync_ranges(frame_durations, frame_rates, ranges, frame_offsets)
