def estimate_from_camera_maps(self, from_cm, to_cm):
"""
Estimate the similarity transform between two corresponding camera maps
Cameras with corresponding frame IDs in the two maps are paired for
transform estimation. Cameras with no corresponding frame ID in the
other map are ignored. An exception is set if there are no shared frame
IDs between the two provided maps (nothing to pair).
:param from_cm: Map of original cameras, sharing N frames with the
transformed cameras, where N > 0.
:type from_cm: CameraMap
:param to_cm: Map of transformed cameras, sharing N frames with the
original cameras, where N > 0.
:type to_cm: CameraMap
:return: New estimated similarity transform mapping camera centers in
the ``from`` space to camera centers in the ``to`` space.
:rtype: Similarity
"""
cptr = self._call_cfunc(
'vital_algorithm_estimate_similarity_transform_estimate_camera_map',
[self.C_TYPE_PTR,
CameraMap.c_ptr_type(),
CameraMap.c_ptr_type()], [self, from_cm, to_cm],
Similarity.c_ptr_type(ctypes.c_double),
{1: VitalAlgorithmException})
return Similarity(ctype=ctypes.c_double, from_cptr=cptr)
def estimate_from_camera_maps(self, from_cm, to_cm):
"""
Estimate the similarity transform between two corresponding camera maps
Cameras with corresponding frame IDs in the two maps are paired for
transform estimation. Cameras with no corresponding frame ID in the
other map are ignored. An exception is set if there are no shared frame
IDs between the two provided maps (nothing to pair).
:param from_cm: Map of original cameras, sharing N frames with the
transformed cameras, where N > 0.
:type from_cm: CameraMap
:param to_cm: Map of transformed cameras, sharing N frames with the
original cameras, where N > 0.
:type to_cm: CameraMap
:return: New estimated similarity transform mapping camera centers in
the ``from`` space to camera centers in the ``to`` space.
:rtype: Similarity
"""
cptr = self._call_cfunc(
'vital_algorithm_estimate_similarity_transform_estimate_camera_map',
[self.C_TYPE_PTR, CameraMap.c_ptr_type(), CameraMap.c_ptr_type()],
[self, from_cm, to_cm],
Similarity.c_ptr_type(ctypes.c_double),
{
1: VitalAlgorithmException
}
)
return Similarity(ctype=ctypes.c_double, from_cptr=cptr)
def test_to_dict(self):
m = {
0: Camera(),
1: Camera(),
5: Camera()
}
cm = CameraMap(m)
m2 = cm.to_dict()
nose.tools.assert_equal(m, m2)
def triangulate(self, cameras, tracks, landmarks):
"""
Triangulate the landmark locations given sets of cameras and tracks
This function only triangulates the landmarks with indices in the
landmark map and which have support in the tracks and cameras
:param cameras: cameras viewing the landmarks
:type cameras: CameraMap
:param tracks: tracks to use as constraints
:type tracks: TrackSet
:param landmarks: landmarks to triangulate
:type landmarks: LandmarkMap
:return: New landmarks instance of triangulated landmarks
"""
# Copy pointer container for reference updating so we don't pollute the
# input instance.
lmap_ptr = LandmarkMap.c_ptr_type()(landmarks.c_pointer.contents)
self._call_cfunc(
'vital_algorithm_triangulate_landmarks_triangulate',
[self.C_TYPE_PTR, CameraMap.c_ptr_type(), TrackSet.c_ptr_type(),
ctypes.POINTER(LandmarkMap.c_ptr_type())],
[self, cameras, tracks, ctypes.byref(landmarks)]
)
r_lmap = landmarks
if ctypes.addressof(lmap_ptr.contents) != ctypes.addressof(landmarks.c_pointer.contents):
r_lmap = LandmarkMap(from_cptr=lmap_ptr)
return r_lmap
def estimate(self, cameras, landmarks):
"""
Estimate a canonical similarity transform for cameras and points
:param cameras: The camera map containing all the cameras
:type cameras: CameraMap
:param landmarks: The landmark map containing all the 3D landmarks
:type landmarks: LandmarkMap
:return: New estimated similarity transformation mapping the data to the
canonical space.
:rtype: Similarity
"""
cptr = self._call_cfunc(
'vital_algorithm_estimate_canonical_transform_estimate',
[self.C_TYPE_PTR, CameraMap.c_ptr_type(), LandmarkMap.c_ptr_type()],
[self, cameras, landmarks],
Similarity.c_ptr_type(ctypes.c_double),
{
1: VitalAlgorithmException
}
)
return Similarity(ctype=ctypes.c_double, from_cptr=cptr)
def initialize(self, cmap, lmap, tset):
"""
Initialize the camera and landmark parameters given a set of tracks
:param cmap: Cameras to initialize
:type cmap: CameraMap
:param lmap: Landmarks to initialize
:type lmap: LandmarkMap
:param tset: Tracks to use as constraints
:type tset: TrackSet
:return: New, initialized camera and landmark maps.
:rtype: (CameraMap, LandmarkMap)
"""
# make a separate copy of pointer container in prep for passing by ref
cmap_ptr = CameraMap.c_ptr_type()(cmap.c_pointer.contents)
lmap_ptr = LandmarkMap.c_ptr_type()(lmap.c_pointer.contents)
self._call_cfunc(
"vital_algorithm_initialize_cameras_landmarks_initialize", [
self.C_TYPE_PTR,
ctypes.POINTER(CameraMap.c_ptr_type()),
ctypes.POINTER(LandmarkMap.c_ptr_type()),
TrackSet.c_ptr_type()
],
[self, ctypes.byref(cmap_ptr),
ctypes.byref(lmap_ptr), tset])
# Initialize new objects if "returned" pointers are different from input
# objects
r_cmap = cmap
if ctypes.addressof(cmap_ptr.contents) != ctypes.addressof(
cmap.c_pointer.contents):
self._log.debug("Creating new CameraMap instance")
r_cmap = CameraMap(from_cptr=cmap_ptr)
r_lmap = lmap
if ctypes.addressof(lmap_ptr.contents) != ctypes.addressof(
lmap.c_pointer.contents):
self._log.debug("Creating new LandmarkMap instance")
r_lmap = LandmarkMap(from_cptr=lmap_ptr)
return r_cmap, r_lmap
def optimize(self, cmap, lmap, tset):
"""
Optimize the camera and landmark parameters given a set of tracks
:param cmap: Cameras to optimize
:type cmap: CameraMap
:param lmap: Landmarks to optimize
:type lmap: LandmarkMap
:param tset: Tracks to use as constraints
:type tset: TrackSet
:return: New, optimized Camera and Landmark maps.
:rtype: (CameraMap, LandmarkMap)
"""
# make a separate copy of pointer container in prep for passing by ref
cmap_ptr = CameraMap.c_ptr_type()(cmap.c_pointer.contents)
lmap_ptr = LandmarkMap.c_ptr_type()(lmap.c_pointer.contents)
self._call_cfunc(
'vital_algorithm_bundle_adjust_optimize',
[self.C_TYPE_PTR,
ctypes.POINTER(CameraMap.c_ptr_type()),
ctypes.POINTER(LandmarkMap.c_ptr_type()),
TrackSet.c_ptr_type()],
[self, ctypes.byref(cmap_ptr), ctypes.byref(lmap_ptr), tset]
)
# Initialize new objects if "returned" pointers are different from input
# objects
r_cmap = cmap
if ctypes.addressof(cmap_ptr.contents) != ctypes.addressof(cmap.c_pointer.contents):
self._log.debug("Creating new CameraMap instance")
r_cmap = CameraMap(from_cptr=cmap_ptr)
r_lmap = lmap
if ctypes.addressof(lmap_ptr.contents) != ctypes.addressof(lmap.c_pointer.contents):
self._log.debug("Creating new LandmarkMap instance")
r_lmap = LandmarkMap(from_cptr=lmap_ptr)
return r_cmap, r_lmap
def noisy_cameras(cam_map, pos_stddev=1., rot_stddev=1.):
"""
Add positional and rotational gaussian noise to cameras
:type cam_map: CameraMap
:type pos_stddev: float
:type rot_stddev: float
:return: Camera map of new, noidy cameras'
"""
cmap = {}
for f, c in cam_map.as_dict().iteritems():
c2 = Camera(
c.center + random_point_3d(pos_stddev),
c.rotation * Rotation.from_rodrigues(random_point_3d(rot_stddev)),
c.intrinsics)
cmap[f] = c2
return CameraMap(cmap)
def init_cameras(num_cams=20, intrinsics=None):
"""
Initialize camera sequence with all cameras at the same location (0, 0, 1)
and looking at origin.
:param num_cams: Number of cameras to create, default 20.
:param intrinsics: Intrinsics to use for all cameras.
:return: Camera map of initialize cameras
"""
if intrinsics is None:
intrinsics = CameraIntrinsics(1000, (640, 480))
r = Rotation()
c = EigenArray.from_iterable((0, 0, 1))
d = {}
for i in range(num_cams):
cam = Camera(c, r, intrinsics).clone_look_at([0, 0, 0], [0, 1, 0])
d[i] = cam
return CameraMap(d)
def camera_seq(num_cams=20, k=None):
"""
Create a camera sequence (elliptical path)
:param num_cams: Number of cameras. Default is 20
:param k: Camera intrinsics to use for all created cameras. Default has
focal length = 1000 and principle point of (640, 480).
:return:
"""
if k is None:
k = CameraIntrinsics(1000, [640, 480])
d = {}
r = Rotation() # identity
for i in xrange(num_cams):
frac = float(i) / num_cams
x = 4 * math.cos(2 * frac)
y = 3 * math.sin(2 * frac)
d[i] = Camera([x, y, 2 + frac], r, k).clone_look_at([0, 0, 0])
return CameraMap(d)
def triangulate(self, cameras, tracks, landmarks):
"""
Triangulate the landmark locations given sets of cameras and tracks
This function only triangulates the landmarks with indices in the
landmark map and which have support in the tracks and cameras
:param cameras: cameras viewing the landmarks
:type cameras: CameraMap
:param tracks: tracks to use as constraints
:type tracks: TrackSet
:param landmarks: landmarks to triangulate
:type landmarks: LandmarkMap
:return: New landmarks instance of triangulated landmarks
"""
# Copy pointer container for reference updating so we don't pollute the
# input instance.
lmap_ptr = LandmarkMap.c_ptr_type()(landmarks.c_pointer.contents)
self._call_cfunc('vital_algorithm_triangulate_landmarks_triangulate', [
self.C_TYPE_PTR,
CameraMap.c_ptr_type(),
TrackSet.c_ptr_type(),
ctypes.POINTER(LandmarkMap.c_ptr_type())
], [self, cameras, tracks,
ctypes.byref(landmarks)])
r_lmap = landmarks
if ctypes.addressof(lmap_ptr.contents) != ctypes.addressof(
landmarks.c_pointer.contents):
r_lmap = LandmarkMap(from_cptr=lmap_ptr)
return r_lmap
def estimate(self, cameras, landmarks):
"""
Estimate a canonical similarity transform for cameras and points
:param cameras: The camera map containing all the cameras
:type cameras: CameraMap
:param landmarks: The landmark map containing all the 3D landmarks
:type landmarks: LandmarkMap
:return: New estimated similarity transformation mapping the data to the
canonical space.
:rtype: Similarity
"""
cptr = self._call_cfunc(
'vital_algorithm_estimate_canonical_transform_estimate', [
self.C_TYPE_PTR,
CameraMap.c_ptr_type(),
LandmarkMap.c_ptr_type()
], [self, cameras, landmarks],
Similarity.c_ptr_type(ctypes.c_double),
{1: VitalAlgorithmException})
return Similarity(ctype=ctypes.c_double, from_cptr=cptr)
def test_to_dict(self):
m = {0: Camera(), 1: Camera(), 5: Camera()}
cm = CameraMap(m)
m2 = cm.to_dict()
nose.tools.assert_equal(m, m2)
def test_size(self):
m = {0: Camera(), 1: Camera(), 5: Camera()}
cm = CameraMap(m)
nose.tools.assert_equal(cm.size, 3)