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