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