class BatchRCFeats(Feature): def __init__(self, actionfile, action_list=[]): self.src_ctx = SrcContext() self.src_ctx.read_h5(actionfile) self.tgt_cld = None self.tgt_ctx = TgtContext(self.src_ctx) self.name2ind = dict([(s, i) for i, s in enumerate(self.src_ctx.seg_names)]) self.costs = np.zeros(self.src_ctx.N) self.N = len(self.src_ctx.seg_names) self.indicators = np.eye(self.N) self.weights = np.r_[-1, np.zeros(self.N)] self.w0 = 0 def features(self, state, **kwargs): self.tgt_cld = state.cloud self.tgt_ctx.set_cld(self.tgt_cld) self.costs = batch_tps_rpm_bij(self.src_ctx, self.tgt_ctx) return np.c_[self.costs, self.indicators] def set_name2ind(self, action_list): self.name2ind = dict([(s, i) for i, s in enumerate(action_list)]) def get_ind(self, a): return self.name2ind[a] @staticmethod def get_size(num_actions): return num_actions + 1
class LandmarkFeats(MulFeats): def __init__(self, actionfile): MulFeats.__init__(self, actionfile) self.landmark_ctx = None def set_landmark_file(self, landmarkf): self.landmark_ctx = GPUContext() self.landmark_ctx.read_h5(landmarkf) self.landmark_targ_ctx = TgtContext(self.landmark_ctx) self.weights = np.zeros(self.src_ctx.N + self.landmark_ctx.N + MulFeats.N_costs) def features(self, state, **kwargs): mul_feats = MulFeats.features(self, state) self.landmark_targ_ctx.set_cld(state.cloud) landmark_feats = batch_tps_rpm_bij(self.landmark_ctx, self.landmark_targ_ctx) landmark_feats = np.exp(-landmark_feats) landmark_feats /= np.sum(landmark_feats) self.costs = np.c_[mul_feats, np.tile(landmark_feats, (self.src_ctx.N, 1))] return self.costs @staticmethod def get_size(num_actions, num_landmarks=70): return num_actions + num_landmarks + MulFeats.N_costs
def batch_cost(self, test_scene_state): if not(self.actionfile): raise ValueError('No actionfile provided for gpu context') tgt_ctx = TgtContext(self.src_ctx) cloud = test_scene_state.cloud cloud = self._clip_cloud(cloud) tgt_ctx.set_cld(cloud) cost_array = batch_tps_rpm_bij(self.src_ctx, tgt_ctx, T_init=self.rad_init, T_final=self.rad_final, outlierfrac=self.outlierfrac, outlierprior=self.outlierprior, outliercutoff=settings.OUTLIER_CUTOFF, em_iter=self.em_iter, component_cost=True) costs = dict(zip(self.src_ctx.seg_names, cost_array)) return costs